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)
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)
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)
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
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)
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 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()
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()
speed_sp=1000, # degrees per second stop_action=Motor.STOP_ACTION_BRAKE) RIGHT_FOOT_MOTOR.run_to_rel_pos( position_sp=-2 * 360, # degrees speed_sp=1000, # 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) RIGHT_FOOT_MOTOR.run_to_rel_pos( position_sp=360, speed_sp=1000, # degrees per second stop_action=Motor.STOP_ACTION_BRAKE) RIGHT_FOOT_MOTOR.wait_while(Motor.STATE_RUNNING) else: LEDS.set_color(group=Leds.LEFT, color=Leds.GREEN, pct=1) LEDS.set_color(group=Leds.RIGHT, color=Leds.GREEN, pct=1) if time() % 3 < 1.5: LEFT_FOOT_MOTOR.run_forever(speed_sp=500 # degrees per second ) RIGHT_FOOT_MOTOR.run_forever(speed_sp=1000 # degrees per second ) else: LEFT_FOOT_MOTOR.run_forever(speed_sp=1000 # degrees per second ) RIGHT_FOOT_MOTOR.run_forever(speed_sp=500 # degrees per second )
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, time_sp=1000, stop_action=Motor.STOP_ACTION_BRAKE) CHEST_MOTOR.wait_while(Motor.STATE_RUNNING) sleep(2) MEDIUM_MOTOR.run_timed(speed_sp=-1000, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) sleep(1)
class TrueTurn: 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 turn(self, degrees, speed=150, tolerance=0.05): self.resetValue() self.stopMotors() self.tolerance = tolerance self.speed = speed multiplier = -1 if degrees > 0: multiplier = 1 self.resetValue() angle = self.GS.value() running = False self.breaker = False rightTurn = False # not actually right leftTurn = False # not actually left slowRightTurn = False # not actually right slowLeftTurn = False # not actually left if tolerance > 0: field = range(math.ceil(degrees - self.tolerance * degrees), math.ceil(degrees + self.tolerance * degrees), multiplier) advancedField = range(math.ceil(degrees - 0.1 * degrees), math.ceil(degrees + 0.1 * degrees), multiplier) print(advancedField) else: field = [self.tolerance] advancedField = range(math.ceil(degrees - 0.1 * degrees), math.ceil(degrees + 0.1 * degrees), multiplier) print(advancedField) while self.GS.value() - angle not in field: print(advancedField) print(self.GS.value() - angle) print(abs(self.GS.value() - angle)) if self.GS.value() - angle in advancedField: print("minor") print(self.GS.value()) if abs(self.GS.value() - angle) < abs( field[0] ): #we have to make them absolute because we want to turn on both sides if not slowRightTurn: print("slow right") self.M1.run_forever(speed_sp=self.speed * multiplier / 2.5) self.M2.run_forever(speed_sp=self.speed * multiplier * -1 / 2.5) slowRightTurn = True slowLeftTurn = False sleep(0.001) if abs(self.GS.value() - angle) > abs( field[len(field) - 1] ): #we have to make them absolute because we want to turn on both sides if not leftTurn: print("slow right") self.M1.run_forever(speed_sp=self.speed * multiplier * -1 / 2) self.M2.run_forever(speed_sp=self.speed * multiplier / 2) slowRightTurn = False slowLeftTurn = True sleep(0.001) else: if abs(self.GS.value() - angle) < abs( field[0] ): #we have to make them absolute because we want to turn on both sides if not rightTurn: print("normal") print(self.GS.value()) self.M1.run_forever(speed_sp=self.speed * multiplier) self.M2.run_forever(speed_sp=self.speed * multiplier * -1) rightTurn = True leftTurn = False else: sleep(0.0012) if abs(self.GS.value() - angle) > abs( field[len(field) - 1] ): #we have to make them absolute because we want to turn on both sides if not leftTurn: print(self.GS.value()) print("normal left") self.M1.run_forever(speed_sp=self.speed * multiplier * -1) self.M2.run_forever(speed_sp=self.speed * multiplier) rightTurn = False leftTurn = True else: sleep(0.0012) self.M1.stop() self.M2.stop() sleep(0.1) print("ok it works") leftTurn = False rightTurn = False slowLeftTurn = False slowRightTurn = False if self.GS.value() - angle not in field: while self.GS.value() - angle not in field: if abs(self.GS.value() - angle) < abs( field[0] ): #we have to make them absolute because we won to turn on both sides if not rightTurn: print(self.GS.value() - angle) print("micro") self.M1.run_forever(speed_sp=self.speed * multiplier / 5) self.M2.run_forever(speed_sp=self.speed * multiplier * -1 / 5) rightTurn = True leftTurn = False sleep(0.001) if abs(self.GS.value() - angle) > abs( field[len(field) - 1] ): #we have to make them absolute because we won to turn on both sides if not leftTurn: print(self.GS.value() - angle) print("working") self.M1.run_forever(speed_sp=self.speed * multiplier * -1 / 5) self.M2.run_forever(speed_sp=self.speed * multiplier / 5) rightTurn = False leftTurn = True sleep(0.001) self.M1.stop() self.M2.stop() self.resetValue() return True def straight(self, direction, speed, tolerance): self.stopMotors() self.resetValue() angle = self.GS.value() multiplier = 1 if angle < 0: multiplier = -1 self.motor_stop = False def inField(field, thing): succes = 0 j = 0 for i in field: if j == 0: if i < thing: succes = 2 break if j == len(field) - 1: if i > thing: succes = 3 break if thing == i: succes = 1 break j = j + 1 return succes field = range(angle - tolerance, angle + tolerance) while self.motor_stop == False: self.M1.run_forever(speed_sp=speed * direction) self.M2.run_forever(speed_sp=speed * direction) sleep(0.2) value = self.GS.value() if inField(field, value) == 2: print("compesating 2") self.M1.run_forever(speed_sp=speed - 50 * direction) while self.GS.value() not in field: sleep(0.02) print(self.GS.value()) self.M1.run_forever(speed_sp=speed * direction) self.M2.run_forever(speed_sp=speed * direction) elif inField(field, value) == 3: print("compesating 3") self.M2.run_forever(speed_sp=speed - 50 * direction) while self.GS.value() not in field: print(self.GS.value()) sleep(0.02) self.M2.run_forever(speed_sp=speed * direction) self.M1.run_forever(speed_sp=speed * direction) if self.motor_stop is True: self.stopMotors() def measureDistanceStart(self): self.distance = self.M1.position # ~ self.MDistanceRunning = True def measureDistance(self, wheelDiameter=5.5): turns = (self.M1.position - self.distance) / 360 dist = turns * wheelDiameter * math.pi return dist def measureDistanceRunning(self): return self.MDistanceRunning def stopMotors(self): self.motor_stop = True self.M2.stop() self.M1.stop() self.resetValue() def resetValue(self): self.GS.mode = 'GYRO-RATE' self.GS.mode = 'GYRO-ANG' def isRunning(self): return not self.motor_stop
from ev3dev.ev3 import LargeMotor, GyroSensor, UltrasonicSensor, ColorSensor, Sound from os import system system('setfont Lat15-TerminusBold14') cl_left = ColorSensor('in1') cl_right = ColorSensor('in4') l = LargeMotor('outA') r = LargeMotor('outC') gyro = GyroSensor('in2') sonic = UltrasonicSensor('in3') gradient = [] count = 0 while 1: l.run_forever(speed_sp=-300) r.run_forever(speed_sp=-300) count += 1 if(count % 25 == 0): count = 1 gradient.append((cl_left.value(), cl_right.value())) if(len(gradient) > 10): gradient.pop(0) print(gradient)
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.leds = Leds() 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 dance_if_ir_beacon_pressed(self): while self.remote_control.beacon: self.left_foot_motor.run_timed(speed_sp=randint(-1000, 1000), time_sp=1000, stop_action=Motor.STOP_ACTION_COAST) self.right_foot_motor.run_timed( speed_sp=randint(-1000, 1000), time_sp=1000, stop_action=Motor.STOP_ACTION_COAST) self.left_foot_motor.wait_while(Motor.STATE_RUNNING) self.right_foot_motor.wait_while(Motor.STATE_RUNNING) def detect_object_by_ir_sensor(self): if self.ir_sensor.proximity < 25: self.leds.set_color(group=Leds.LEFT, color=Leds.RED, pct=1) self.leds.set_color(group=Leds.RIGHT, color=Leds.RED, pct=1) self.speaker.play(wav_file='/home/robot/sound/Object.wav').wait() self.speaker.play(wav_file='/home/robot/sound/Detected.wav').wait() self.speaker.play( wav_file='/home/robot/sound/Error alarm.wav').wait() else: self.leds.all_off() 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.dance_if_ir_beacon_pressed() # DON'T use IR Sensor in 2 different modes in the same program / loop # - https://github.com/pybricks/support/issues/62 # - https://github.com/ev3dev/ev3dev/issues/1401 # self.detect_object_by_ir_sensor() self.blast_bazooka_if_touched()
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) STING_MOTOR.run_timed(speed_sp=400, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) STING_MOTOR.wait_while(Motor.STATE_RUNNING) GO_MOTOR.run_forever(speed_sp=-500) SPEAKER.play(wav_file='/home/robot/sound/Blip 2.wav').wait() while (BEACON_SEEKER.distance <= -128) or (BEACON_SEEKER.distance >= 30): pass while BEACON_SEEKER.heading <= 5: pass GO_MOTOR.run_forever(speed_sp=-200) while BEACON_SEEKER.heading >= 3: pass # FIXME: Spik3r doesn't stop at the correct angle
class Spik3r: 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() def sting_by_ir_beacon(self): while True: if self.beacon.beacon: self.sting_motor.run_to_rel_pos( speed_sp=750, position=-220, stop_action=Motor.STOP_ACTION_HOLD) self.sting_motor.wait_while(Motor.STATE_RUNNING) self.speaker.play( wav_file='/home/robot/sound/Blip 3.wav').wait() self.sting_motor.run_timed(speed_sp=-1000, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) self.sting_motor.wait_while(Motor.STATE_RUNNING) self.sting_motor.run_timed(speed_sp=1000, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) self.sting_motor.wait_while(Motor.STATE_RUNNING) while self.beacon.beacon: pass def be_noisy_to_people(self): while True: if self.color_sensor.reflected_light_intensity > 30: for i in range(4): self.speaker.play_song( (('D4', 'e3'), ('D4', 'e3'), ('D4', 'e3'), ('G4', 'h'), ('D5', 'h'), ('C5', 'e3'), ('B4', 'e3'), ('A4', 'e3'), ('G5', 'h'), ('D5', 'q'), ('C5', 'e3'), ('B4', 'e3'), ('A4', 'e3'), ('G5', 'h'), ('D5', 'q'), ('C5', 'e3'), ('B4', 'e3'), ('C5', 'e3'), ('A4', 'h.'))) def pinch_if_touched(self): while True: if self.touch_sensor.is_pressed: self.claw_motor.run_timed(speed_sp=500, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) self.claw_motor.wait_while(Motor.STATE_RUNNING) self.claw_motor.run_timed(speed_sp=-500, time_sp=0.3 * 1000, stop_action=Motor.STOP_ACTION_HOLD) self.claw_motor.wait_while(Motor.STATE_RUNNING) def keep_driving_by_ir_beacon(self): while True: if self.beacon.red_up and self.beacon.blue_up: self.go_motor.run_forever(speed_sp=910) elif self.beacon.blue_up: self.go_motor.run_forever(speed_sp=-1000) else: self.go_motor.stop(stop_action=Motor.STOP_ACTION_HOLD) def main(self): self.dis.image.paste(im=Image.open('/home/robot/image/Evil.bmp')) self.dis.update() # FIXME: ValueError: invalid literal for int() with base 10: '' or '9\n9' # when multiple threads access the same Sensor Thread(target=self.pinch_if_touched, daemon=True).start() Thread(target=self.be_noisy_to_people, daemon=True).start() Thread(target=self.sting_by_ir_beacon, daemon=True).start() self.keep_driving_by_ir_beacon()
stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) SPEAKER.play(wav_file='/home/robot/sound/Laughing 2.wav').wait() MEDIUM_MOTOR.run_timed( speed_sp=-200, # deg/s time_sp=1000, # ms stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) else: SCREEN.image.paste(im=Image.open('/home/robot/image/Crazy 1.bmp')) SCREEN.update() LEFT_MOTOR.run_forever(speed_sp=750) RIGHT_MOTOR.run_forever(speed_sp=750) MEDIUM_MOTOR.run_timed( speed_sp=750, # deg/s time_sp=0.1 * 1000, # ms stop_action=Motor.STOP_ACTION_HOLD) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) sleep(0.1) SCREEN.image.paste(im=Image.open('/home/robot/image/Crazy 2.bmp')) SCREEN.update() # LEFT_MOTOR.run_forever(speed_sp=750) RIGHT_MOTOR.stop(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) STING_MOTOR.run_timed( speed_sp=400, time_sp=1000, stop_action=Motor.STOP_ACTION_HOLD) STING_MOTOR.wait_while(Motor.STATE_RUNNING) for i in range(3): GO_MOTOR.run_forever(speed_sp=-1000) SPEAKER.play(wav_file='/home/robot/sound/Blip 2.wav').wait() while INFRARED_SENSOR.proximity >= 40: pass GO_MOTOR.run_forever(speed_sp=250) SPEAKER.play(wav_file='/home/robot/sound/Blip 4.wav').wait() STING_MOTOR.run_to_rel_pos( speed_sp=750, position_sp=-220, stop_action=Motor.STOP_ACTION_HOLD) STING_MOTOR.wait_while(Motor.STATE_RUNNING)
#!/usr/bin/env python3 from ev3dev.ev3 import( Motor, LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B ) LARGE_MOTOR = LargeMotor(address=OUTPUT_B) MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) LARGE_MOTOR.run_forever(speed_sp=1000) for i in range(3): MEDIUM_MOTOR.run_timed( speed_sp=100, time_sp=1000, stop_action=Motor.STOP_ACTION_COAST) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING) MEDIUM_MOTOR.run_timed( speed_sp=-100, time_sp=1000, stop_action=Motor.STOP_ACTION_COAST) MEDIUM_MOTOR.wait_while(Motor.STATE_RUNNING)