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)
