with open(record_file, 'a') as csvfile: # append to file
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)
print speed2[1]
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()
print speed2[1]
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()
