def __init__(self, init_pose, soc=None, port_motor_rear=OUTPUT_A, port_motor_steer=OUTPUT_B, port_sensor_gyro='in1'): # initialization of devices self.__button_halt = Button() self.__motor_rear = LargeMotor(port_motor_rear) self.__motor_steer = LargeMotor(port_motor_steer) self.__sensor_gyro = GyroSensor(port_sensor_gyro) self.__velocity_controller = VelocityController(self, 0, 0, adaptation=False) self.s = soc # NOTE: possible using other controllers. Change only here! self.__trajectory_controller = ControllerWithLinearization() self.__path_controller = PathController() self.__localization = Localization(self, init_pose) self.__robot_state = [0, 0, 0, 0, 0, 0] # x,y, dx,dy, theta,omega self.reset() Sound.speak('Ready').wait()
def __init__(self, port_motor_rear=OUTPUT_A, port_motor_steer=OUTPUT_B, port_sensor_gyro='in1', port_sensor_us_front='in2', port_sensor_us_rear='in3'): # initialization of devices self.__button_halt = Button() self.__motor_rear = LargeMotor(port_motor_rear) self.__motor_steer = LargeMotor(port_motor_steer) self.__sensor_gyro = GyroSensor(port_sensor_gyro) self.__sensor_us_rear = UltrasonicSensor(port_sensor_us_rear) self.__sensor_us_front = UltrasonicSensor(port_sensor_us_front) self.__velocity_controller = VelocityController(self, 0, 0) # NOTE: possible using other controllers. Change only here! self.__trajectory_controller = ControllerWithLinearization() self.__localization = Localization(self) self.__robot_state = [0, 0, 0, 0, 0, 0] # x,y, dx,dy, theta,omega self.reset() Sound.speak('Initialization complete!').wait()
def __init__(self): self.left_color_sensor = ColorSensor('in4') self.right_color_sensor = ColorSensor('in1') self.left_large_motor = LargeMotor('outD') self.right_large_motor = LargeMotor('outA') self.touch_sensor = TouchSensor('in3') self.listeners = [] self.prev_is_pressed = False
def init_large_motor(port: str) -> LargeMotor: """ Preveri, ali je motor priklopljen na izhod `port`. Vrne objekt za motor (LargeMotor). """ motor = LargeMotor(port) while not motor.connected: print('\nPriklopi motor na izhod ' + port + ' in pritisni + spusti gumb DOL.') wait_for_button('down') motor = LargeMotor(port) return motor
def speedUp(self, motor: ev3.LargeMotor, speed: int) -> None: """ Proportionally speed up the motor as speed increases. Args: motor (ev3.LargeMotor): the motor to speed up. speed (int): amount to speed up by. Returns: None: calculates and sets speed of motor. """ motorSpeed = speed * self.__amplify motor.run_forever(speed_sp=motorSpeed)
def slowDown(self, motor: ev3.LargeMotor, speed: int) -> None: """ Proportionally slow down the motor as speed increases. Args: motor (ev3.LargeMotor): the motor to slow down. speed (int): amount to slow down by. Returns: None: calculates and sets speed of motor. """ motorSpeed = (self.__maxIntensity - speed) * self.__amplify motor.run_forever(speed_sp=motorSpeed)
def __init__(self, left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C, polarity: str = Motor.POLARITY_NORMAL, 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.left_motor.polarity = self.right_motor.polarity = polarity self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.tank_drive_remote_control = \ RemoteControl( sensor=self.ir_sensor, channel=ir_beacon_channel)
def __init__(self, motor, speed=0): self._motor = LargeMotor(motor) assert self._motor.connected self._motor.reset() self._motor.position = 0 self._motor.stop_action = 'brake' self._set_speed(speed)
def __init__(self, motor1Port, motor2Port, gyroPort=None, wheelDiameter=None): #init if GyroSensor != None: self.GS = GyroSensor(gyroPort) else: self.GS = GyroSensor() self.M1 = LargeMotor(motor1Port) self.M2 = LargeMotor(motor2Port) self.motor_stop = True self.wheelDiameter = wheelDiameter self.time = 0 self.MDistanceRunning = True self.distance = 0 self.pauseDistance = []
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 __init__(self, left_foot_track_motor_port: str = OUTPUT_B, right_foot_track_motor_port: str = OUTPUT_C, bazooka_blast_motor_port: str = OUTPUT_A, touch_sensor_port: str = INPUT_1, color_sensor_port: str = INPUT_3, ir_sensor_port: str = INPUT_4, driving_ir_beacon_channel: int = 1, shooting_ir_beacon_channel: int = 2): self.left_foot_track_motor = LargeMotor( address=left_foot_track_motor_port) self.right_foot_track_motor = LargeMotor( address=right_foot_track_motor_port) self.bazooka_blast_motor = MediumMotor( address=bazooka_blast_motor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.driving_remote_control = RemoteControl( sensor=self.ir_sensor, channel=driving_ir_beacon_channel) self.shooting_remote_control = RemoteControl( sensor=self.ir_sensor, channel=shooting_ir_beacon_channel) self.touch_sensor = TouchSensor(address=INPUT_1) self.color_sensor = ColorSensor(address=INPUT_3) self.leds = Leds() self.screen = Screen() assert self.left_foot_track_motor.connected assert self.right_foot_track_motor.connected assert self.bazooka_blast_motor.connected assert self.ir_sensor.connected assert self.touch_sensor.connected assert self.color_sensor.connected # reset the motors for motor in (self.left_foot_track_motor, self.right_foot_track_motor, self.bazooka_blast_motor): motor.reset() motor.position = 0 motor.stop_action = Motor.STOP_ACTION_BRAKE self.draw_face()
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 __init__( self, left_foot_motor_port: str = OUTPUT_B, right_foot_motor_port: str = OUTPUT_C, bazooka_blast_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.left_foot_motor = LargeMotor(address=left_foot_motor_port) self.right_foot_motor = LargeMotor(address=right_foot_motor_port) self.bazooka_blast_motor = MediumMotor(address=bazooka_blast_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) self.screen = Screen() self.speaker = Sound()
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 __init__(self, left_motor_port, right_motor_port, front_us_port, right_us_port, left_us_port): self.leftMotor = LargeMotor('out' + left_motor_port) self.rightMotor = LargeMotor('out' + right_motor_port) self.FRONT_US_SENSOR = UltrasonicSensor('in' + front_us_port) self.RIGHT_US_SENSOR = UltrasonicSensor('in' + right_us_port) self.LEFT_US_SENSOR = UltrasonicSensor('in' + left_us_port) self.TOUCH_SENSOR = TouchSensor() assert self.leftMotor.connected, "Connect left Motor to port" + \ str(left_motor_port) assert self.rightMotor.connected, "Connect right Motor to port" + \ str(right_motor_port) assert self.TOUCH_SENSOR.connected, "Connect a touch sensor" assert self.FRONT_US_SENSOR.connected, "Connect the ultrasound sensor in the front" assert self.RIGHT_US_SENSOR.connected, "Connect the ultrasound sensor on the right" assert self.LEFT_US_SENSOR.connected, "Connect the ultrasound sensor on the left" #set sensor mode to cm self.FRONT_US_SENSOR.mode = 'US-DIST-CM' self.RIGHT_US_SENSOR.mode = 'US-DIST-CM' self.LEFT_US_SENSOR.mode = 'US-DIST-CM'
def __init__(self, sting_motor_port: str = OUTPUT_D, go_motor_port: str = OUTPUT_B, claw_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.sting_motor = LargeMotor(address=sting_motor_port) self.go_motor = LargeMotor(address=go_motor_port) self.claw_motor = MediumMotor(address=claw_motor_port) self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel self.beacon = RemoteControl(sensor=self.ir_sensor, channel=self.ir_beacon_channel) self.touch_sensor = TouchSensor(address=touch_sensor_port) self.color_sensor = ColorSensor(address=color_sensor_port) self.dis = Screen() self.speaker = Sound()
#!/usr/bin/python from ev3dev.ev3 import MediumMotor as MediumMotor from ev3dev.ev3 import LargeMotor as LargeMotor from time import sleep a = MediumMotor(address='outA') b = LargeMotor(address='outB') c = LargeMotor(address='outC') a.reset() b.reset() c.reset() a.position_sp = 50 a.duty_cycle_sp = 50 a.command = 'run-to-abs-pos' b.position_sp = -450 b.duty_cycle_sp = 50 b.command = 'run-to-abs-pos' c.position_sp = -450 b.duty_cycle_sp = 50 b.command = 'run-to-abs-pos' sleep(5)
#!/usr/bin/env python3 from ev3dev.ev3 import (Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_D, InfraredSensor, INPUT_4, Leds, Sound) from random import randint from time import sleep MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) TAIL_MOTOR = LargeMotor(address=OUTPUT_B) CHEST_MOTOR = LargeMotor(address=OUTPUT_D) IR_SENSOR = InfraredSensor(address=INPUT_4) 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)
#!/usr/bin/env python3 from ev3dev.ev3 import ( Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, TouchSensor, InfraredSensor, RemoteControl, INPUT_1, INPUT_4, Sound ) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) LEFT_MOTOR = LargeMotor(address=OUTPUT_B) RIGHT_MOTOR = LargeMotor(address=OUTPUT_C) TOUCH_SENSOR = TouchSensor(address=INPUT_1) IR_SENSOR = InfraredSensor(address=INPUT_4) BEACON_CONTROL = RemoteControl(sensor=IR_SENSOR, channel=1) SPEAKER = Sound() def drive_once_by_ir_beacon(speed: float = 1000): if BEACON_CONTROL.red_up and BEACON_CONTROL.blue_up: # go forward LEFT_MOTOR.run_forever(speed_sp=speed) RIGHT_MOTOR.run_forever(speed_sp=speed) elif BEACON_CONTROL.red_down and BEACON_CONTROL.blue_down: # go backward LEFT_MOTOR.run_forever(speed_sp=-speed)
class Robot(object): def __init__(self): self.left_color_sensor = ColorSensor('in4') self.right_color_sensor = ColorSensor('in1') self.left_large_motor = LargeMotor('outD') self.right_large_motor = LargeMotor('outA') self.touch_sensor = TouchSensor('in3') self.listeners = [] self.prev_is_pressed = False def move_forward(self): self.left_large_motor.run_forever(speed_sp=DEFAULT_POWER) self.right_large_motor.run_forever(speed_sp=DEFAULT_POWER) def move_backward(self): self.left_large_motor.run_forever(speed_sp=-DEFAULT_POWER) self.right_large_motor.run_forever(speed_sp=-DEFAULT_POWER) def steer_left(self): self.left_large_motor.run_forever(speed_sp=NON_STEERING_POWER) self.right_large_motor.run_forever(speed_sp=STEERING_POWER) def steer_right(self): self.left_large_motor.run_forever(speed_sp=STEERING_POWER) self.right_large_motor.run_forever(speed_sp=NON_STEERING_POWER) def turn_left(self): self.stop() self.left_large_motor.run_timed(speed_sp=-TURNING_POWER, time_sp=TURNING_MILLISECONDS) self.right_large_motor.run_timed(speed_sp=TURNING_POWER, time_sp=TURNING_MILLISECONDS) # Block any calls to the motor while the train is turning self.left_large_motor.wait_while('running') self.right_large_motor.wait_while('running') def turn_right(self): self.stop() self.left_large_motor.run_timed(speed_sp=TURNING_POWER, time_sp=TURNING_MILLISECONDS) self.right_large_motor.run_timed(speed_sp=-TURNING_POWER, time_sp=TURNING_MILLISECONDS) # Block any calls to the motor while the train is turning self.left_large_motor.wait_while('running') self.right_large_motor.wait_while('running') def stop(self): self.left_large_motor.stop() self.right_large_motor.stop() def step(self): left_color = self.left_color_sensor.color right_color = self.right_color_sensor.color if left_color == Color.INVALID or right_color == Color.INVALID: for listener in self.listeners: listener.on_invalid(self, left_color == Color.INVALID, right_color == Color.INVALID) if left_color == Color.BLACK or right_color == Color.BLACK: for listener in self.listeners: listener.on_black(self, left_color == Color.BLACK, right_color == Color.BLACK) if left_color == Color.BLUE or right_color == Color.BLUE: for listener in self.listeners: listener.on_blue(self, left_color == Color.BLUE, right_color == Color.BLUE) if left_color == Color.GREEN or right_color == Color.GREEN: for listener in self.listeners: listener.on_green(self, left_color == Color.GREEN, right_color == Color.GREEN) if left_color == Color.YELLOW or right_color == Color.YELLOW: for listener in self.listeners: listener.on_yellow(self, left_color == Color.YELLOW, right_color == Color.YELLOW) if left_color == Color.RED or right_color == Color.RED: for listener in self.listeners: listener.on_red(self, left_color == Color.RED, right_color == Color.RED) if left_color == Color.WHITE or right_color == Color.WHITE: for listener in self.listeners: listener.on_white(self, left_color == Color.WHITE, right_color == Color.WHITE) if left_color == Color.BROWN or right_color == Color.BROWN: for listener in self.listeners: listener.on_brown(self, left_color == Color.BROWN, right_color == Color.BROWN) if self.prev_is_pressed and not self.touch_sensor.is_pressed: for listener in self.listeners: listener.on_click(self) self.prev_is_pressed = self.touch_sensor.is_pressed def add_listener(self, listener: 'RobotListener'): self.listeners.append(listener)
#!/usr/bin/python from ev3dev.ev3 import LargeMotor as LargeMotor from ev3dev.ev3 import TouchSensor as TouchSensor from time import sleep power = -100 run_time = 3 motor_b = LargeMotor(address='outB') motor_c = LargeMotor(address='outC') motor_b.reset() motor_c.reset() motor_b.command = 'run-direct' motor_c.command = 'run-direct' trigger = TouchSensor() print("Fire when ready!") while True: if trigger.value() == True: break else: sleep(.01)
class Ev3rstorm: def __init__(self, left_foot_motor_port: str = OUTPUT_B, right_foot_motor_port: str = OUTPUT_C, bazooka_blast_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.left_foot_motor = LargeMotor(address=left_foot_motor_port) self.right_foot_motor = LargeMotor(address=right_foot_motor_port) self.bazooka_blast_motor = MediumMotor( address=bazooka_blast_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) self.screen = Screen() self.speaker = Sound() def drive_once_by_ir_beacon( self, speed: float = 1000 # degrees per second ): # forward if self.remote_control.red_up and self.remote_control.blue_up: self.left_foot_motor.run_forever(speed_sp=speed) self.right_foot_motor.run_forever(speed_sp=speed) # backward elif self.remote_control.red_down and self.remote_control.blue_down: self.left_foot_motor.run_forever(speed_sp=-speed) self.right_foot_motor.run_forever(speed_sp=-speed) # turn left on the spot elif self.remote_control.red_up and self.remote_control.blue_down: self.left_foot_motor.run_forever(speed_sp=-speed) self.right_foot_motor.run_forever(speed_sp=speed) # turn right on the spot elif self.remote_control.red_down and self.remote_control.blue_up: self.left_foot_motor.run_forever(speed_sp=speed) self.right_foot_motor.run_forever(speed_sp=-speed) # turn left forward elif self.remote_control.red_up: self.right_foot_motor.run_forever(speed_sp=speed) # turn right forward elif self.remote_control.blue_up: self.left_foot_motor.run_forever(speed_sp=speed) # turn left backward elif self.remote_control.red_down: self.right_foot_motor.run_forever(speed_sp=-speed) # turn right backward elif self.remote_control.blue_down: self.left_foot_motor.run_forever(speed_sp=-speed) # otherwise stop else: self.left_foot_motor.stop(stop_action=Motor.STOP_ACTION_COAST) self.right_foot_motor.stop(stop_action=Motor.STOP_ACTION_COAST) def blast_bazooka_if_touched(self): if self.touch_sensor.is_pressed: if self.color_sensor.ambient_light_intensity < 5: # 15 not dark enough self.speaker.play(wav_file='/home/robot/sound/Up.wav').wait() self.bazooka_blast_motor.run_to_rel_pos( speed_sp=1000, # degrees per second position_sp=-3 * 360, # degrees stop_action=Motor.STOP_ACTION_HOLD) else: self.speaker.play(wav_file='/home/robot/sound/Down.wav').wait() self.bazooka_blast_motor.run_to_rel_pos( speed_sp=1000, # degrees per second position_sp=3 * 360, # degrees stop_action=Motor.STOP_ACTION_HOLD) while self.touch_sensor.is_pressed: pass def main( self, driving_speed: float = 1000 # degrees per second ): self.screen.image.paste(im=Image.open('/home/robot/image/Target.bmp')) self.screen.update() while True: self.drive_once_by_ir_beacon(speed=driving_speed) self.blast_bazooka_if_touched()
def run(fun): # Connect two motors and two (different) light sensors mL = LargeMotor('outC') mR = LargeMotor('outB') sL = ColorSensor('in1') sR = ColorSensor('in4') gy = GyroSensor('in3') # Check if the sensors are connected assert sL.connected, "Left ColorSensor is not connected" assert sR.connected, "Right ColorSensor is not conected" assert gy.connected, "Gyro is not connected" gyro = Gyro() light_sensors = LightSensors() encoder = Encoder() # Set the motor mode mL.polarity = "normal" # "inversed" mR.polarity = "normal" # "inversed" mL.position = 0 mR.position = 0 def stop_motors(): mL.run_direct() mR.run_direct() mL.duty_cycle_sp = 0 mR.duty_cycle_sp = 0 stop_motors() # The example doesn't end on its own. # Use CTRL-C to exit it (needs command line). # This is a generic way to be informed # of this event and then take action. def signal_handler(sig, frame): stop_motors() print('Shut down gracefully') exit(0) # Install the signal handler for CTRL+C signal(SIGINT, signal_handler) print('Press Ctrl+C to exit') per = { 'mL': mL, 'mR': mR, 'sL': sL, 'sR': sR, 'gy': gy } light_sensors.init(per) gyro.init(gy) # Endless loop reading sensors and controlling motors. last_log = time() last_now = time() max_dt = 0 dts = 0 speed_mL = None speed_mR = None while True: state = {} state = light_sensors(per, state) state = encoder(per, state) state = gyro(per, state) state = fun(per, state) max_speed = 40 * TICKS_PER_CM _speed_mL = state.get('mL', 0) if _speed_mL != speed_mL: speed_mL = _speed_mL mL.run_forever(speed_sp=speed_mL/100 * max_speed) _speed_mR = state.get('mR', 0) if _speed_mR != speed_mR: speed_mR = _speed_mR mR.run_forever(speed_sp=speed_mR/100 * max_speed) dts += 1 now = time() dt = now - last_now last_now = now if dt > max_dt: max_dt = dt if now - last_log > 5.0: last_log = now print("avg fps: ", dts / 5.0) print('min fps: ', 1 / max_dt) max_dt = 0 dts = 0
#!/usr/bin/env python3 from ev3dev.ev3 import (Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, InfraredSensor, RemoteControl, BeaconSeeker, INPUT_4, Leds, Sound) LEFT_FOOT_MOTOR = LargeMotor(address=OUTPUT_B) RIGHT_FOOT_MOTOR = LargeMotor(address=OUTPUT_C) BAZOOKA_BLAST_MOTOR = MediumMotor(address=OUTPUT_A) IR_SENSOR = InfraredSensor(address=INPUT_4) REMOTE_CONTROL = RemoteControl(sensor=IR_SENSOR, channel=1) BEACON_SEEKER = BeaconSeeker(sensor=IR_SENSOR, channel=1) LEDS = Leds() SPEAKER = Sound() while True: LEDS.set_color(group=Leds.LEFT, color=Leds.ORANGE, pct=1) LEDS.set_color(group=Leds.RIGHT, color=Leds.ORANGE, pct=1) if REMOTE_CONTROL.beacon: heading_difference = BEACON_SEEKER.heading - (-3) proximity_difference = BEACON_SEEKER.distance - 70 if (heading_difference == 0) and (proximity_difference == 0): LEFT_FOOT_MOTOR.stop(stop_action=Motor.STOP_ACTION_HOLD) RIGHT_FOOT_MOTOR.stop(stop_action=Motor.STOP_ACTION_HOLD)
class IRBeaconRemoteControlledTank: def __init__(self, left_motor_port: str = OUTPUT_B, right_motor_port: str = OUTPUT_C, polarity: str = Motor.POLARITY_NORMAL, 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.left_motor.polarity = self.right_motor.polarity = polarity self.ir_sensor = InfraredSensor(address=ir_sensor_port) self.tank_drive_remote_control = \ RemoteControl( sensor=self.ir_sensor, channel=ir_beacon_channel) def drive_once_by_ir_beacon( self, speed: float = 1000 # degrees per second ): # forward if self.tank_drive_remote_control.red_up and \ self.tank_drive_remote_control.blue_up: self.left_motor.run_forever(speed_sp=speed) self.right_motor.run_forever(speed_sp=speed) # backward elif self.tank_drive_remote_control.red_down and \ self.tank_drive_remote_control.blue_down: self.left_motor.run_forever(speed_sp=-speed) self.right_motor.run_forever(speed_sp=-speed) # turn left on the spot elif self.tank_drive_remote_control.red_up and \ self.tank_drive_remote_control.blue_down: self.left_motor.run_forever(speed_sp=-speed) self.right_motor.run_forever(speed_sp=speed) # turn right on the spot elif self.tank_drive_remote_control.red_down and \ self.tank_drive_remote_control.blue_up: self.left_motor.run_forever(speed_sp=speed) self.right_motor.run_forever(speed_sp=-speed) # turn left forward elif self.tank_drive_remote_control.red_up: self.right_motor.run_forever(speed_sp=speed) # turn right forward elif self.tank_drive_remote_control.blue_up: self.left_motor.run_forever(speed_sp=speed) # turn left backward elif self.tank_drive_remote_control.red_down: self.right_motor.run_forever(speed_sp=-speed) # turn right backward elif self.tank_drive_remote_control.blue_down: self.left_motor.run_forever(speed_sp=-speed) # otherwise stop else: self.left_motor.stop(stop_action=Motor.STOP_ACTION_COAST) self.right_motor.stop(stop_action=Motor.STOP_ACTION_COAST) # this method must be used in a parallel process/thread # in order not to block other operations def keep_driving_by_ir_beacon( self, speed: float = 1000 # degrees per second ): while True: self.drive_once_by_ir_beacon(speed=speed)
from ev3dev.ev3 import LargeMotor, Sound, \ GyroSensor, OUTPUT_A, INPUT_1 from time import sleep, time from math import sin, pi N = 1000 motor = LargeMotor(OUTPUT_A) gyro = GyroSensor(INPUT_1) Sound().beep() fout = open("data.txt", "w") sleep(1) start_time = time() for i in range(0, N): t = (time() - start_time) * 1000 rotation = gyro.value() + 4 u_float = 100 * sin(pi * t / N) motor.run_forever(speed_sp=u_float * 10) #should use speed_sp!!! s = "%d\t%d\t%d" % (t, rotation, u_float) fout.write(s) sleep(0.004) fout.close()
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 MyMotor(object): def __init__(self, motor, speed=0): self._motor = LargeMotor(motor) assert self._motor.connected self._motor.reset() self._motor.position = 0 self._motor.stop_action = 'brake' self._set_speed(speed) @property def speed(self): return int(self._speed/10) def _set_speed(self, speed): assert speed >= -100 and speed <= 100 self._speed = speed*10 def run_forever(self, speed): if speed is not None: self._set_speed(speed) self._motor.run_forever(speed_sp=self._speed) def run_timed(self, timed, speed): if speed is not None: self._set_speed(speed) runtimed = timed * 1000 self._motor.run_timed(speed_sp=self._speed, time_sp=runtimed) def run_position(self, postion, speed, lastpostion=False): if speed is not None: self._set_speed(speed) if lastpostion: self._motor.run_to_abs_pos(speed_sp=self._speed) else: self._motor.run_to_rel_pos(speed_sp=self._speed) def stop(self): self._motor.stop() def run_back(self, speed): if not self._speed: self.stop() self._set_speed(speed) self._motor.run_forever(speed_sp=self._speed)
#!/usr/bin/env python from ev3dev.ev3 import LargeMotor, \ OUTPUT_A, Sound, PowerSupply from time import time, sleep from math import pi, copysign N = 1000 file_name = "data_pc.txt" Kp = 20 pwr = 0 motor = LargeMotor(OUTPUT_A) battary = PowerSupply() Sound().beep() fout = open(file_name, "w") sleep(0.05) motor.reset() print(motor.position) start_time = time() for i in range(0, N): t = time() - start_time rotation = motor.position e = (360 - rotation) * pi / 180 pwr = Kp * e pwr = pwr / battary.measured_volts * 100 if abs(pwr) > 100: pwr = copysign(1, pwr) * 100 motor.run_direct(duty_cycle_sp=pwr) line = "%f\t%d\n" % (t, rotation) fout.write(line)
#!/usr/bin/env python3 from ev3dev.ev3 import ( Motor, MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_D, InfraredSensor, INPUT_4, Sound ) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) GO_MOTOR = LargeMotor(address=OUTPUT_B) STING_MOTOR = LargeMotor(address=OUTPUT_D) INFRARED_SENSOR = InfraredSensor(address=INPUT_4) SPEAKER = Sound() MEDIUM_MOTOR.run_timed( speed_sp=500, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) MEDIUM_MOTOR.run_timed( speed_sp=-500, time_sp=0.3 * 1000, stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING)
#!/usr/bin/env python3 from ev3dev.ev3 import (Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_D, INPUT_4, InfraredSensor, Sound) from threading import Thread from time import sleep LARGE_MOTOR = LargeMotor(address=OUTPUT_D) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) IR_SENSOR = InfraredSensor(address=INPUT_4) SPEAKER = Sound() def rattle(): while True: MEDIUM_MOTOR.run_timed(speed_sp=100, time_sp=1000, stop_action=Motor.STOP_ACTION_COAST) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) SPEAKER.play(wav_file='/home/robot/sound/Snake rattle.wav').wait() MEDIUM_MOTOR.run_timed(speed_sp=-100, time_sp=1000, stop_action=Motor.STOP_ACTION_COAST) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) sleep(1)
#!/usr/bin/env python3 from ev3dev.ev3 import LargeMotor, ColorSensor, GyroSensor, InfraredSensor, Button from Classes.Motores import * from Classes.PID import * from time import time, sleep from os import system import paho.mqtt.client as mqtt # Motores M_PORTA = LargeMotor("outC") M_ESQUERDO = LargeMotor("outA") M_DIREITO = LargeMotor("outB") # Sensores infravermelhos PROX1 = InfraredSensor("in4") PROX2 = InfraredSensor("in3") PROX1.mode = "IR-PROX" PROX2.mode = "IR-PROX" # Giroscopio GYRO = GyroSensor("in1") GYRO.mode = "GYRO-ANG" # Sensor de cor COLOR = ColorSensor("in2") COLOR.mode = "COL-COLOR" # Variaveis usadas durante o programa # Constantes TEMPO_CURVA_90 = 1700
#!/usr/bin/env python3 from ev3dev.ev3 import (Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, Sound) from time import sleep LEFT_MOTOR = LargeMotor(address=OUTPUT_B) RIGHT_MOTOR = LargeMotor(address=OUTPUT_C) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) SPEAKER = Sound() MEDIUM_MOTOR.run_forever(speed_sp=-1000) for i in range(2): LEFT_MOTOR.run_to_rel_pos( position_sp=2 * 360, # degrees speed_sp=750, # degrees per second stop_action=Motor.STOP_ACTION_BRAKE) RIGHT_MOTOR.run_to_rel_pos( position_sp=2 * 360, # degrees speed_sp=750, # degrees per second stop_action=Motor.STOP_ACTION_BRAKE) LEFT_MOTOR.wait_while(Motor.STATE_RUNNING) RIGHT_MOTOR.wait_while(Motor.STATE_RUNNING) MEDIUM_MOTOR.run_forever(speed_sp=1000) SPEAKER.play(wav_file='/home/robot/sound/Airbrake.wav').wait()