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()