else:
print "failed "
rc.Open()
address = 0x80
version = rc.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
while (1):
rc.SpeedDistanceM1(address, 12000, 48000, 1)
rc.SpeedDistanceM2(address, -12000, 48000, 1)
buffers = (0, 0, 0)
while (buffers[1] != 0x80
and buffers[2] != 0x80): #Loop until distance command has completed
displayspeed()
buffers = rc.ReadBuffers(address)
time.sleep(2)
rc.SpeedDistanceM1(address, -12000, 48000, 1)
rc.SpeedDistanceM2(address, 12000, 48000, 1)
buffers = (0, 0, 0)
while (buffers[1] != 0x80
and buffers[2] != 0x80): #Loop until distance command has completed
displayspeed()
buffers = rc.ReadBuffers(address)
#motor_direction = ""
elif (motor_direction == "unsure"):
print("rotating")
roboclaw.BackwardM1(address,40)
roboclaw.ForwardM2(address,40)
elif motor_direction == "neither":
print("Neutral")
print (distance)
roboclaw.SpeedDistanceM1M2(address,0,0,0,0,1)
if (distance >35):
roboclaw.SpeedDistanceM1M2(0x81,-150,10,-150,10,1)#replace with something more bulletfproof
if (distance > 20 and distance <35):
print ('pickup object')
#roboclaw.BackwardM1M2(0x81,64)
#distanceMove = float(distance/35) * 150 #integrate a better value for how far
roboclaw.SpeedDistanceM2(0x80,-1000,500,1)
roboclaw.SpeedDistanceM1M2(0x81,-150,300,-150,300,1)
sleep(3)
roboclaw.SpeedM1M2(0x80,0,0)
roboclaw.SpeedDistanceM2(0x80,1000,500,1)
#rest of the code
sleep(3)
roboclaw.SpeedM1M2(0x80,0,0)
roboclaw.SpeedM1M2(0x81,0,0)
currentDetection = "face"
client.publish("voice", "robot")
sleep(1)
client.publish("voice", "face")
p.ChangeDutyCycle(6)
time.sleep(0.5)
p.ChangeDutyCycle(0)
recordwriter = csv.writer(csvfile,
delimiter=',',
quotechar='|',
quoting=csv.QUOTE_MINIMAL)
recordwriter.writerow([round(x, 2), round(y, 2)])
waypoint_num = waypoint_num + 1
if (testmode): # in test mode just do a box
while True:
rc.ResetEncoders(address)
buffers = (0, 0, 0)
displayspeed()
time.sleep(2)
if (waypoints == 0): # straight
rc.SpeedDistanceM1(address, 2000, 500 * tickdistanceL, 1)
rc.SpeedDistanceM2(address, 2000, 500 * tickdistanceR, 1)
if (waypoints == 1): # turn
rc.SpeedDistanceM1(address, -2000, 200 * tickdistanceL, 1)
rc.SpeedDistanceM2(address, 2000, 200 * tickdistanceR, 1)
buffers = (0, 0, 0)
while (buffers[1] != 0x80 and buffers[2] != 0x80
): #Loop until distance command has completed
displayspeed()
buffers = rc.ReadBuffers(address)
print("Next waypoint")
if (waypoints < 1):
waypoints = waypoints + 1
else:
waypoints = 0
if ((not testmode)
and (not recordmode)): # This is regular waypoint mode
else:
print "failed "
rc.Open()
address = 0x80
version = rc.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
while (1):
rc.SpeedDistanceM1(address, SPEED, DIST, 1)
rc.SpeedDistanceM2(address, -SPEED, DIST, 1)
buffers = (0, 0, 0)
while (buffers[1] != 0x80
and buffers[2] != 0x80): #Loop until distance command has completed
displayspeed()
buffers = rc.ReadBuffers(address)
time.sleep(2)
rc.SpeedDistanceM1(address, -SPEED, DIST, 1)
rc.SpeedDistanceM2(address, SPEED, DIST, 1)
buffers = (0, 0, 0)
while (buffers[1] != 0x80
and buffers[2] != 0x80): #Loop until distance command has completed
displayspeed()
buffers = rc.ReadBuffers(address)
