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)
