def motorboth():
""" set of tests to run on two DC motors connected to A and B channel """
print(" TEST: testing motor ")
# Motorssetup
MotorOne = rpi_dc_lib.TB6612FNGDc(AI1, AI2, PWA, Freq, True,
"motor_one both")
MotorTwo = rpi_dc_lib.TB6612FNGDc(BI1, BI2, PWB, Freq, True,
"motor_two both")
# ================ Both Motors running =============
try:
print("Both motors forward")
MotorOne.forward(25)
MotorTwo.forward(25)
input("press key to stop")
print("motor stop\n")
MotorOne.stop(0)
MotorTwo.stop(0)
time.sleep(3)
except KeyboardInterrupt:
print("CTRL-C: Terminating program.")
except Exception as error:
print(error)
print("Unexpected error:")
finally:
MotorOne.cleanup(False)
MotorTwo.cleanup(False)
def motortwo():
""" set of tests to run on one DC motor connected to B channel """
print(" TEST: testing motor ")
# Motorssetup
MotorTwo = rpi_dc_lib.TB6612FNGDc(BI1 ,BI2 ,PWB ,Freq ,True, "motor_two")
# ================ Motors two test section 1=============
try:
print("1. motor forward")
MotorTwo.forward(15)
input("press key to stop")
MotorTwo.stop(0)
print("motor stop\n")
time.sleep(3)
print("2. motor forward speed up")
for i in range(15,30):
MotorTwo.forward(i)
time.sleep(1)
MotorTwo.stop(0)
print("motor stop\n")
time.sleep(3)
print("3. motor backward")
MotorTwo.backward(15)
input("press key to stop")
MotorTwo.stop(0)
print("motor stop\n")
time.sleep(3)
print("4. motor backward speed up")
for i in range(15,30):
MotorTwo.backward(i)
time.sleep(1)
MotorTwo.stop(0)
print("motor stop\n")
time.sleep(3)
print("5 .brake check")
MotorTwo.forward(50)
time.sleep(3)
MotorTwo.brake(0)
print("motor brake\n")
except KeyboardInterrupt:
print("CTRL-C: Terminating program.")
except Exception as error:
print(error)
print("Unexpected error:")
finally:
MotorTwo.cleanup(False)
def right_motor_thread():
global r
global exit_flag
global modes
MotorTwo = rpi_dc_lib.TB6612FNGDc(BI1, BI2, PWB, Freq, True,
"motor_two") #Motorsetup
MotorFour = rpi_dc_lib.TB6612FNGDc(BI1_2, BI2_2, PWB_2, Freq_2, True,
"motor_four") #Motorsetup
while True:
if (exit_flag == 0):
if (modes == 0):
if (r == 1):
MotorTwo.stop(0)
print("turn right(right)")
time.sleep(turnsleep)
print("turn finished")
r = 0
elif (right_motor["mode"] == 1): #forward
if (right_motor["speed"] == 1):
MotorTwo.forward(20)
MotorFour.forward(20)
print("forward duty cycle of 20")
elif (right_motor["speed"] == 2):
MotorTwo.forward(40)
MotorFour.forward(40)
print("forward duty cycle of 40")
elif (right_motor["speed"] == 3):
MotorTwo.forward(60)
MotorFour.forward(60)
print("forward duty cycle of 60")
elif (right_motor["speed"] == 4):
MotorTwo.forward(80)
MotorFour.forward(80)
print("forward duty cycle of 80")
elif (right_motor["speed"] == 5):
MotorTwo.forward(100)
MotorFour.forward(100)
print("forward duty cycle of 100")
elif (right_motor["mode"] == 2): #backward
if (right_motor["speed"] == 1):
MotorTwo.backward(20)
MotorFour.backward(20)
print("backward duty cycle of 20")
elif (right_motor["speed"] == 2):
MotorTwo.backward(40)
MotorFour.backward(40)
print("backward duty cycle of 40")
elif (right_motor["speed"] == 3):
MotorTwo.backward(60)
MotorFour.backward(60)
print("backward duty cycle of 60")
elif (right_motor["speed"] == 4):
MotorTwo.backward(80)
MotorFour.backward(80)
print("backward duty cycle of 80")
elif (right_motor["speed"] == 5):
MotorTwo.backward(100)
MotorFour.backward(100)
print("backward duty cycle of 100")
elif (right_motor["mode"] == 0):
MotorTwo.stop(0)
MotorFour.stop(0)
else:
if (cv_flag == 0):
print("in mode 2(right)")
#MotorFour.stop(0)
a = GPIO.input(17)
b = GPIO.input(16)
if (b == 0):
if (a == 0):
MotorTwo.forward(straight_line_speed)
MotorFour.forward(straight_line_speed)
else:
MotorTwo.forward(turning_speed)
MotorFour.forward(turning_speed)
elif (b == 1):
MotorTwo.stop(0)
MotorFour.stop(0)
else:
MotorTwo.stop(0)
MotorFour.stop(0)
time.sleep(3)
else:
quit()
def left_motor_thread():
global l
global exit_flag
global modes
rpi_dc_lib.TB6612FNGDc.standby(Standby, True)
rpi_dc_lib.TB6612FNGDc.standby(Standby_2, True)
MotorOne = rpi_dc_lib.TB6612FNGDc(AI1, AI2, PWA, Freq, True,
"motor_one") # Motorssetup
MotorThree = rpi_dc_lib.TB6612FNGDc(AI1_2, AI2_2, PWA_2, Freq_2, True,
"motor_three")
while True:
if (exit_flag == 0):
if (modes == 0):
if (l == 1):
MotorOne.stop(0)
print("turn left(left)")
time.sleep(turnsleep)
print("turn finished")
l = 0
elif (left_motor["mode"] == 1): #forward
if (left_motor["speed"] == 1):
MotorOne.forward(20)
MotorThree.forward(20)
print("forward duty cycle of 20")
elif (left_motor["speed"] == 2):
MotorOne.forward(40)
MotorThree.forward(40)
print("forward duty cycle of 40")
elif (left_motor["speed"] == 3):
MotorOne.forward(60)
MotorThree.forward(60)
print("forward duty cycle of 60")
elif (left_motor["speed"] == 4):
MotorOne.forward(80)
MotorThree.forward(80)
print("forward duty cycle of 80")
elif (left_motor["speed"] == 5):
MotorOne.forward(100)
print("forward duty cycle of 100")
elif (left_motor["mode"] == 2): #backward
if (left_motor["speed"] == 1):
MotorOne.backward(20)
MotorThree.backward(20)
print("backward duty cycle of 20")
elif (left_motor["speed"] == 2):
MotorOne.backward(40)
MotorThree.backward(40)
print("backward duty cycle of 40")
elif (left_motor["speed"] == 3):
MotorOne.backward(60)
MotorThree.backward(60)
print("backward duty cycle of 60")
elif (left_motor["speed"] == 4):
MotorOne.backward(80)
MotorThree.backward(80)
print("backward duty cycle of 80")
elif (left_motor["speed"] == 5):
MotorOne.backward(100)
MotorThree.backward(100)
print("backward duty cycle of 100")
elif (left_motor["mode"] == 0):
MotorOne.stop(0)
MotorThree.stop(0)
else:
print("in mode 2(right)")
if (cv_flag == 0):
#MotorFour.stop(0)
a = GPIO.input(17)
b = GPIO.input(16)
if (a == 0):
if (b == 0):
MotorOne.forward(straight_line_speed)
MotorThree.forward(straight_line_speed)
else:
MotorOne.forward(turning_speed)
MotorThree.forward(turning_speed)
elif (a == 1):
MotorOne.stop(0)
MotorThree.stop(0)
#MotorFour.stop(0)
else:
MotorOne.stop(0)
MotorThree.stop(0)
time.sleep(3)
else:
quit()
ws_client = None # ====== DC motor driven by TB6612FNG ==== # TB66 -- GPIO RPI PWA = 17 AI1 = 22 AI2 = 27 PWB = 18 BI1 = 23 BI2 = 24 Standby = 25 Freq = 50 MotorOne = rpi_dc_lib.TB6612FNGDc(AI1, AI2, PWA, Freq, True, "motor_one") MotorTwo = rpi_dc_lib.TB6612FNGDc(BI1, BI2, PWB, Freq, True, "motor_two") # ======================================== # Light sensor --------------------------- sensor = TSL2591.TSL2591() # sensor.SET_InterruptThreshold(0xff00, 0x0010) # ======================================== reset_speed = 25 speed_m1 = None speed_m2 = None def reset_motors_speed():