def test_MoveDifferential(self):
tank_drive = MoveDifferential(OUTPUT_A, OUTPUT_D, EV3EducationSetTire,
15 * STUD_MM)
tank_drive.turn_right(20, 90 - 34)
self.assertEqual(tank_drive.is_running, False)
tank_drive.odometry_start()
tank_drive.on_to_coordinates(30, -50, 300)
self.assertEqual(tank_drive.is_running, False)
tank_drive.on_for_distance(30, 300)
self.assertEqual(tank_drive.is_running, False)
tank_drive.on_arc_right(30, 200, 600, block=True)
self.assertEqual(tank_drive.is_running, False)
cv2.imshow("Image", frame)
cv2.waitKey(2000)
filename = str(i) + 'cali.jpg'
cv2.imwrite(filename, frame)
# mdiff.on_to_coordinates(SpeedRPM(15), 0, (i*100))
mdiff.on_for_distance(15, 100)
# mdiff.turn_to_angle(SpeedRPM(5), 90)
print(
"wait a few seconds after movement before taking another photo...")
time.sleep(3)
# Now go back to where we started and rotate in place to 90 degrees (facing orig forward direction)
print("driving back to origin and rotating back to orig pose...")
mdiff.on_to_coordinates(SpeedRPM(20), 0, 0)
mdiff.turn_to_angle(SpeedRPM(5), 90)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
mdiff.off()
print("")
print("!!!!!!!!!!!!!!!!!!!!!!")
print("")
print("Stopping motors A and B")
print("")
print("!!!!!!!!!!!!!!!!!!!!!!")
print("")
mdiff.odometry_coordinates_log()
mdiff.odometry_stop()
# mdiff.on_to_coordinates(SpeedRPM(25), 0, 0)
# mdiff.turn_to_angle(SpeedRPM(25), 90)
# Drive in a rectangle 10 times
# for i in range(11):
# # mdiff.turn_to_angle(SpeedRPM(25), 120)
# mdiff.on_to_coordinates(SpeedRPM(25), 0, DistanceFeet(1).mm)
# mdiff.on_to_coordinates(SpeedRPM(25), DistanceFeet(2).mm, DistanceFeet(1).mm)
# mdiff.on_to_coordinates(SpeedRPM(25), DistanceFeet(2).mm, 0)
# mdiff.on_to_coordinates(SpeedRPM(25), 0, 0)
# mdiff.turn_to_angle(SpeedRPM(25), 90)
# Drive between two points 10 times
for i in range(11):
# mdiff.turn_to_angle(SpeedRPM(25), 120)
mdiff.on_to_coordinates(SpeedRPM(25), 0, 2500)
mdiff.on_to_coordinates(SpeedRPM(25), 0, 0)
mdiff.turn_to_angle(SpeedRPM(25), 90)
# Use odometry to drive to specific coordinates
# mdiff.on_to_coordinates(SpeedRPM(25), 600, 300)
# mdiff.turn_to_angle(SpeedRPM(25), 90)
# Now go back to where we started and rotate in place to 90 degrees
# mdiff.on_to_coordinates(SpeedRPM(25), 0, 0)
# mdiff.turn_to_angle(SpeedRPM(25), 90)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
mdiff.off()
print("")
print("!!!!!!!!!!!!!!!!!!!!!!")
STUD_MM = 8 # test with a robot that: # - uses the standard wheels known as EV3Tire # - wheels are 16 studs apart mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM) # Rotate 90 degrees clockwise mdiff.turn_right(SpeedRPM(40), 90) # Drive forward 500 mm mdiff.on_for_distance(SpeedRPM(40), 500) # Drive in arc to the right along an imaginary circle of radius 150 mm. # Drive for 700 mm around this imaginary circle. mdiff.on_arc_right(SpeedRPM(80), 150, 700) # Enable odometry mdiff.odometry_start() # Use odometry to drive to specific coordinates mdiff.on_to_coordinates(SpeedRPM(40), 300, 300) # Use odometry to go back to where we started mdiff.on_to_coordinates(SpeedRPM(40), 0, 0) # Use odometry to rotate in place to 90 degrees mdiff.turn_to_angle(SpeedRPM(40), 90) # Disable odometry mdiff.odometry_stop()
# mdiff.on_to_coordinates(SpeedRPM(25), 0, DistanceFeet(1).mm)
# mdiff.on_to_coordinates(SpeedRPM(25), DistanceFeet(2).mm, DistanceFeet(1).mm)
# mdiff.on_to_coordinates(SpeedRPM(25), DistanceFeet(2).mm, 0)
# mdiff.on_to_coordinates(SpeedRPM(25), 0, 0)
# mdiff.turn_to_angle(SpeedRPM(25), 90)
# # Drive between two points 10 times
# for i in range(11):
# # mdiff.turn_to_angle(SpeedRPM(25), 120)
# mdiff.on_to_coordinates(SpeedRPM(25), 0, 2500)
# mdiff.on_to_coordinates(SpeedRPM(25), 0, 0)
# mdiff.turn_to_angle(SpeedRPM(25), 90)
# Use odometry to drive to specific coordinates
# mdiff.on_to_coordinates(SpeedRPM(25), 600, 300)
mdiff.on_to_coordinates(SpeedRPM(15), 0, 150)
# mdiff.turn_to_angle(SpeedRPM(25), 90)
# Now go back to where we started and rotate in place to 90 degrees
# mdiff.on_to_coordinates(SpeedRPM(25), 0, 0)
# mdiff.turn_to_angle(SpeedRPM(25), 90)
except KeyboardInterrupt: # except the program gets interrupted by Ctrl+C on the keyboard.
mdiff.off()
print("")
print("!!!!!!!!!!!!!!!!!!!!!!")
print("")
print("Stopping motors A and B")
print("")
print("!!!!!!!!!!!!!!!!!!!!!!")
print("")