class Gripp3r(IRBeaconRemoteControlledTank):
def __init__(self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
grip_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
super().__init__(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
ir_sensor_port=ir_sensor_port,
ir_beacon_channel=ir_beacon_channel)
self.grip_motor = MediumMotor(address=grip_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.beacon = RemoteControl(sensor=self.ir_sensor,
channel=ir_beacon_channel)
self.speaker = Sound()
def grip_or_release_by_ir_beacon(self, speed: float = 50):
while True:
if self.beacon.beacon:
if self.touch_sensor.is_pressed:
self.speaker.play(
wav_file='/home/robot/sound/Air release.wav')
self.grip_motor.run_timed(
speed_sp=500,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.grip_motor.wait_while(Motor.STATE_RUNNING)
else:
self.speaker.play(
wav_file='/home/robot/sound/Airbrake.wav')
self.grip_motor.run_forever(speed_sp=-500)
while not self.touch_sensor.is_pressed:
pass
self.grip_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
while self.beacon.beacon:
pass
def main(self, speed: float = 1000):
self.grip_motor.run_timed(speed_sp=-500,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.grip_motor.wait_while(Motor.STATE_RUNNING)
Process(target=self.grip_or_release_by_ir_beacon, daemon=True).start()
self.keep_driving_by_ir_beacon(speed=speed)
class Rov3r(RemoteControlledTank):
def __init__(self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
gear_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
super().__init__(left_motor=left_motor_port,
right_motor=right_motor_port,
polarity=Motor.POLARITY_NORMAL)
self.gear_motor = MediumMotor(address=gear_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.beacon = RemoteControl(sensor=self.ir_sensor,
channel=ir_beacon_channel)
self.speaker = Sound()
self.dis = Screen()
def spin_gears(self, speed: float = 1000):
while True:
if self.beacon.beacon:
self.gear_motor.run_forever(speed_sp=speed)
else:
self.gear_motor.stop(stop_action=Motor.STOP_ACTION_HOLD)
def change_screen_when_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.dis.image.paste(
im=Image.open('/home/robot/image/Angry.bmp'))
else:
self.dis.image.paste(
im=Image.open('/home/robot/image/Fire.bmp'))
self.dis.update()
def make_noise_when_seeing_black(self):
while True:
if self.color_sensor.color == ColorSensor.COLOR_BLACK:
self.speaker.play(wav_file='/home/robot/sound/Ouch.wav').wait()
def main(self):
self.speaker.play(wav_file='/home/robot/sound/Yes.wav').wait()
Thread(target=self.make_noise_when_seeing_black, daemon=True).start()
Thread(target=self.spin_gears, daemon=True).start()
Thread(target=self.change_screen_when_touched, daemon=True).start()
super().main() # RemoteControlledTank.main()
class CuriosityRov3r(IRBeaconRemoteControlledTank):
def __init__(
self,
left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C,
medium_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1):
super().__init__(
left_motor_port=left_motor_port, right_motor_port=right_motor_port,
ir_sensor_port=ir_sensor_port, ir_beacon_channel=ir_beacon_channel)
self.medium_motor = MediumMotor(address=medium_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.beacon = RemoteControl(sensor=self.ir_sensor,
channel=ir_beacon_channel)
self.dis = Screen()
self.noise = Sound()
def spin_fan(self, speed: float = 1000):
while True:
if self.color_sensor.reflected_light_intensity > 20:
self.medium_motor.run_forever(speed_sp=speed)
else:
self.medium_motor.stop(stop_action=Motor.STOP_ACTION_HOLD)
def say_when_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.dis.image.paste(im=Image.open('/home/robot/image/Angry.bmp'))
self.dis.update()
self.noise.play(wav_file='/home/robot/sound/No.wav').wait()
self.medium_motor.run_timed(
speed_sp=-500,
time_sp=3000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.medium_motor.wait_while(Motor.STATE_RUNNING)
def main(self, speed: float = 1000):
Thread(target=self.say_when_touched,
daemon=True).start()
Thread(target=self.spin_fan,
daemon=True).start()
self.keep_driving_by_ir_beacon(speed=speed)
# right backward
LEFT_MOTOR.run_forever(speed_sp=-speed)
RIGHT_MOTOR.run_forever(speed_sp=0)
else:
LEFT_MOTOR.stop(stop_action=Motor.STOP_ACTION_BRAKE)
RIGHT_MOTOR.stop(stop_action=Motor.STOP_ACTION_BRAKE)
while True:
drive_once_by_ir_beacon(speed=1000)
if BEACON_CONTROL.beacon:
if TOUCH_SENSOR.is_pressed:
MEDIUM_MOTOR.run_timed(
speed_sp=500,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING)
else:
MEDIUM_MOTOR.run_forever(speed_sp=-500)
while not TOUCH_SENSOR.is_pressed:
pass
MEDIUM_MOTOR.stop(stop_action=Motor.STOP_ACTION_BRAKE)
while BEACON_CONTROL.beacon:
pass
LIGHTS = Leds()
SPEAKER = Sound()
CHEST_MOTOR.run_timed(speed_sp=-300,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
CHEST_MOTOR.wait_while(Motor.STATE_RUNNING)
while True:
if IR_SENSOR.proximity < 30:
LIGHTS.set_color(group=Leds.LEFT, color=Leds.RED, pct=1)
LIGHTS.set_color(group=Leds.RIGHT, color=Leds.RED, pct=1)
MEDIUM_MOTOR.stop(stop_action=Motor.STOP_ACTION_HOLD)
TAIL_MOTOR.stop(stop_action=Motor.STOP_ACTION_BRAKE)
SPEAKER.play(wav_file='/home/robot/sound/Snake hiss.wav')
CHEST_MOTOR.run_timed(speed_sp=1000,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
CHEST_MOTOR.wait_while(Motor.STATE_RUNNING)
MEDIUM_MOTOR.run_forever(speed_sp=1000)
TAIL_MOTOR.run_forever(speed_sp=-1000)
CHEST_MOTOR.run_timed(speed_sp=-300,
class Gripp3r:
def __init__(self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
grip_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.left_motor = LargeMotor(address=left_motor_port)
self.right_motor = LargeMotor(address=right_motor_port)
self.grip_motor = MediumMotor(address=grip_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.beacon = RemoteControl(sensor=self.ir_sensor,
channel=ir_beacon_channel)
self.speaker = Sound()
def keep_driving_by_ir_beacon(self, speed: float = 1000):
while True:
if self.beacon.red_up and self.beacon.blue_up:
# go forward
self.left_motor.run_forever(speed_sp=speed)
self.right_motor.run_forever(speed_sp=speed)
elif self.beacon.red_down and self.beacon.blue_down:
# go backward
self.left_motor.run_forever(speed_sp=-speed)
self.right_motor.run_forever(speed_sp=-speed)
elif self.beacon.red_up and self.beacon.blue_down:
# turn around left
self.left_motor.run_forever(speed_sp=-speed)
self.right_motor.run_forever(speed_sp=speed)
elif self.beacon.red_down and self.beacon.blue_up:
# turn around right
self.left_motor.run_forever(speed_sp=speed)
self.right_motor.run_forever(speed_sp=-speed)
elif self.beacon.red_up:
# turn left
self.left_motor.run_forever(speed_sp=0)
self.right_motor.run_forever(speed_sp=speed)
elif self.beacon.blue_up:
# turn right
self.left_motor.run_forever(speed_sp=speed)
self.right_motor.run_forever(speed_sp=0)
elif self.beacon.red_down:
# left backward
self.left_motor.run_forever(speed_sp=0)
self.right_motor.run_forever(speed_sp=-speed)
elif self.beacon.blue_down:
# right backward
self.left_motor.run_forever(speed_sp=-speed)
self.right_motor.run_forever(speed_sp=0)
else:
self.left_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
self.right_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
def grip_or_release_by_ir_beacon(self, speed: float = 50):
while True:
if self.beacon.beacon:
if self.touch_sensor.is_pressed:
self.speaker.play(
wav_file='/home/robot/sound/Air release.wav')
self.grip_motor.run_timed(
speed_sp=500,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.grip_motor.wait_while(Motor.STATE_RUNNING)
else:
self.speaker.play(
wav_file='/home/robot/sound/Airbrake.wav')
self.grip_motor.run_forever(speed_sp=-500)
while not self.touch_sensor.is_pressed:
pass
self.grip_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
while self.beacon.beacon:
pass
def main(self, speed: float = 1000):
self.grip_motor.run_timed(speed_sp=-500,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.grip_motor.wait_while(Motor.STATE_RUNNING)
Thread(target=self.grip_or_release_by_ir_beacon, daemon=True).start()
self.keep_driving_by_ir_beacon(speed=speed)
class R3ptar:
def __init__(self,
turn_motor_port: str = OUTPUT_A,
move_motor_port: str = OUTPUT_B,
scare_motor_port: str = OUTPUT_D,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
self.turn_motor = MediumMotor(address=turn_motor_port)
self.move_motor = LargeMotor(address=move_motor_port)
self.scare_motor = LargeMotor(address=scare_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.beacon = RemoteControl(sensor=self.ir_sensor,
channel=ir_beacon_channel)
self.noise = Sound()
def keep_driving_by_ir_beacon(self, speed: float = 1000):
while True:
if self.beacon.red_up and self.beacon.blue_up:
self.move_motor.run_forever(speed_sp=speed)
elif self.beacon.red_down and self.beacon.blue_down:
self.move_motor.run_forever(speed_sp=-speed)
elif self.beacon.red_up:
self.turn_motor.run_forever(speed_sp=-500)
self.move_motor.run_forever(speed_sp=speed)
elif self.beacon.blue_up:
self.turn_motor.run_forever(speed_sp=500)
self.move_motor.run_forever(speed_sp=speed)
elif self.beacon.red_down:
self.turn_motor.run_forever(speed_sp=-500)
self.move_motor.run_forever(speed_sp=-speed)
elif self.beacon.blue_down:
self.turn_motor.run_forever(speed_sp=500)
self.move_motor.run_forever(speed_sp=-speed)
else:
self.turn_motor.stop(stop_action=Motor.STOP_ACTION_HOLD)
self.move_motor.stop(stop_action=Motor.STOP_ACTION_COAST)
def bite_by_ir_beacon(self, speed: float = 1000):
while True:
if self.beacon.beacon:
self.noise.play(wav_file='/home/robot/sound/Snake hiss.wav')
self.scare_motor.run_timed(speed_sp=speed,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.scare_motor.wait_while(Motor.STATE_RUNNING)
self.scare_motor.run_timed(speed_sp=-300,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.scare_motor.wait_while(Motor.STATE_RUNNING)
while self.beacon.beacon:
pass
def run_away_if_chased(self):
while True:
if self.color_sensor.reflected_light_intensity > 30:
self.move_motor.run_timed(speed_sp=500,
time_sp=4000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.move_motor.wait_while(Motor.STATE_RUNNING)
for i in range(2):
self.turn_motor.run_timed(
speed_sp=500,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.turn_motor.wait_while(Motor.STATE_RUNNING)
self.turn_motor.run_timed(
speed_sp=-500,
time_sp=1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.turn_motor.wait_while(Motor.STATE_RUNNING)
def bite_if_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.noise.play(wav_file='/home/robot/sound/Snake hiss.wav')
self.scare_motor.run_timed(speed_sp=1000,
time_sp=1000,
stop_action=Motor.STOP_ACTION_COAST)
self.scare_motor.wait_while(Motor.STATE_RUNNING)
self.scare_motor.run_timed(speed_sp=-300,
time_sp=1000,
stop_action=Motor.STOP_ACTION_COAST)
self.scare_motor.wait_while(Motor.STATE_RUNNING)
def main(self, speed: float = 1000):
Process(target=self.bite_by_ir_beacon, daemon=True).start()
Process(target=self.bite_if_touched, daemon=True).start()
Process(target=self.run_away_if_chased, daemon=True).start()
self.keep_driving_by_ir_beacon(speed=speed)
class Dinor3x(IRBeaconRemoteControlledTank):
"""
Challenges:
- Can you make DINOR3X remote controlled with the IR-Beacon?
- Can you attach a colorsensor to DINOR3X, and make it behave differently
depending on which color is in front of the sensor
(red = walk fast, white = walk slow, etc.)?
"""
# https://sites.google.com/site/ev3python/learn_ev3_python/using-motors
MEDIUM_MOTOR_POWER_FACTOR = 1.4
def __init__(self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
jaw_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
super().__init__(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
ir_sensor_port=ir_sensor_port,
ir_beacon_channel=ir_beacon_channel)
self.jaw_motor = MediumMotor(address=jaw_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.beacon = RemoteControl(sensor=self.ir_sensor,
channel=ir_beacon_channel)
self.button = Button()
self.speaker = Sound()
def calibrate_legs(self):
self.left_motor.run_forever(speed_sp=100)
self.right_motor.run_forever(speed_sp=200)
while self.touch_sensor.is_pressed:
pass
self.left_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
self.right_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
self.left_motor.run_forever(speed_sp=400)
while not self.touch_sensor.is_pressed:
pass
self.left_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
self.left_motor.run_to_rel_pos(position_sp=-0.2 * 360,
speed_sp=500,
stop_action=Motor.STOP_ACTION_BRAKE)
self.left_motor.wait_while(Motor.STATE_RUNNING)
self.right_motor.run_forever(speed_sp=400)
while not self.touch_sensor.is_pressed:
pass
self.right_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
self.right_motor.run_to_rel_pos(position_sp=-0.2 * 360,
speed_sp=500,
stop_action=Motor.STOP_ACTION_BRAKE)
self.right_motor.wait_while(Motor.STATE_RUNNING)
self.left_motor.reset()
self.right_motor.reset()
def close_mouth(self):
self.jaw_motor.run_forever(speed_sp=self.MEDIUM_MOTOR_POWER_FACTOR *
200)
sleep(1)
self.jaw_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
def roar(self):
self.speaker.play(wav_file='/home/robot/sound/T-rex roar.wav')
self.jaw_motor.run_to_rel_pos(position_sp=-60,
speed_sp=self.MEDIUM_MOTOR_POWER_FACTOR *
400,
stop_action=Motor.STOP_ACTION_BRAKE)
self.jaw_motor.wait_while(Motor.STATE_RUNNING)
# FIXME: jaw keeps opening wider and wider and doesn't close
for i in range(12):
self.jaw_motor.run_timed(speed_sp=-self.MEDIUM_MOTOR_POWER_FACTOR *
400,
time_sp=0.05 * 1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.jaw_motor.wait_while(Motor.STATE_RUNNING)
self.jaw_motor.run_timed(speed_sp=self.MEDIUM_MOTOR_POWER_FACTOR *
400,
time_sp=0.05 * 1000,
stop_action=Motor.STOP_ACTION_BRAKE)
self.jaw_motor.wait_while(Motor.STATE_RUNNING)
self.jaw_motor.run_forever(speed_sp=self.MEDIUM_MOTOR_POWER_FACTOR *
200)
sleep(0.5)
def walk_until_blocked(self):
self.left_motor.run_forever(speed_sp=-400)
self.right_motor.run_forever(speed_sp=-400)
while self.ir_sensor.proximity >= 25:
pass
self.left_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
self.right_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
def run_away(self):
self.left_motor.run_to_rel_pos(speed_sp=750,
position_sp=3 * 360,
stop_action=Motor.STOP_ACTION_BRAKE)
self.right_motor.run_to_rel_pos(speed_sp=750,
position_sp=3 * 360,
stop_action=Motor.STOP_ACTION_BRAKE)
self.left_motor.wait_while(Motor.STATE_RUNNING)
self.right_motor.wait_while(Motor.STATE_RUNNING)
def jump(self):
"""
Dinor3x Mission 02 Challenge: make it jump
"""
...
# TRANSLATED FROM EV3-G MY BLOCKS
# -------------------------------
def leg_adjust(self,
cyclic_degrees: float,
speed: float = 1000,
leg_offset_percent: float = 0,
mirrored_adjust: bool = False,
brake: bool = True):
...
def leg_to_pos(self,
speed: float = 1000,
left_position: float = 0,
right_position: float = 0):
self.left_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
self.right_motor.stop(stop_action=Motor.STOP_ACTION_BRAKE)
self.left_motor.run_to_rel_pos(
speed_sp=speed,
position_sp=left_position - cyclic_position_offset(
rotation_sensor=self.left_motor.position, cyclic_degrees=360),
stop_action=Motor.STOP_ACTION_BRAKE)
self.left_motor.wait_while(Motor.STATE_RUNNING)
self.right_motor.run_to_rel_pos(
speed_sp=speed,
position_sp=right_position - cyclic_position_offset(
rotation_sensor=self.right_motor.position, cyclic_degrees=360),
stop_action=Motor.STOP_ACTION_BRAKE)
self.right_motor.wait_while(Motor.STATE_RUNNING)
def turn(self, speed: float = 1000, n_steps: int = 1):
...
def walk(self, speed: float = 1000):
...
def walk_steps(self, speed: float = 1000, n_steps: int = 1):
...
class Kraz33Hors3:
def __init__(
self,
back_foot_motor_port: str = OUTPUT_C, front_foot_motor_port: str = OUTPUT_B,
gear_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4, ir_beacon_channel: int = 1):
self.front_foot_motor = LargeMotor(address=front_foot_motor_port)
self.back_foot_motor = LargeMotor(address=back_foot_motor_port)
self.gear_motor = MediumMotor(address=gear_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.remote_control = RemoteControl(sensor=self.ir_sensor,
channel=ir_beacon_channel)
def drive_once_by_ir_beacon(
self,
speed: float = 1000 # deg/s
):
# forward
if self.remote_control.red_up and self.remote_control.blue_up:
self.front_foot_motor.run_timed(
speed_sp=speed,
time_sp=1000, # ms
stop_action=Motor.STOP_ACTION_COAST)
self.back_foot_motor.run_timed(
speed_sp=-speed,
time_sp=1000, # ms
stop_action=Motor.STOP_ACTION_COAST)
self.front_foot_motor.wait_while(Motor.STATE_RUNNING)
self.back_foot_motor.wait_while(Motor.STATE_RUNNING)
# backward
elif self.remote_control.red_down and self.remote_control.blue_down:
self.front_foot_motor.run_timed(
speed_sp=-speed,
time_sp=1000, # ms
stop_action=Motor.STOP_ACTION_COAST)
self.back_foot_motor.run_timed(
speed_sp=speed,
time_sp=1000, # ms
stop_action=Motor.STOP_ACTION_COAST)
self.front_foot_motor.wait_while(Motor.STATE_RUNNING)
self.back_foot_motor.wait_while(Motor.STATE_RUNNING)
# move crazily
elif self.remote_control.beacon:
self.gear_motor.run_forever(speed_sp=speed)
self.front_foot_motor.run_timed(
speed_sp=speed / 3,
time_sp=1000, # ms
stop_action=Motor.STOP_ACTION_COAST)
self.back_foot_motor.run_timed(
speed_sp=speed / 3,
time_sp=1000, # ms
stop_action=Motor.STOP_ACTION_COAST)
self.front_foot_motor.wait_while(Motor.STATE_RUNNING)
self.back_foot_motor.wait_while(Motor.STATE_RUNNING)
else:
self.gear_motor.stop(stop_action=Motor.STOP_ACTION_COAST)
def keep_driving_by_ir_beacon(
self,
speed: float = 1000 # deg/s
):
while True:
self.drive_once_by_ir_beacon(speed=speed)
def back_whenever_touched(
self,
speed: float = 1000 # deg/s
):
while True:
if self.touch_sensor.is_pressed:
self.front_foot_motor.run_timed(
speed_sp=-speed,
time_sp=1000, # ms
stop_action=Motor.STOP_ACTION_COAST)
self.back_foot_motor.run_timed(
speed_sp=speed,
time_sp=1000, # ms
stop_action=Motor.STOP_ACTION_COAST)
self.front_foot_motor.wait_while(Motor.STATE_RUNNING)
self.back_foot_motor.wait_while(Motor.STATE_RUNNING)
def main(self,
speed: float = 1000 # deg/s
):
Process(target=self.back_whenever_touched).start()
self.keep_driving_by_ir_beacon(speed=speed)
@Copyright: TrijeMaliKlinci
"""
from ev3dev.ev3 import LargeMotor, MediumMotor
from time import sleep
# motor_A = LargeMotor('outA')
# motor_B = LargeMotor('outD')
motor_C = MediumMotor('outC')
# motor_C.run_forever(speed_sp=-1000)
# sleep(0.25)
print(motor_C.position)
motor_C.run_forever(speed_sp=1000)
sleep(0.20)
motor_C.stop(stop_action='hold')
print(motor_C.position)
# motor_A.run_forever(speed_sp=400)
# motor_B.run_forever(speed_sp=400)
# motor_left.stop(stop_action='brake')
# motor_right.stop(stop_action='brake')
# time = 0.5
# speed = 900
# motor_A.run_forever(speed_sp=-speed)
# motor_B.run_forever(speed_sp=-speed)
# sleep(time)
# motor_A.run_forever(speed_sp=0)
# motor_B.run_forever(speed_sp=0)
class VerticalMovementManager:
"""
Initializes the manager.
whereMotor - interface on which vertical motor is attached
"""
def __init__(self, whereMotor=OUTPUT_A):
# Initialize motor
self._motor = MediumMotor(whereMotor)
if not self._motor.connected:
raise ValueError("Medium motor at " + whereMotor +
" not connected!")
self._motor.stop_action = MediumMotor.STOP_ACTION_HOLD
# Default speed is 0, setting this is necessary
self._motor.speed_sp = 500
# Position of the lift in percentage units, preserved across runs
self._pkl_pos = LiftPos.BOTTOM
# TODO if we need sensors for other things, initialize this manager
# outside of VerticalMovementManager and only pass a reference
self._sensors = ArduinoSensorsManager()
"""
Sets the stored position of the lift. Useful when it was moved
manually or fell.
"""
def set_position(self, pos):
self._pkl_pos = pos
"""
Moves the lift to the specified percentage position. Doesn't use
any sensors besides the motor tacho count.
pos - where to move
"""
def _move_to_raw(self, pos):
if self._pkl_pos != pos:
curr_pos_m = percent_to_motor(
self._pkl_pos) # Current position [motor]
pos_m = percent_to_motor(pos) # Desired position [motor]
self._motor.run_to_rel_pos(position_sp=pos_m - curr_pos_m)
self._motor.wait_until_not_moving()
self._pkl_pos = pos
class _MoveWhileResult(Enum):
STALLED = 1, # Engine was stalled
OVER_RANGE = 2, # Stopped at percentage range boundary
OVER_LIM = 3, # Stopped at given limit
COND = 4, # Stopped due to condition False
"""
Moves with the specified slowdown for as long as the given condition evalutes to True. An
optional maximum distance in motor units can be specified. Makes sure not to go outside of the
valid position range. Returns the distance travelled in motor units.
cond - () -> Boolean function. evaluated as frequently as possible. movement stops when False
mult - how many times to slow down. larger values allow for more precision and an earlier stop
[lim] - maximum distance to travel in motor units
"""
def _move_while(self, cond, mult=4, lim=None):
print("_move_while(cond={},mult={},lim={})".format(cond, mult, lim))
# First position within valid range
if self._pkl_pos < 0:
self._move_to_raw(0)
elif self._pkl_pos > 100:
self._move_to_raw(100)
init_pos = self._motor.position
sign = mult // abs(mult)
# Set motor parameters
self._motor.speed_sp //= mult
self._motor.polarity = 'normal' if mult > 0 else 'inversed'
self._motor.run_forever()
ret = None
while self._motor.is_running and cond():
if self._motor.is_stalled:
ret = self._MoveWhileResult.STALLED
break
else:
motor_pos = self._motor.position * sign
diff = motor_pos - init_pos
if lim != None and diff >= lim:
ret = self._MoveWhileResult.OVER_LIM
break
new_pos = self._pkl_pos + motor_to_percent(diff)
if new_pos <= -2 or new_pos >= 102:
ret = self._MoveWhileResult.OVER_RANGE
break
if ret == None:
ret = self._MoveWhileResult.COND
# Reset motor parameters
self._motor.stop()
self._motor.polarity = 'normal'
self._motor.speed_sp *= mult
# Update position
diff = self._motor.position - init_pos
self._pkl_pos += motor_to_percent(diff)
return ret
"""
Tries to position the lift in the middle of the switch.
reed - Reed switch number
pos - an approximate percentage position of the switch
"""
def _move_to_switch(self, reed, pos):
print("_move_to_switch(reed={},pos={})".format(reed, pos))
SPREAD = 7 # Minimum distance to the sensor to begin sensing
# Distance and direction to the sensor
diff = pos - self._pkl_pos
sign = int(diff / abs(diff))
assert (abs(sign) == 1)
see_mag = lambda: self._sensors.read_reed(reed)
if abs(diff) >= SPREAD:
if sign > 0:
print("Switch above lift")
else:
print("Switch below lift")
# Move up to sensor at full speed, pray it doesn't miss
mvd = self._move_while(lambda: not see_mag(), sign)
if mvd != self._MoveWhileResult.COND and mvd != self._MoveWhileResult.OVER_RANGE:
raise ValueError(
"ERROR: Sensor not within reach. Move result: " + str(mvd))
# Scale the peak to get to the center, use reduced speed for accuracy
mvd = self._move_while(see_mag, 5 * sign, 1000)
else:
print("WARNING: Lift close to desired position, not moving.")
return
# First find one of the peaks
mult = 4 * sign
if not see_mag():
print("Looking for peak 1")
diff = 500
mvd = self._move_while(lambda: not see_mag(), mult, diff)
while mvd != self._MoveWhileResult.COND:
print(mvd)
print("Moved far, reverting search direction")
diff *= 2
mult *= -1
mvd = self._move_while(lambda: not see_mag(), mult, diff)
sign = int(mult / abs(mult))
peak1 = [0, 0]
peak2 = [0, 0]
# We're on a peak, find its limits by going up and down
#print("Scanning peak 1")
self._move_while(see_mag, mult)
peak1[(sign + 1) // 2] = self._motor.position
#print("Scanning peak 1 in 2nd direction")
# Move a tiny bit down to be within peak again
mvd = self._move_while(lambda: not see_mag(), -mult, 500)
if mvd != self._MoveWhileResult.COND:
raise ValueError("ERROR: peak 1 not within reach")
self._move_while(see_mag, -mult)
peak1[(-sign + 1) // 2] = self._motor.position
print(peak1)
return
#print("Looking for peak 2")
mvd = self._move_while(lambda: not see_mag(), -4, 250)
if mvd == self._MoveWhileResult.OVER_LIM or mvd == self._MoveWhileResult.OVER_RANGE:
#print("Moved far down from peak 1, so peak 2 is above. Moving to center")
self._move_to_raw(self._pkl_pos +
motor_to_percent(peak1[1] -
self._motor.position))
self._move_to_raw(self._pkl_pos + 1)
else:
#print("Moved just a bit from peak 1, so peak 2 is below. Moving to center")
top = self._motor.position
self._move_to_raw(self._pkl_pos +
motor_to_percent(peak1[0] - top) / 2)
"""
Moves the lift to the specified percentage position, using all
available information.
pos - where to move
"""
def move_to(self, pos):
if not self._pkl_pos == pos:
reed = get_reed_id(pos)
if reed is not None:
# If there is a sensor at that height, position the lift
# accurately until it's at the sensor
self._move_to_switch(reed, pos)
else:
# Otherwise move to the position using just the tacho count
self._move_to_raw(pos)
