def _do_action(self):
self.robot.sensormap.tank_drive.stop()
self.align()
# Now we know we are aligned
self.robot.sensormap.measurement_motor.on_for_rotations(
SpeedRPM(-5), .3)
if self.get_lake_color():
self.detected.add(self.get_lake_color())
self.robot.sensormap.measurement_motor.on_for_rotations(
SpeedRPM(5), .3)
self.robot.rotate_degrees(.25, lock=self.lock)
self.goal_passed = self.colors == self.detected
def reverse_for_rotations(self, nr_rotations, rpm=60, lock=None):
"""
Reverses the Robot (makes it move backwards).
:param nr_rotations: Number of degrees Robot turns.
:param rpm: Speed at which the Robot reverses in rotations per minute.
:param lock: Optional Lock to stop the operation when requested
"""
self.sensormap.tank_drive.on_for_rotations(SpeedRPM(-rpm), SpeedRPM(-rpm), nr_rotations, block=False)
end_time = datetime.now() + timedelta(seconds=(nr_rotations*60)/rpm)
while datetime.now() < end_time:
if lock and lock.is_locked():
self.sensormap.tank_drive.stop()
break
time.sleep(0.01)
def gyro(an):
baseAngle = gy.value()
mdiff.turn_left(SpeedRPM(40), an)
time.sleep(0.5)
angle = abs(gy.value() - baseAngle)
diffAng = angle - an
if (diffAng < 0):
mdiff.turn_left(SpeedRPM(40), abs(diffAng))
else:
mdiff.turn_right(SpeedRPM(40), abs(diffAng))
time.sleep(2)
def turn_for_rotations(self, rotations, rpm=30, lock=None):
"""
Turn for a number of degrees with the given speed.
Can be pre-empted when given a Lock.
:param rotations: The number of rotations to turn.
:param rpm: The speed to turn at.
:param lock: Optional Lock to stop the operation when requested.
"""
self.sensormap.tank_drive.on_for_rotations(SpeedRPM(rpm), SpeedRPM(-rpm), abs(rotations), block=False)
end_time = datetime.now() + timedelta(seconds=(abs(rotations)*60)/abs(rpm))
while datetime.now() < end_time:
if lock and lock.is_locked():
self.sensormap.tank_drive.stop()
break
time.sleep(0.01)
def _move_forward_to_hopefully_correct_position(recursion_count: int):
_front_distance = _base_front_travel_distance_after_backing_off_and_correctin_angle_cm - recursion_count
self._motor_pair.on_for_rotations(
steering=Steering.STRAIGHT.value,
speed=SpeedRPM(self._correction_speed_rpm * self._move_forward_speed_factor),
rotations=_front_distance * 10 / self._wheel_circumference_mm
)
def _back_off_from_wall():
self._motor_pair.on_for_rotations(
steering=Steering.STRAIGHT.value,
speed=SpeedRPM(self._correction_speed_rpm * self._move_forward_speed_factor),
rotations=-((_back_off_from_wall_distance_cm + recursion_count) * 10 / self._wheel_circumference_mm),
brake=True, block=True
)
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'),
('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(
SpeedPercent(35).to_native_units(m), 35 / 100 * m.max_speed)
self.assertEqual(SpeedDPS(300).to_native_units(m), 300)
self.assertEqual(SpeedNativeUnits(300).to_native_units(m), 300)
self.assertEqual(SpeedDPM(30000).to_native_units(m), (30000 / 60))
self.assertEqual(SpeedRPS(2).to_native_units(m), 360 * 2)
self.assertEqual(SpeedRPM(100).to_native_units(m), (360 * 100 / 60))
self.assertEqual(DistanceMillimeters(42).mm, 42)
self.assertEqual(DistanceCentimeters(42).mm, 420)
self.assertEqual(DistanceDecimeters(42).mm, 4200)
self.assertEqual(DistanceMeters(42).mm, 42000)
self.assertAlmostEqual(DistanceInches(42).mm, 1066.8)
self.assertAlmostEqual(DistanceFeet(42).mm, 12801.6)
self.assertAlmostEqual(DistanceYards(42).mm, 38404.8)
self.assertEqual(DistanceStuds(42).mm, 336)
def _move_forward_mm(self, distance_mm: float, speed_rpm: int):
_speed = SpeedRPM(speed_rpm * self._motor_pair_polarity_factor)
_rotations = distance_mm / self._wheel_circumference_mm
self._motor_pair.on_for_rotations(steering=Steering.STRAIGHT.value,
speed=_speed,
rotations=_rotations,
brake=True,
block=True)
def sample3():
mdiff.on_for_distance(speed = 50,distance_mm = 350)
sleep(2)
#putting arm down
medium_motorA.on_for_degrees(speed = 10, degrees = -110)
mdiff.on_arc_right(SpeedRPM(50), 70, 90)
#putting arm back up
medium_motorA.on_for_degrees(speed = 10, degrees = 110)
def move():
'''
tank_drive = MoveTank(OUTPUT_A, OUTPUT_D)
tank_drive.on_for_rotations(50, 75, 10)
# drive in a turn for 10 rotations of the outer motor
'''
mdiff.odometry_start()
# Enable odometry
while True:
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 500, 0)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 500, 500)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 0, 500)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 0, 0)
'''
mdiff.odometry_start()
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 250, 0)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 500, 0)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 500, 750)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 250, 750)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 750, 750)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 750, 250)
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 0, 0)
'''
'''
# Rotate 90 degrees clockwise
mdiff.turn_right(SpeedRPM(SPEEDRPM), 90)
# Drive forward 500 mm
mdiff.on_for_distance(SpeedRPM(SPEEDRPM), 500)
mdiff.turn_left(SpeedRPM(SPEEDRPM), 90)
mdiff.on_for_distance(SpeedRPM(SPEEDRPM), 500)
mdiff.turn_left(SpeedRPM(SPEEDRPM), 90)
mdiff.on_for_distance(SpeedRPM(SPEEDRPM), 1000)
# Use odometry to go back to where we started
mdiff.on_to_coordinates(SpeedRPM(SPEEDRPM), 0, 0)
'''
# Disable odometry
mdiff.odometry_stop()
def _correct_front_distance(self, front_distance_cm: float):
_distance_to_compensate_mm = (front_distance_cm - self._ideal_distance_cm) * 10
self._logger.debug('Need to correct front distance for {} mm'.format(_distance_to_compensate_mm))
self._motor_pair.on_for_rotations(
steering=Steering.STRAIGHT.value,
speed=SpeedRPM(self._correction_speed_rpm * self._move_forward_speed_factor),
rotations=(_distance_to_compensate_mm / self._wheel_circumference_mm),
brake=True, block=True
)
def _correct_angle():
_angle_diff = angle_after - angle_before
_steering = Steering.LEFT_ON_SPOT if _angle_diff > 0 else Steering.RIGHT_ON_SPOT
_rotations = (self._wheelbase_width_at_centers_mm * abs(_angle_diff)) / (self._wheel_diameter_mm * 360)
self._motor_pair.on_for_rotations(
steering=_steering.value,
speed=SpeedRPM(self._correction_speed_rpm),
rotations=_rotations
)
def sample2():
medium_motorA.on_for_degrees(speed = 10, degrees = -110)
sleep(5)
#go to sample 2 position
mdiff.on_arc_left(SpeedRPM(50), 1200, 400)
sleep(5)
#moving the arm up with second sample and next commands are going to cache site
medium_motorA.on_for_degrees(speed = 10, degrees = 110)
def _correct_side_distance(self, distances_after):
_side_distance_correction_fix_degrees = 10
self._logger.debug('I am in between two side walls. Checking if i am too close to one..')
if distances_after['left'] < self._ideal_distance_cm:
self._logger.debug('I am too close to left wall. Correcting angle a bit..')
self._motor_pair.on_for_degrees(
steering=Steering.RIGHT_ON_SPOT.value,
speed=SpeedRPM(self._correction_speed_rpm),
degrees=_side_distance_correction_fix_degrees
)
elif distances_after['right'] < self._ideal_distance_cm:
self._logger.debug('I am too close to right wall. Correcting angle a bit..')
self._motor_pair.on_for_degrees(
steering=Steering.LEFT_ON_SPOT.value,
speed=SpeedRPM(self._correction_speed_rpm),
degrees=_side_distance_correction_fix_degrees
)
else:
self._logger.debug('No problem, i am fine between the walls.')
def sample1():
#move straight
mdiff.on_for_distance(speed = 50, distance_mm = 690)
sleep(5)
# turn left for 60 degrees
mdiff.turn_left(speed = 50, degrees = 60)
#arc with radius 150 and distance 65
mdiff.on_arc_right(SpeedRPM(50), 150, 65)
mdiff.on_for_distance(speed = 50, distance_mm = -145)
sleep(5)
# moving the arm down
medium_motorA.on_for_degrees(speed = 10, degrees = -110)
mdiff.on_arc_left(SpeedRPM(50), 125,67)
sleep(5)
mdiff.on_for_distance(10,50)
sleep(5)
#moving the arm up
medium_motorA.on_for_degrees(speed = 10, degrees = 110)
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(SpeedPercent(35).get_speed_pct(m), 35)
self.assertEqual(SpeedDPS(300).get_speed_pct(m), 300 / 1050 * 100)
self.assertEqual(SpeedNativeUnits(300).get_speed_pct(m), 300 / 1050 * 100)
self.assertEqual(SpeedDPM(30000).get_speed_pct(m), (30000 / 60) / 1050 * 100)
self.assertEqual(SpeedRPS(2).get_speed_pct(m), 360 * 2 / 1050 * 100)
self.assertEqual(SpeedRPM(100).get_speed_pct(m), (360 * 100 / 60) / 1050 * 100)
def _correct_bad_angle_after_turn(self, angle_before: int, angle_after: int, steering: Steering):
_min_angle_correction = 15
_angle_diff = abs(self._ideal_side_turn_angle - abs(angle_after - angle_before))
if _angle_diff < _min_angle_correction:
_angle_diff = _min_angle_correction
self._logger.debug('Angle diff = {}'.format(_angle_diff))
_rotations = (self._wheelbase_width_at_centers_mm * _angle_diff) / (self._wheel_diameter_mm * 360)
self._motor_pair.on_for_rotations(
steering=steering.value,
speed=SpeedRPM(self._correction_speed_rpm),
rotations=_rotations
)
def test_units(self):
clean_arena()
populate_arena([('large_motor', 0, 'outA'),
('large_motor', 1, 'outB')])
m = Motor()
self.assertEqual(
SpeedPercent(35).to_native_units(m), 35 / 100 * m.max_speed)
self.assertEqual(SpeedDPS(300).to_native_units(m), 300)
self.assertEqual(SpeedNativeUnits(300).to_native_units(m), 300)
self.assertEqual(SpeedDPM(30000).to_native_units(m), (30000 / 60))
self.assertEqual(SpeedRPS(2).to_native_units(m), 360 * 2)
self.assertEqual(SpeedRPM(100).to_native_units(m), (360 * 100 / 60))
def MoveForward(self, steering=0, speed=-20):
steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B, motor_class=LargeMotor)
drill = Drill()
#We make this condition to check if the Robot
if drill.Drilling() == 1:
steer_pair.off()
if self.is_drilled == False:
self.is_drilled = True
print("drilling")
sleep(2)
mm = MediumMotor(OUTPUT_D)
sp = 90
time = 10
mm.on_for_seconds(speed=SpeedRPM(sp), seconds=time)
print("Number of rotations =" + str(sp / 6))
else:
steer_pair.on_for_seconds(steering=0,
speed=-1 * SpeedRPM(12),
seconds=1,
brake=True,
block=True)
def _correct_angle_using_back_wall(self,
previous_distance_to_check: float):
if self._turns_until_next_angle_corretion <= 0:
if previous_distance_to_check < 4:
self._logger.debug(
'I am correcting my angle using the back wall...')
self._motor_pair.on_for_rotations(
steering=Steering.STRAIGHT.value,
speed=SpeedRPM(self._angle_corretcion_speed * -1),
rotations=(self._angle_correction_move_backward_mm /
self._wheel_circumference_mm),
brake=True,
block=True)
self._motor_pair.on_for_rotations(
steering=Steering.STRAIGHT.value,
speed=SpeedRPM(self._angle_corretcion_speed * 1),
rotations=(self._angle_correction_move_forward_mm /
self._wheel_circumference_mm),
brake=True,
block=True)
self._turns_until_next_angle_corretion = 3
else:
self._turns_until_next_angle_corretion = self._turns_until_next_angle_corretion - 1
def large_motor():
lm = LargeMotor()
'''
This will run the large motor at 50% of its
rated maximum speed of 1050 deg/s.
50% x 1050 = 525 deg/s
'''
lm.on_for_seconds(50, seconds=3)
sleep(1)
'''
speed and seconds are both POSITIONAL
arguments which means
you don't have to include the parameter names as
long as you put the arguments in this order
(speed then seconds) so this is the same as
the previous command:
'''
lm.on_for_seconds(50, 3)
sleep(1)
'''
This will run at 500 degrees per second (DPS).
You should be able to hear that the motor runs a
little slower than before.
'''
lm.on_for_seconds(SpeedDPS(500), seconds=3)
sleep(1)
# 36000 degrees per minute (DPM) (rarely useful!)
lm.on_for_seconds(SpeedDPM(36000), seconds=3)
sleep(1)
# 2 rotations per second (RPS)
lm.on_for_seconds(SpeedRPS(2), seconds=3)
sleep(1)
# 100 rotations per minute(RPM)
lm.on_for_seconds(SpeedRPM(100), seconds=3)
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()
def drive_motors(distance):
movediff = MoveDifferential(OUTPUT_A, OUTPUT_C, EV3EducationSetTire,
10 * 8)
movediff.on_for_distance(SpeedRPM(20), distance)
#!/usr/bin/env python3 from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveDifferential, SpeedRPM from ev3dev2.wheel import EV3Tire STUD_MM = 8 # test with a robot that: # - usa ruedas EV3tire # - wheels are 16 studs apart mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 14 * 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
#!/usr/bin/env python3 from ev3dev2.motor import MediumMotor, OUTPUT_A from ev3dev2.motor import SpeedDPS, SpeedRPS, SpeedRPM from time import sleep medium_motor = MediumMotor(OUTPUT_A) # We'll make the motor turn 180 degrees # at speed 50 (780 dps for the medium motor) medium_motor.on_for_degrees(speed=50, degrees=180) # then wait one second sleep(1) # then – 180 degrees at 500 dps medium_motor.on_for_degrees(speed=SpeedDPS(500), degrees=-180) sleep(1) # then 0.5 rotations at speed 50 (780 dps) medium_motor.on_for_rotations(speed=50, rotations=0.5) sleep(1) # then – 0.5 rotations at 1 rotation per second (360 dps) medium_motor.on_for_rotations(speed=SpeedRPS(1), rotations=-0.5) sleep(1) medium_motor.on_for_seconds(speed=SpeedRPM(60), seconds=5) sleep(1) medium_motor.on(speed=60) sleep(2) medium_motor.off()
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveDifferential, SpeedRPM from ev3dev2.wheel import EV3Tire STUD_MM = 8 # test with a robot that: # - uses the standard wheels known as EV3Tire # - wheels are 16 studs apart mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 17 * STUD_MM) # Rotate 90 degrees clockwise mdiff.turn_right(SpeedRPM(40), 90 / 1.666667)
def runStraight(speed, distanceMM):
mdiff.on_for_distance(SpeedRPM(speed), distanceMM)
def _turn_on_spot_deg(self, direction: Steering, degrees: int):
_rotations = (self._wheelbase_width_at_centers_mm *
degrees) / (self._wheel_diameter_mm * 360)
self._motor_pair.on_for_rotations(steering=direction.value,
speed=SpeedRPM(self._turn_speed_rpm),
rotations=_rotations)
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, MoveDifferential from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS from ev3dev2.wheel import EV3Tire from time import sleep from ev3dev2.motor import MediumMotor, OUTPUT_A medium_motorA = MediumMotor(OUTPUT_A) large_motorB = LargeMotor(OUTPUT_B) large_motorC = LargeMotor(OUTPUT_C) STUD_MM = 8 mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM) mdiff.on_for_distance(speed = 50, distance_mm = 690) sleep(5) mdiff.turn_left(speed = 50, degrees = 80) mdiff.on_arc_right(SpeedRPM(60), 150, 65) mdiff.on_for_distance(speed = 50, distance_mm = -145) sleep(5) medium_motorA.on_for_degrees(speed = 10, degrees = -110) mdiff.on_arc_left(SpeedRPM(60), 150,67) sleep(10) mdiff.on_for_distance(10,50) #sleep while picking up the first green piece #medium_motorA.on_for_degrees(speed = 10, degrees = -110) sleep(5) #mdiff.turn_left(speed = 35, degrees = 10) sleep(2) medium_motorA.on_for_degrees(speed = 10, degrees = 110) sleep(2) mdiff.on_arc_right(SpeedRPM(60), 5200, 400) sleep(5) medium_motorA.on_for_degrees(speed = 10, degrees = -110)
STUD_MM = 8
INCH_MM = 25.4
ONE_FOOT_CICLE_RADIUS_MM = (12 * INCH_MM) / 2
ONE_FOOT_CICLE_CIRCUMFERENCE_MM = 2 * pi * ONE_FOOT_CICLE_RADIUS_MM
# Testing with RileyRover
# http://www.damienkee.com/home/2013/8/2/rileyrover-ev3-classroom-robot-design.html
#
# The centers of the wheels are 16 studs apart but this is not the
# "effective" wheel seperation. Test drives of circles with
# a diameter of 1-foot shows that the effective wheel seperation is
# closer to 16.3 studs. ndward has a writeup that goes into effective
# wheel seperation.
# https://sites.google.com/site/ev3basic/ev3-basic-programming/going-further/writerbot-v1/drawing-arcs
mdiff = MoveDifferential(OUTPUT_A, OUTPUT_B, EV3Tire, 16.3 * STUD_MM)
# This goes crazy on brickpi3, does it do the same on ev3?
#mdiff.on_for_distance(SpeedRPM(-40), 720, brake=False)
#mdiff.on_for_distance(SpeedRPM(40), 720, brake=False)
# Test arc left/right turns
#mdiff.on_arc_right(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM, ONE_FOOT_CICLE_CIRCUMFERENCE_MM / 4)
mdiff.on_arc_left(SpeedRPM(80), ONE_FOOT_CICLE_RADIUS_MM,
ONE_FOOT_CICLE_CIRCUMFERENCE_MM)
# Test turning in place
#mdiff.turn_right(SpeedRPM(40), 180)
#mdiff.turn_left(SpeedRPM(40), 180)
