class AutoCar: def __init__(self, left_motor_port, right_motor_port): self.__movesteering = MoveSteering(left_motor_port, right_motor_port) self.__touch = TouchSensor() def __run_forward(self): self.__movesteering.on(0, 30) def __stop(self): self.__movesteering.off() def __play_text_sound(self, words): sound = Sound() sound.speak(words) def __back_left(self): self.__movesteering.on_for_rotations(50, 20, -1) def __touch_sensor_pressed(self): return self.__touch.is_pressed def __touch_execute(self): self.__stop() self.__play_text_sound("Ouch") #take some time, consume resource self.__back_left() def run(self): self.__run_forward() touched = self.__touch_sensor_pressed() if (touched): self.__touch_execute()
class AutoDrive: def __init__(self, left_motor_port, right_motor_port, infra_mode, ultra_mode): self.__moveSteering = MoveSteering(left_motor_port, right_motor_port) self.__mediumMotor = MediumMotor() self.__ir = InfraredSensor() self.__ir.mode = infra_mode self.__us = UltrasonicSensor() self.__us.mode = ultra_mode def __run_forward(self): self.__moveSteering.on(0, 30) def __run_backward_rotations(self, rotations): self.__moveSteering.on_for_rotations(0, 20, -rotations) def __stop(self): self.__moveSteering.off() def __back_and_turn(self, steering): self.__moveSteering.on_for_rotations(-steering, 20, -1) def __scan_around_distance(self): proximities = {} distance = 0 index = 0 for deg in [-90, 30, 30, 30, 30, 30, 30]: self.__mediumMotor.on_for_degrees(10, deg) distance = self.__ir.proximity proximities[-90 + index * 30] = distance index += 1 time.sleep(1) self.__mediumMotor.on_for_degrees(10, -90) # print("%s" % proximities) return proximities def __find_clear_direction(self, proximitiesDic): max_distance = max(list(proximitiesDic.values())) direction = list(proximitiesDic.keys())[list( proximitiesDic.values()).index(max_distance)] #print("%d" % direction) steering = self.__convert_direction_to_steering(direction) return steering def __convert_direction_to_steering(self, direction): return 5 * direction / 9 def __ultrasonic_distance(self): return self.__us.distance_centimeters def run(self): self.__run_forward() block_distance = self.__ultrasonic_distance() if (block_distance < 20): self.__stop() around_distance = self.__scan_around_distance() steering = self.__find_clear_direction(around_distance) self.__run_backward_rotations(0.5) self.__back_and_turn(steering)
def square(): motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C) for i in range(4): # Move robot forward for 3 seconds motor_pair.on_for_seconds(steering=0, speed=50, seconds=3) # Turn robot left 90 degrees (adjust rotations for your particular robot) motor_pair.on_for_rotations(steering=-100, speed=15, rotations=0.5)
class Driver: def __init__(self): self.driver = MoveSteering(OUTPUT_B, OUTPUT_C) self.speed = 40 def set_speed(self, speed): self.speed = max(-100, min(100, speed)) def get_speed(self): return self.speed def move(self): self.driver.on(0, SpeedPercent(self.speed)) def move_cm(self, cm): TRANSFORM_CONST = 37.0 self.driver.on_for_degrees(0, SpeedPercent(self.speed), cm * TRANSFORM_CONST) def move_neg_cm(self, cm): TRANSFORM_CONST = 37.0 self.driver.on_for_degrees(0, SpeedPercent(self.speed), -cm * TRANSFORM_CONST) def reverse(self): self.driver.on(0, SpeedPercent(-self.speed)) def reverse_cm(self, cm): TRANSFORM_CONST = 37.0 self.driver.on_for_degrees(0, SpeedPercent(-self.speed), cm*TRANSFORM_CONST) def stop(self): self.driver.off() def turn(self, steering): steering = max(-100, min(100, steering)) self.driver.on(steering, self.speed) def turn_rotations(self, steering, rotations): steering = max(-100, min(100, steering)) self.driver.on_for_rotations(steering, SpeedPercent(self.speed), rotations) def turn_degrees(self, degrees): TRANSFORM_CONST = 3.9 self.driver.on_for_degrees(100, SpeedPercent(self.speed), degrees * TRANSFORM_CONST) def turn_neg_degrees(self, degrees): TRANSFORM_CONST = 3.9 steering = 100 if degrees > 0 else -100 self.driver.on_for_degrees(steering, SpeedPercent(self.speed), -degrees * TRANSFORM_CONST) def move_seconds(self, seconds): self.driver.on_for_seconds(0, self.speed, seconds) def back_seconds(self, seconds): self.driver.on_for_seconds(0, -self.speed, seconds)
class Wheels: def __init__(self, left_address=OUTPUT_A, right_address=OUTPUT_B, touch_address=INPUT_3): self._wheels = MoveSteering(left_address, right_address) self._left = self._wheels.left_motor self._right = self._wheels.right_motor self._touch = TouchSensor(touch_address) def move_straight(self, speed=50): self._wheels.on(0, SpeedPercent(speed)) def move_for_distance(self, speed=50, distance_mm=100): MoveDifferential(OUTPUT_A, OUTPUT_B, EV3EducationSetTire, 80).on_for_distance(speed, distance_mm, brake=False) def rotate_right_in_place(self, speed=50, amount=1.0, brake=True, block=True): self._wheels.on_for_rotations(-100, SpeedPercent(speed), amount, brake=brake, block=block) def rotate_left_in_place(self, speed=50, amount=1.0, brake=True, block=True): self._wheels.on_for_rotations(100, SpeedPercent(speed), amount, brake=brake, block=block) def reverse(self, speed=50): self._wheels.on(0, SpeedPercent(-speed)) def reverse_until_bumped(self, speed=50, timeout=None): self.reverse(speed) time = datetime.now() ms = time.microsecond while not timeout or time.microsecond - ms < timeout: if self._touch.is_pressed: self._wheels.off() break def stop(self): self._wheels.off(brake=False)
class FoobotDrive: def __init__(self, wheel_diameter, wheel_dist, gear_ratio=1, default_speed=50, default_turn_speed=20): self.wheel_diameter = wheel_diameter self.wheel_circ = wheel_diameter * pi self.wheel_dist = wheel_dist self.default_speed = default_speed self.default_turn_speed = default_turn_speed self.gear_ratio = gear_ratio self.move_steering = MoveSteering(OUTPUT_A, OUTPUT_B) self.move_tank = MoveTank(OUTPUT_A, OUTPUT_B) def move_straigth(self, distance, speed=0): if speed == 0: speed = self.default_speed rotations = distance / self.wheel_dist / self.gear_ratio self.move_tank.stop() self.move_tank.on_for_rotations(self.default_speed, self.default_speed, rotations) def turn(self, deg, speed=0): steering = -100 if speed == 0: speed = self.default_turn_speed if deg < 0: deg = 0 - deg steering = 0 - steering rotation_arc = (pi * self.wheel_dist) * (deg / 360) rotations = rotation_arc / 8 / self.gear_ratio self.move_tank.stop() self.move_steering.on_for_rotations(steering, speed, rotations) def turn_right(self, deg, speed=0): self.turn(0 - deg, speed) def turn_left(self, deg, speed=0): self.turn(deg, speed) def run(self, speed=0): if speed == 0: speed = self.default_speed self.move_tank.on(speed) def stop(self): self.move_tank.stop()
def test_steering_units(self): clean_arena() populate_arena([('large_motor', 0, 'outA'), ('large_motor', 1, 'outB')]) drive = MoveSteering(OUTPUT_A, OUTPUT_B) drive.on_for_rotations(25, SpeedDPS(400), 10) self.assertEqual(drive.left_motor.position, 0) self.assertEqual(drive.left_motor.position_sp, 10 * 360) self.assertEqual(drive.left_motor.speed_sp, 400) self.assertEqual(drive.right_motor.position, 0) self.assertEqual(drive.right_motor.position_sp, 5 * 360) self.assertEqual(drive.right_motor.speed_sp, 200)
def move(): motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C) touch_sensor = TouchSensor() # Start robot moving forward motor_pair.on(steering=0, speed=60) # Wait until robot touches wall touch_sensor.wait_for_pressed() # Stop moving forward motor_pair.off() # Reverse away from wall motor_pair.on_for_seconds(steering=0, speed=-10, seconds=2) motor_pair.on_for_rotations(steering=-100, speed=15, rotations=0.5) motor_pair.on_for_rotations(steering=-100, speed=15, rotations=0.5)
class Robot(): def __init__(self, baseSpeed=500, dt=50): self.leftMotor = LargeMotor(OUTPUT_C) self.rightMotor = LargeMotor(OUTPUT_A) self.steeringDrive = MoveSteering(OUTPUT_C, OUTPUT_A) self.craneMotor = MediumMotor(OUTPUT_B) self.baseSpeed = baseSpeed self.dt = dt def steer(self, controlSignal): # ograniczenie sterowania leftMotorU = max(-1000, min(self.baseSpeed + controlSignal, 1000)) rightMotorU = max(-1000, min(self.baseSpeed - controlSignal, 1000)) # sterowanie silnikami self.leftMotor.run_timed(time_sp=self.dt, speed_sp=leftMotorU, stop_action="coast") self.rightMotor.run_timed(time_sp=self.dt, speed_sp=rightMotorU, stop_action="coast") sleep(self.dt / 1000) def rotateLeft(self): self.steeringDrive.on_for_rotations(-72, 40, 1) sleep(1) def rotateRight(self): self.steeringDrive.on_for_rotations(72, 40, 1) sleep(1) def liftCrane(self): self.craneMotor.on_for_degrees(20, 45) sleep(1) def dipCrane(self): self.craneMotor.on_for_degrees(-20, 45) sleep(1)
class GyroCar: def __init__(self, left_motor_port, right_motor_port, gyro_mode): self.__moveSteering = MoveSteering(left_motor_port, right_motor_port) self.__gyro = GyroSensor() self.__gyro.mode = gyro_mode def __run_forward(self): self.__moveSteering.on(0, 20) def run_rotations(self, rotations): self.__moveSteering.on_for_rotations(0, 20, rotations) def stop(self): self.__moveSteering.off() def __back_and_turn(self, steering): self.__moveSteering.on_for_rotations(-steering, 20, -1) def reset_angle(self): self.__gyro.reset() def is_on_flat(self): print("is_on_flat: %d" % self.__gyro.angle) time.sleep(1) return (math.fabs(self.__gyro.angle) < 10) def is_on_slope(self): print("is_on_slope: %d" % self.__gyro.angle) time.sleep(1) return (math.fabs(self.__gyro.angle) >= 10) def run(self): self.__run_forward() @property def angle(self): return self.__gyro.angle
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) wf = 1 # Set wf to 1 for home version and 0.77 for edu version # Adjust steer and rots until the robot moves neatly out of the traffic lane steer = 28 rots = -1.8 steer_pair.on_for_rotations(steering=0, speed=25, rotations=3.7 * wf) steer_pair.on_for_rotations(steering=steer, speed=15, rotations=rots * wf) steer_pair.on_for_rotations(steering=-steer, speed=15, rotations=rots * wf) steer_pair.on_for_rotations(steering=0, speed=25, rotations=0.7 * wf)
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from ev3dev2.sensor.lego import TouchSensor from ev3dev2.sound import Sound from time import sleep steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) ts = TouchSensor() sound = Sound() while True: # forever steer_pair.on(steering=0, speed=30) ts.wait_for_pressed() # play_type=1 means 'Play the sound without blocking the program' #sound.play_file('/home/robot/sounds/Backing alert.wav',play_type=1) # Back up along a curved path steer_pair.on_for_rotations(steering=-20, speed=-25, rotations=3)
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank, MoveSteering from ev3dev2.sensor import INPUT_1 from ev3dev2.sensor.lego import TouchSensor from ev3dev2.led import Leds from ev3dev2.sound import Sound # B - left, C- right # 18 rotations: 3 m tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) #drive robot when touch sensor is pressed. tank_drive.on_for_rotations(75, 75, 18) tank_drive.on(SpeedPercent(0), SpeedPercent(0)) steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C) # drive in a turn for 10 rotations of the outer motor steering_drive.on_for_rotations(70, SpeedPercent(75), 1.7) tank_drive.on_for_rotations(75, 75, 18) # drive classes: tank, steeing, joystick; # https://python-ev3dev.readthedocs.io/en/ev3dev-stretch/motors.html#multiple-motor-groups # motor classes: https://python-ev3dev.readthedocs.io/en/ev3dev-stretch/motors.html#units
class MindstormsGadget(AlexaGadget): """ A Mindstorms gadget that can perform bi-directional interaction with an Alexa skill. """ def __init__(self): """ Performs Alexa Gadget initialization routines and ev3dev resource allocation. """ super().__init__() # Robot state self.patrol_mode = False self.follow_mode = False # Internal Variables self.light_intensity = 0 self.batt_voltage = 0 # Connect two large motors on output ports B and C #self.drive = MoveTank(OUTPUT_D, OUTPUT_C) self.steerdrive = MoveSteering(OUTPUT_C, OUTPUT_D) self.leds = Leds() self.ir = InfraredSensor() self.ir.mode = 'IR-SEEK' self.touch = TouchSensor() self.light = ColorSensor(address='ev3-ports:in4') self.sound = Sound() # Start threads threading.Thread(target=self._patrol_thread, daemon=True).start() threading.Thread(target=self._follow_thread, daemon=True).start() threading.Thread(target=self._pat_thread, daemon=True).start() threading.Thread(target=self._power_thread, daemon=True).start() threading.Thread(target=self._light_sensor_thread, daemon=True).start() def on_connected(self, device_addr): """ Gadget connected to the paired Echo device. :param device_addr: the address of the device we connected to """ self.leds.set_color("LEFT", "GREEN") self.leds.set_color("RIGHT", "GREEN") logger.info("{} connected to Echo device".format(self.friendly_name)) def on_disconnected(self, device_addr): """ Gadget disconnected from the paired Echo device. :param device_addr: the address of the device we disconnected from """ self.leds.set_color("LEFT", "BLACK") self.leds.set_color("RIGHT", "BLACK") logger.info("{} disconnected from Echo device".format(self.friendly_name)) def on_custom_mindstorms_gadget_control(self, directive): """ Handles the Custom.Mindstorms.Gadget control directive. :param directive: the custom directive with the matching namespace and name """ try: payload = json.loads(directive.payload.decode("utf-8")) print("Control payload: {}".format(payload), file=sys.stderr) control_type = payload["type"] if control_type == "move": # Expected params: [direction, duration, speed] self._move(payload["direction"], int(payload["duration"]), int(payload["speed"])) if control_type == "command": # Expected params: [command] self._activate(payload["command"]) if control_type == "follow": self.follow_mode = True if control_type == "stopfollow": self.follow_mode = False except KeyError: print("Missing expected parameters: {}".format(directive), file=sys.stderr) def _move(self, direction, duration: int, speed: int, is_blocking=False): """ Handles move commands from the directive. Right and left movement can under or over turn depending on the surface type. :param direction: the move direction :param duration: the duration in seconds :param speed: the speed percentage as an integer :param is_blocking: if set, motor run until duration expired before accepting another command """ print("Move command: ({}, {}, {}, {})".format(direction, speed, duration, is_blocking), file=sys.stderr) if direction in Direction.FORWARD.value: self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking) if direction in Direction.BACKWARD.value: self.drive.on_for_seconds(SpeedPercent(-speed), SpeedPercent(-speed), duration, block=is_blocking) if direction in (Direction.RIGHT.value + Direction.LEFT.value): self._turn(direction, speed) self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(speed), duration, block=is_blocking) if direction in Direction.STOP.value: self.drive.off() self.patrol_mode = False def _activate(self, command, speed=50): """ Handles preset commands. :param command: the preset command :param speed: the speed if applicable """ print("Activate command: ({}, {})".format(command, speed), file=sys.stderr) if command in Command.MOVE_CIRCLE.value: self.drive.on_for_seconds(SpeedPercent(int(speed)), SpeedPercent(5), 12) if command in Command.MOVE_SQUARE.value: for i in range(4): self._move("right", 2, speed, is_blocking=True) if command in Command.PATROL.value: # Set patrol mode to resume patrol thread processing self.patrol_mode = True if command in Command.SENTRY.value: self.sentry_mode = True self._send_event(EventName.SPEECH, {'speechOut': "Sentry mode activated"}) # Perform Shuffle posture self.drive.on_for_seconds(SpeedPercent(80), SpeedPercent(-80), 0.2) time.sleep(0.3) self.drive.on_for_seconds(SpeedPercent(-40), SpeedPercent(40), 0.2) self.leds.set_color("LEFT", "YELLOW", 1) self.leds.set_color("RIGHT", "YELLOW", 1) def _turn(self, direction, speed): """ Turns based on the specified direction and speed. Calibrated for hard smooth surface. :param direction: the turn direction :param speed: the turn speed """ if direction in Direction.LEFT.value: self.drive.on_for_seconds(SpeedPercent(0), SpeedPercent(speed), 2) if direction in Direction.RIGHT.value: self.drive.on_for_seconds(SpeedPercent(speed), SpeedPercent(0), 2) def _send_event(self, name: EventName, payload): """ Sends a custom event to trigger a sentry action. :param name: the name of the custom event :param payload: the sentry JSON payload """ self.send_custom_event('Custom.Mindstorms.Gadget', name.value, payload) def _patrol_thread(self): """ Performs random movement when patrol mode is activated. """ while True: while self.patrol_mode: print("Patrol mode activated randomly picks a path", file=sys.stderr) direction = random.choice(list(Direction)) duration = random.randint(1, 5) speed = random.randint(1, 4) * 25 while direction == Direction.STOP: direction = random.choice(list(Direction)) # direction: all except stop, duration: 1-5s, speed: 25, 50, 75, 100 self._move(direction.value[0], duration, speed) time.sleep(duration) time.sleep(1) def _pat_thread(self): """ Detects when the touch sensor is pressed. """ while True: self.touch.wait_for_bump() sound = "Ahh, I like that." self._send_event(EventName.SPEECH, {'speechOut': sound}) def _light_sensor_thread(self): """ """ while True: self.light.mode='COL-AMBIENT' time.sleep(0.5) self.light_intensity = self.light.ambient_light_intensity if self.batt_voltage < 3.6: # Set the LED to be red. self.light.mode='REF-RAW' else: self.light.mode='COL-COLOR' # Set the LED to be white. print("Light Intensity: ", self.light_intensity) time.sleep(5) def _follow_thread(self): """ The thread to manage following the lease. """ while True: if self.follow_mode: # Get heading to beacon heading = self.ir.heading() print("IR Heading: ", heading) # Can't see the beacon if heading == 0: time.sleep(1) continue drive_dir = -heading # Drive self.steerdrive.on_for_rotations(drive_dir, SpeedPercent(30), 2, block=True) time.sleep(1) def _power_thread(self): """ Sends power output to Alexa skill. """ charge_current_pid = 'FIXME' load_current_pid = 'FIXME' batt_voltage_pid = 'FIXME' time.sleep(2) while True: try: # list properties_v2 api_response = api_instance.properties_v2_show(id, batt_voltage_pid) print('Battery Voltage: ', round(api_response.last_value, 3)) voltage = round(api_response.last_value, 3) voltage = 3.54 self.batt_voltage = voltage except ApiException as e: print("Exception when calling PropertiesV2Api->propertiesV2List: %s\n" % e) try: api_response = api_instance.properties_v2_show(id, load_current_pid) print('Load Current: ', round(api_response.last_value, 2)) load_current = round(api_response.last_value, 1) except ApiException as e: print("Exception when calling PropertiesV2Api->propertiesV2List: %s\n" % e) try: api_response = api_instance.properties_v2_show(id, charge_current_pid) print('Charge Current: ', round(api_response.last_value, 2)) charge_current = round(api_response.last_value, 1) except ApiException as e: print("Exception when calling PropertiesV2Api->propertiesV2List: %s\n" % e) time.sleep(15) self._send_event(EventName.POWER, {'voltage': voltage, 'load_current': load_current, 'charge_current': charge_current, 'light':self.light_intensity })
wheels.on(0, SpeedPercent(50)) while proximity_sensor.distance_centimeters >= 22: print('First wall: {}'.format(proximity_sensor.distance_centimeters)) wheels.on(-100, SpeedPercent(15)) start_angle = gyro.angle while gyro.angle > -82: print(gyro.angle) wheels.on(0, SpeedPercent(50)) while proximity_sensor.distance_centimeters >= 12: pass wheels.off(brake=True) front_claw.on_for_degrees(SpeedPercent(20), -570) wheels.on_for_rotations(0, SpeedPercent(25), 0.5) front_claw.on_for_degrees(SpeedPercent(100), 570, block=False) wheels.on_for_rotations(0, SpeedPercent(65), 0.4) front_claw.on_for_degrees(SpeedPercent(20), -570) wheels.on(0, SpeedPercent(-50)) while not touch_sensor.is_pressed: pass wheels.off() gyro.mode = GyroSensor.MODE_GYRO_CAL gyro.mode = GyroSensor.MODE_GYRO_RATE gyro.mode = GyroSensor.MODE_GYRO_ANG time.sleep(1)
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from time import sleep steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) # 스티어링 모드 설정 steer_pair.on(0,50) # 직진 스팅어링으로 속도 50% 무한 주행 sleep(1) steer_pair.off(brake=True) # 주행 멈춤 sleep(1) steer_pair.on_for_seconds(30,50, 2) # 좌회전 스팅어링으로 속도 50% 2초 주행 sleep(1) # 직진 스팅어링으로 속도 50% 반대방향 3회전 주행 steer_pair.on_for_rotations(0,50, -3) sleep(1) steer_pair.on_for_degrees(0,50, 180) # 직진 스팅어링으로 속도 50% 180도 주행 sleep(1)
minDistance = 20 # Minimum distance (in cm) before the brick starts decelerating or stops to turn around # _maxSpeed = 400 # Flag to be used as manual start/stop switch; default is False (brick does not move) started = False while True: # Brick LED is yellow when ready (waiting for a button press) leds.set_color('LEFT', 'YELLOW') leds.set_color('RIGHT', 'YELLOW') if button.any(): started = True leds.set_color('LEFT', 'GREEN') leds.set_color('RIGHT', 'GREEN') motor.on(SpeedPercent(motorSpeed), SpeedPercent(motorSpeed)) while started is True: if ultrasonic.distance_centimeters < minDistance: motor.off() # Stop motors death = 4 while ultrasonic.distance_centimeters < minDistance: steer.on_for_rotations(steeringValue, steeringSpeed, steeringDegrees) death -= 1 if (death == 0): started = False break # Run the motors up to 'motorSpeed' degrees per second: if death != 0: motor.on(SpeedPercent(motorSpeed), SpeedPercent(motorSpeed))
elif steering < -100: return -100 else: return steering # Start program reset() starting = True while starting or infrared_sensor.distance( ) == None or infrared_sensor.distance() >= 50: starting = False search() steering_drive.on_for_rotations(steering=0, speed=SpeedPercent(75), rotations=4) while infrared_sensor.distance() > 2: steering_drive.on(steering=(safe_steering(infrared_sensor.heading() * 5)), speed=SpeedPercent(50)) steering_drive.off() sound.play_file("/home/robot/sounds/Detected.wav", play_type=sound.PLAY_WAIT_FOR_COMPLETE) steering_drive.on_for_rotations(steering=0, speed=SpeedPercent(50), rotations=0.7) grab()
clear_screen=True) SCREEN.update() TANK_DRIVER.on_for_rotations(left_speed=75, right_speed=75, rotations=5, brake=True, block=True) SCREEN.image_filename(filename='/home/robot/image/Middle left.bmp', clear_screen=True) SCREEN.update() STEER_DRIVER.on_for_rotations(steering=50, speed=75, rotations=5, brake=True, block=True) SCREEN.image_filename(filename='/home/robot/image/Neutral.bmp', clear_screen=True) SCREEN.update() TANK_DRIVER.on_for_rotations(left_speed=75, right_speed=75, rotations=5, brake=True, block=True) SCREEN.image_filename(filename='/home/robot/image/Middle right.bmp', clear_screen=True)
print(gyro.angle) wheels.on(0, SpeedPercent(20)) while proximity_sensor.distance_centimeters >= 10: pass wheels.off(brake=True) # wheels.on_for_rotations(0, SpeedPercent(10), 0.2) # moves up to get the claw on the extend_arm.on_for_rotations(SpeedPercent(-20), 1.2, False) # front_claw.on_for_degrees(SpeedPercent(20), -570) # clenches the claw # wheels.on_for_rotations(0, SpeedPercent(10), 0.5) # moves up to get the claw on the # front_claw.on_for_degrees(SpeedPercent(100), 570, block=False) #opens the claw again # wheels.on_for_rotations(0, SpeedPercent(-50), 1) extend_arm.on_for_rotations(SpeedPercent(60), 1.2, False) wheels.on_for_rotations(0, SpeedPercent(35), 1) wheels.on_for_rotations(0, SpeedPercent(-50), 1) # front_claw.on_for_degrees(SpeedPercent(20), -570) wheels.on(0, SpeedPercent(-25)) while not touch_sensor.is_pressed: pass wheels.off() gyro.mode = GyroSensor.MODE_GYRO_CAL gyro.mode = GyroSensor.MODE_GYRO_RATE gyro.mode = GyroSensor.MODE_GYRO_ANG
def walkRotations(direction, velocity, rotations): steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C) steering_drive.on_for_rotations(direction, SpeedPercent(velocity), rotations)
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from time import sleep steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) # gentle turn left steer_pair.on_for_rotations(steering=-25, speed=75, rotations=2) sleep(1) # same as above but using degrees instead of rotations steer_pair.on_for_degrees(steering=-25, speed=75, degrees=720) sleep(1) # turn right on the spot for 3 seconds steer_pair.on_for_seconds(steering=100, speed=40, seconds=3) sleep(1) # equivalent to above steer_pair.on(steering=100, speed=40) sleep(3) steer_pair.off()
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_D, OUTPUT_A from ev3dev2.motor import MediumMotor, OUTPUT_B from ev3dev2.motor import MediumMotor, MoveSteering, OUTPUT_A, OUTPUT_D from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM from time import sleep lmA = LargeMotor(OUTPUT_A) lmD = LargeMotor(OUTPUT_D) SmB = MediumMotor(OUTPUT_B) steer_pair = MoveSteering(OUTPUT_D, OUTPUT_A) steer_pair.on_for_rotations(0,SpeedRPS(2),3) SmB.on_for_seconds(SpeedRPS(1),1) steer_pair.on_for_rotations(0,SpeedRPS(2),3) SmB.on_for_seconds(SpeedRPS(-1),1)
#!/usr/bin/env python3 from ev3dev2.motor import OUTPUT_B, OUTPUT_C, MoveSteering from ev3dev2.sensor.lego import TouchSensor from time import sleep ts = TouchSensor() sound = Sound() steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) steer_pair.on_for_rotations(steering=0, speed=50, rotations=4.5, brake=True, block=True) sleep(1) steer_pair.on_for_degrees(steering=100, speed=10, degrees=180, brake=True, block=True) sleep(1) steer_pair.on_for_rotations(steering=0, speed=50, rotations=3) sleep(1) steer_pair.on_for_degrees(steering=100, speed=10, degrees=180, brake=True, block=True) sleep(1) steer_pair.on_for_rotations(steering=0, speed=50, rotations=5) sleep(1) steer_pair.on_for_degrees(steering=100, speed=10, degrees=180, brake=True, block=True) sleep(1) steer_pair.on(steering=0, speed=50) while not ts.is_pressed: # while touch sensor is not pressed sleep(0.01) tank_pair.off() sleep(5)
right_speed=75, rotations=2, brake=True, block=True) MEDIUM_MOTOR.on(speed=100, brake=False, block=False) SPEAKER.play_file(wav_file='/home/robot/sound/Airbrake.wav', volume=100, play_type=Sound.PLAY_WAIT_FOR_COMPLETE) sleep(0.5) STEER_DRIVER.on_for_rotations(steering=35, speed=75, rotations=3, brake=True, block=True) MEDIUM_MOTOR.on(speed=-100, block=False, brake=False) SPEAKER.play_file(wav_file='/home/robot/sound/Air release.wav', volume=100, play_type=Sound.PLAY_WAIT_FOR_COMPLETE) sleep(0.5) STEER_DRIVER.on_for_rotations(steering=35, speed=75, rotations=-3, brake=True,
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from time import sleep # Demonstrate how to use MoveSteering class. # Have the robot rotate 360 degrees on one wheel then the other # Using the same four methods we are now familiar with: # on(), on_for_degrees(), on_for_rotations(), on_for_seconds() steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C) steering_drive.on(-100, 35) sleep(2) steering_drive.on(100, 35) sleep(2) steering_drive.stop() # Store the value of 2 360 rotations in a variable degrees = 360 * 2 # -100 = turn all the way to the left; steering_drive.on_for_degrees(-100, 30, degrees) # 100 = turn all the way to the right steering_drive.on_for_degrees(100, 30, degrees) steering_drive.on_for_rotations(-100, 30, 2) steering_drive.on_for_rotations(100, 30, 2)
class EV3Motors(Motors): def __init__(self, distance_sensors: EV3DistanceDetectors, gyro: Gyro, logger=None, **kwargs): self._logger = logger or logging.getLogger(__name__) self._distance_sensors = distance_sensors self._gyro = gyro self._motor_pair = MoveSteering(OUTPUT_A, OUTPUT_B) self._position_corrector = PositionCorrector(self._motor_pair, self._gyro) self._maze_square_length_mm = KwArgsUtil.kwarg_or_default( 180, 'maze_square_length_mm', **kwargs) self._move_forward_speed_rpm = KwArgsUtil.kwarg_or_default( 60, 'move_forward_speed_rpm', **kwargs) self._motor_pair_polarity_factor = KwArgsUtil.kwarg_or_default( 1, 'motor_pair_polarity_factor', **kwargs) self._turn_speed_rpm = KwArgsUtil.kwarg_or_default( 50, 'turn_speed_rpm', **kwargs) self._wheel_diameter_mm = KwArgsUtil.kwarg_or_default( 56, 'wheel_diameter_mm', **kwargs) self._wheel_circumference_mm = math.pi * self._wheel_diameter_mm self._wheelbase_width_at_centers_mm = KwArgsUtil.kwarg_or_default( 97.5, 'wheelbase_width_at_centers_mm', **kwargs) self._turns_until_next_angle_corretion = KwArgsUtil.kwarg_or_default( 3, 'turns_until_next_angle_corretion', **kwargs) self._angle_corretcion_speed = KwArgsUtil.kwarg_or_default( 25, 'angle_corretcion_speed', **kwargs) self._angle_correction_move_backward_mm = KwArgsUtil.kwarg_or_default( 80.0, 'angle_correction_move_backward_mm', **kwargs) self._angle_correction_move_forward_mm = KwArgsUtil.kwarg_or_default( 20.0, 'angle_correction_move_forward_mm', **kwargs) self._wait_for_motors_and_gyro_after_move_sec = KwArgsUtil.kwarg_or_default( 0.1, 'wait_for_motors_and_gyro_after_move_sec', **kwargs) def _log_distances_and_angle(self, phase: str, distances: dict, angle: int): self._logger.debug('Distances {}: left={}, right={}, front={}'.format( phase, distances['left'], distances['right'], distances['front'])) self._logger.debug('Gyro angle {}={}'.format(phase, angle)) 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 _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) 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 _move(self, move_function, correct_function): _distances_before = self._distance_sensors.get_distances() _angle_before = self._gyro.get_orientation() self._log_distances_and_angle('before', _distances_before, _angle_before) move_function() # Allow some time for motors to stop and gyro to react time.sleep(self._wait_for_motors_and_gyro_after_move_sec) _distances_after = self._distance_sensors.get_distances() _angle_after = self._gyro.get_orientation() self._log_distances_and_angle('after move before correction', _distances_after, _angle_after) correct_function(distances_before=_distances_before, angle_before=_angle_before, distances_after=_distances_after, angle_after=_angle_after) def move_forward(self): def move_function(): self._move_forward_mm(distance_mm=self._maze_square_length_mm, speed_rpm=self._move_forward_speed_rpm) def correct_function(angle_before, distances_after, angle_after, **kwargs): self._position_corrector.correct_after_move_forward( angle_before, distances_after, angle_after) self._logger.debug('Move_forward') self._move(move_function, correct_function) self._logger.debug('Move_forward done') def turn_left(self): def move_function(): self._turn_on_spot_deg(direction=Steering.LEFT_ON_SPOT, degrees=74) def correct_function(distances_before, angle_before, angle_after, **kwargs): self._position_corrector.correct_after_turn_left( angle_before, angle_after) self._correct_angle_using_back_wall(distances_before['right']) self._logger.debug('turn_left') self._move(move_function, correct_function) self._logger.debug('turn_left done') def turn_right(self): def move_function(): self._turn_on_spot_deg(direction=Steering.RIGHT_ON_SPOT, degrees=74) def correct_function(distances_before, angle_before, angle_after, **kwargs): self._position_corrector.correct_after_turn_right( angle_before, angle_after) self._correct_angle_using_back_wall(distances_before['left']) self._logger.debug('turn_right') self._move(move_function, correct_function) self._logger.debug('turn_right done') def turn_back(self): self._logger.debug('turn_back') _turn_method = random.choice([self.turn_left, self.turn_right]) _turn_method() _turn_method() self._logger.debug('turn_back done') def no_turn(self): pass
while proximity_sensor.distance_centimeters >= 22: print('First wall: {}'.format(proximity_sensor.distance_centimeters)) wheels.on(-100, SpeedPercent(15)) start_angle = gyro.angle while gyro.angle > -82: print(gyro.angle) wheels.on(0, SpeedPercent(50)) while proximity_sensor.distance_centimeters >= 12: pass wheels.off(brake=True) front_claw.on_for_degrees(SpeedPercent(20), -570) # clenches the claw wheels.on_for_rotations(0, SpeedPercent(25), 0.5) # moves up to get the claw on the front_claw.on_for_degrees(SpeedPercent(100), 570, block=False) # opens the claw again wheels.on_for_rotations(0, SpeedPercent(-100), 0.5) wheels.on_for_rotations(0, SpeedPercent(100), 0.5) wheels.on_for_rotations(0, SpeedPercent(-100), 0.5) front_claw.on_for_degrees(SpeedPercent(20), -570) wheels.on(0, SpeedPercent(-50)) while not touch_sensor.is_pressed: pass wheels.off() gyro.mode = GyroSensor.MODE_GYRO_CAL
while True: if lml.position > 328 or lmr.position > 328: #299 mt.off() break sleep(0.1) lml.reset() lmr.reset() if os.path.isfile(GREEN_LIGHT_path): os.remove(GREEN_LIGHT_path) if os.path.isfile(GREEN_LIGHT_path): print('Clean GREEN_LIGHT') os.remove(GREEN_LIGHT_path) # 第一個右彎 ms.on_for_rotations(50, 100, 1) # gs.angle > 76, 1.08 ms.wait_until_not_moving() # 2 mt.on(rs+0.2, rs) while True: if csl.reflected_light_intensity > 39 or csr.reflected_light_intensity > 39: mt.off() break ''' if os.path.isfile(RESTART_TRAFFIC_LIGHT_path):
#!/usr/bin/env python3 """ Move robot in a square path without using the Gyro sensor. This script is a simple demonstration of moving forward and turning. """ from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C) for i in range(4): # Move robot forward for 3 seconds motor_pair.on_for_seconds(steering=0, speed=50, seconds=3) # Turn robot left 90 degrees (adjust rotations for your particular robot) motor_pair.on_for_rotations(steering=-100, speed=5, rotations=0.5)