Пример #1
0
 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
Пример #2
0
 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)
Пример #3
0
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)
Пример #4
0
 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
     )
Пример #7
0
    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)
Пример #8
0
 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)
Пример #9
0
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)
Пример #10
0
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
     )
Пример #13
0
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.')
Пример #15
0
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)
Пример #16
0
    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
     )
Пример #18
0
    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))
Пример #19
0
    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)
Пример #20
0
 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
Пример #21
0
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)
Пример #22
0
        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()
Пример #23
0
def drive_motors(distance):
    movediff = MoveDifferential(OUTPUT_A, OUTPUT_C, EV3EducationSetTire,
                                10 * 8)
    movediff.on_for_distance(SpeedRPM(20), distance)
Пример #24
0
#!/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
Пример #25
0
#!/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()
Пример #26
0
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)
Пример #27
0
def runStraight(speed, distanceMM):
    mdiff.on_for_distance(SpeedRPM(speed), distanceMM)
Пример #28
0
 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)
Пример #29
0
#!/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)
Пример #30
0
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)