def sharp_turn_right(self):
drive.SetSpeed(50, -50)
def sharp_turn_left(self):
drive.SetSpeed(-50, 50)
def halt_here(self):
drive.SetSpeed(0, 0)
def slight_turn_right(self):
drive.SetSpeed(50, 0)
import RPi.GPIO as GPIO import time import drive GPIO.setup(11, GPIO.IN) #SO1 GPIO.setup(13, GPIO.IN) #SO2 GPIO.setup(15, GPIO.IN) #S03 GPIO.setup(19, GPIO.IN) #SO4 GPIO.setup(21, GPIO.IN) #SO5 drive.SetSpeed(50, 0) time.sleep(0.8) drive.SetSpeed(50, 50) time.sleep(0.05) drive.SetSpeed(0, 50) time.sleep(1) drive.SetSpeed(50, 50) time.sleep(0.25)
def forward(self):
drive.SetSpeed(50, 50)
def sharp_turn_left():
drive.SetSpeed(-speed, speed)
import drive
import encoders
speed = 40
encoder=encoders.wheels(26,24)
drive.SetSpeed(speed, speed);
while(encoder.get()[0]<30):
pass
drive.stop()
print(encoder.get())
def slight_turn_right():
drive.SetSpeed(speed, -speed / 4)
def sharp_turn_right():
drive.SetSpeed(speed, -speed)
def forward():
drive.SetSpeed(speed, speed)
def halt_here():
drive.SetSpeed(0, 0)
def forward():
drive.SetSpeed(50, 50)
def slight_turn_left(self):
drive.SetSpeed(0, 50)
def slight_turn_left():
drive.SetSpeed(-speed / 4, speed)
# \ # X \ # o-----o # A B # start the robot at A, facing towards B. D is the goal, X are obstacles. # # The commands have been determined by hand, i.e., by trial and error. # Alternatively, we could have chosen to drive in arcs. speed = 50 encoder=encoders.wheels(26,24) # drive from A to B drive.SetSpeed(speed, speed); print(encoder.get()) drive.sleep(1) # turn towards C drive.SetSpeed(-speed,speed); drive.sleep(0.7) #drive from B to C drive.SetSpeed(speed, speed); drive.sleep(1.5) # turn towards D drive.SetSpeed(speed,-speed); drive.sleep(0.7)
angle_difference = -10
elif (s1 == 0) and (s2 == 0) and (s3 == 1) and (s4 == 1) and (s5 == 0):
angle_difference = 10
elif (s1 == 0) and (s2 == 0) and (s3 == 0) and (s4 == 1) and (s5 == 1):
angle_difference = 30
elif (s1 == 0) and (s2 == 0) and (s3 == 0) and (s4 == 1) and (s5 == 0):
angle_difference = 30
elif (s1 == 0) and (s2 == 0) and (s3 == 0) and (s4 == 0) and (s5 == 1):
angle_difference = 30
elif (s1 == 0) and (s2 == 0) and (s3 == 1) and (s4 == 1) and (s5 == 1):
angle_difference = 30
elif (s1 == 1) and (s2 == 0) and (s3 == 0) and (s4 == 0) and (s5 == 0):
angle_difference = -30
elif (s1 == 0) and (s2 == 1) and (s3 == 0) and (s4 == 0) and (s5 == 0):
angle_difference = -30
elif (s1 == 1) and (s2 == 1) and (s3 == 0) and (s4 == 0) and (s5 == 0):
angle_difference = -30
elif (s1 == 1) and (s2 == 1) and (s3 == 1) and (s4 == 0) and (s5 == 0):
angle_difference = -30
pi = math.pi
d_phi = lamda * (-math.sin(pi * (angle_difference) / 180))
v_mm_per_second = d_phi / math.pi * 53
v_pulses_per_second = v_mm_per_second / 0.13
# print(-v_pulses_per_second + speed, v_pulses_per_second + speed)
drive.SetSpeed(-v_pulses_per_second + speed,
v_pulses_per_second + speed)
except:
drive.stop()
