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
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)
Program za vodenje robota EV3 [Robo liga FRI 2019: Sadovnjak] @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)
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)
LEDS = Leds() SPEAKER = Sound() while True: if IR_SENSOR.proximity < 25: LEFT_FOOT_MOTOR.stop(stop_action=Motor.STOP_ACTION_BRAKE) RIGHT_FOOT_MOTOR.stop(stop_action=Motor.STOP_ACTION_BRAKE) LEDS.set_color(group=Leds.LEFT, color=Leds.RED, pct=1) LEDS.set_color(group=Leds.RIGHT, color=Leds.RED, pct=1) SPEAKER.play(wav_file='/home/robot/sound/Object.wav').wait() SPEAKER.play(wav_file='/home/robot/sound/Detected.wav').wait() SPEAKER.play(wav_file='/home/robot/sound/Error alarm.wav').wait() MEDIUM_MOTOR.run_forever(speed_sp=1000 # degrees per second ) LEFT_FOOT_MOTOR.run_to_rel_pos( position_sp=360, # degrees speed_sp=1000, # degrees per second stop_action=Motor.STOP_ACTION_BRAKE) RIGHT_FOOT_MOTOR.run_to_rel_pos( position_sp=360, # degrees speed_sp=800, # degrees per second stop_action=Motor.STOP_ACTION_BRAKE) LEFT_FOOT_MOTOR.wait_while(Motor.STATE_RUNNING) RIGHT_FOOT_MOTOR.wait_while(Motor.STATE_RUNNING) MEDIUM_MOTOR.run_forever(speed_sp=-1000 # degrees per second )