class R3ptar(EV3Brick): def __init__(self, turn_motor_port: Port = Port.A, move_motor_port: Port = Port.B, scare_motor_port: Port = Port.D, touch_sensor_port: Port = Port.S1, color_sensor_port: Port = Port.S3, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.turn_motor = Motor(port=turn_motor_port, positive_direction=Direction.CLOCKWISE) self.move_motor = Motor(port=move_motor_port, positive_direction=Direction.CLOCKWISE) self.scare_motor = Motor(port=scare_motor_port, positive_direction=Direction.CLOCKWISE) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.color_sensor = ColorSensor(port=color_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def drive_once_by_ir_beacon(self, speed: float = 1000): ir_beacons_pressed = set( self.ir_sensor.buttons(channel=self.ir_beacon_channel)) if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}: self.move_motor.run(speed=speed) elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}: self.move_motor.run(speed=-speed) elif ir_beacons_pressed == {Button.LEFT_UP}: self.turn_motor.run(speed=-500) self.move_motor.run(speed=speed) elif ir_beacons_pressed == {Button.RIGHT_UP}: self.turn_motor.run(speed=500) self.move_motor.run(speed=speed) elif ir_beacons_pressed == {Button.LEFT_DOWN}: self.turn_motor.run(speed=-500) self.move_motor.run(speed=-speed) elif ir_beacons_pressed == {Button.RIGHT_DOWN}: self.turn_motor.run(speed=500) self.move_motor.run(speed=-speed) else: self.turn_motor.hold() self.move_motor.stop() def bite_by_ir_beacon(self, speed: float = 1000): if Button.BEACON in self.ir_sensor.buttons( channel=self.ir_beacon_channel): self.speaker.play_file(file=SoundFile.SNAKE_HISS) self.scare_motor.run_time(speed=speed, time=1000, then=Stop.HOLD, wait=True) self.scare_motor.run_time(speed=-300, time=1000, then=Stop.COAST, wait=True) while Button.BEACON in self.ir_sensor.buttons( channel=self.ir_beacon_channel): pass def run_away_if_chased(self): if self.color_sensor.reflection() > 30: self.move_motor.run_time(speed=500, time=4000, then=Stop.HOLD, wait=True) for i in range(2): self.turn_motor.run_time(speed=500, time=1000, then=Stop.HOLD, wait=True) self.turn_motor.run_time(speed=-500, time=1000, then=Stop.HOLD, wait=True) def bite_if_touched(self): if self.touch_sensor.pressed(): self.speaker.play_file(file=SoundFile.SNAKE_HISS) self.scare_motor.run_time(speed=1000, time=1000, then=Stop.HOLD, wait=True) self.scare_motor.run_time(speed=-300, time=1000, then=Stop.COAST, wait=True) def main(self, speed: float = 1000): while True: self.drive_once_by_ir_beacon(speed=speed) self.bite_by_ir_beacon(speed=speed) self.bite_if_touched() self.run_away_if_chased()
class MbMotor(): """ Control a motor, besides the fact that you can detect if a motor got stalled the main reason for this class is to solve a bug for pybricks.ev3devices.Motor. The bug is that when you set the motor to move in a Direction.COUNTERCLOCKWISE sometimes it failes to detect it. This class is made on top of pybricks.ev3devices.Motor Args: port (Port): The port of the device clockwise_direction (bool): Sets the defualt movement of the motor clockwise or counterclockwise, True for clockwise else counterclockwise exit_exec (Function): Function that returns True or False, the motor will stop if returns True """ def __init__(self, port, clockwise_direction=True, exit_exec=lambda: False): self.core = Motor(port) self.port = port self.direction = 1 if clockwise_direction else -1 self.exit_exec = exit_exec def angle(self): """ Get the distance the robot has moved in degrees Returns: angle (int): The distance the robot has moved in degrees """ return self.core.angle() * self.direction def speed(self): """ Get the speed of the motor Returns: speed (int): The current speed of the motor """ return self.core.speed() * self.direction def stalled(self, min_speed=0): if abs(self.speed()) <= abs(min_speed): return True return False def run_angle(self, speed, angle, wait=True, detect_stall=False): """ Run the motor to a specific angle Args: speed (int): The speed of the motor angle (int): Degrees to run the motor at wait (bool): Sets if the robot is going to stop for the motor to complete this method or not """ def exec(self, speed, angle): moved_enough = False self.reset_angle() self.run(speed) while True: if abs(self.angle()) > 50: moved_enough = True if moved_enough and detect_stall: if self.stalled(): break if abs(self.angle()) > abs(angle) or self.exit_exec(): break self.hold() if wait: exec(self, speed, angle) else: threading.Thread(target=exec, args=[self, speed, angle]).start() def run_time(self, speed, msec, wait=True): """ Run the motor to a amount of time Args: speed (int): The speed of the motor msec (int): Time to move the robot wait (bool): Sets if the robot is going to stop for the motor to complete this method or not """ def exec(self, speed, msec): self.reset_angle() self.run(speed) s = time() while True: if round(time() - s, 2) * 1000 >= abs(msec) or self.exit_exec(): break self.hold() if wait: exec(self, speed, msec) else: threading.Thread(target=exec, args=[self, speed, msec]).start() def run(self, speed): """ Run the motor to a constant speed Args: speed (int): Speed to run at Note: speed parameter should be between -800 and 800 """ self.core.run(speed * self.direction) def dc(self, speed): """ Run the motor to a constant speed Args: speed (int): Speed to run at Note: speed parameter should be between -100 and 100 """ self.core.dc(speed * self.direction) def hold(self): """ Stop the motor and hold its position """ self.core.hold() def brake(self): """ Passively stop the motor """ self.core.brake() def stop(self): """ No current is being aplied to the robot, so its gonna stop due to friction """ self.core.stop() def reset_angle(self, angle=0): """ Set the motor angle Args: angle (int): Angle to set the motor at """ self.core.reset_angle(angle) def is_stalled(self, min_speed=0): """ Check if the motor got stalled Args: min_speed (int): The minim speed the motor should be moving at """ if abs(self.speed()) <= abs(min_speed): return True return False def __repr__(self): return "Motor Properties:\nPort: " + str( self.port) + "\nDefault Direction: " + str(self.direction)
class Spik3r(EV3Brick): def __init__(self, sting_motor_port: Port = Port.D, go_motor_port: Port = Port.B, claw_motor_port: Port = Port.A, touch_sensor_port: Port = Port.S1, color_sensor_port: Port = Port.S3, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.sting_motor = Motor(port=sting_motor_port, positive_direction=Direction.CLOCKWISE) self.go_motor = Motor(port=go_motor_port, positive_direction=Direction.CLOCKWISE) self.claw_motor = Motor(port=claw_motor_port, positive_direction=Direction.CLOCKWISE) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel self.touch_sensor = TouchSensor(port=touch_sensor_port) self.color_sensor = ColorSensor(port=color_sensor_port) def sting_by_ir_beacon(self): while True: ir_buttons_pressed = set( self.ir_sensor.buttons(channel=self.ir_beacon_channel)) if ir_buttons_pressed == {Button.BEACON}: self.sting_motor.run_angle(speed=-750, rotation_angle=220, then=Stop.HOLD, wait=False) self.speaker.play_file(file=SoundFile.EV3) self.sting_motor.run_time(speed=-1000, time=1000, then=Stop.HOLD, wait=True) self.sting_motor.run_time(speed=1000, time=1000, then=Stop.HOLD, wait=True) while Button.BEACON in self.ir_sensor.buttons( channel=self.ir_beacon_channel): pass def be_noisy_to_people(self): while True: if self.color_sensor.reflection() > 30: for i in range(4): self.speaker.play_file(file=SoundFile.ERROR_ALARM) def pinch_if_touched(self): while True: if self.touch_sensor.pressed(): self.claw_motor.run_time(speed=500, time=1000, then=Stop.HOLD, wait=True) self.claw_motor.run_time(speed=-500, time=0.3 * 1000, then=Stop.HOLD, wait=True) def keep_driving_by_ir_beacon(self): while True: ir_buttons_pressed = set( self.ir_sensor.buttons(channel=self.ir_beacon_channel)) if ir_buttons_pressed == {Button.RIGHT_UP, Button.LEFT_UP}: self.go_motor.run(speed=910) elif ir_buttons_pressed == {Button.RIGHT_UP}: self.go_motor.run(speed=-1000) else: self.go_motor.stop() def main(self): self.screen.load_image(ImageFile.EVIL) # FIXME: OSError: [Errno 5] EIO: # Unexpected hardware input/output error with a motor or sensor: # --> Try unplugging the sensor or motor and plug it back in again. # --> To see which sensor or motor is causing the problem, # check the line in your script that matches # the line number given in the 'Traceback' above. # --> Try rebooting the hub/brick if the problem persists. Process(target=self.be_noisy_to_people).start() Process(target=self.sting_by_ir_beacon).start() Process(target=self.pinch_if_touched).start() self.keep_driving_by_ir_beacon()
from pybricks.parameters import Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align from pybricks.tools import wait, StopWatch, DataLog from pybricks.robotics import DriveBase import time # 모터 선언 motorB = Motor(Port.B) motorC = Motor(Port.C) # 컬러 센서 선언 color_sensor = ColorSensor(Port.S1) # N번째 검은 선에서 멈춘다. N = 3 # 반복문 for count in range(N): motorB.run(300) motorC.run(300) # 반사값이 15보다 작으면 while 종료 while True: if color_sensor.reflection() < 15: break brick.sound.beep() # 모터 정지 motorB.stop(Stop.BRAKE) motorC.stop(Stop.BRAKE)
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor) from pybricks.parameters import (Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align) from pybricks.tools import print, wait, StopWatch from pybricks.robotics import DriveBase left_motor = Motor(Port.B) right_motor = Motor(Port.C) touch_sensor = TouchSensor(Port.S1) while (not touch_sensor.pressed()): left_motor.run(360) right_motor.run(360) wait(10) while (touch_sensor.pressed()): wait(10) left_motor.stop(Stop.BRAKE) right_motor.stop(Stop.BRAKE) wait(1000)
from pybricks.tools import wait, StopWatch, DataLog from pybricks.robotics import DriveBase from pybricks.media.ev3dev import SoundFile, ImageFile esquerda = Motor(Port.B, Direction.COUNTERCLOCKWISE) direita = Motor(Port.C, Direction.COUNTERCLOCKWISE) MotorMA = Motor(Port.A) MotorMD = Motor(Port.D) # EsqCor = ColorSensor(Port.S1) # DirCor = ColorSensor(Port.S4) gabriel = DriveBase(esquerda, direita, wheel_diameter=100, axle_track=166.2) #Ajustar valores ev3 = EV3Brick() while True: if Button.LEFT in ev3.buttons.pressed(): MotorMA.run(-400) if Button.RIGHT in ev3.buttons.pressed(): MotorMA.run(400) if Button.DOWN in ev3.buttons.pressed(): MotorMD.run(-400) if Button.UP in ev3.buttons.pressed(): MotorMD.run(400) if ev3.buttons.pressed() == []: MotorMD.stop() MotorMA.stop() if Button.CENTER in ev3.buttons.pressed(): MotorMD.reset_angle(0) MotorMA.reset_angle(0)
class Dinor3x(EV3Brick): """ Challenges: - Can you make DINOR3X remote controlled with the IR-Beacon? - Can you attach a colorsensor to DINOR3X, and make it behave differently depending on which color is in front of the sensor (red = walk fast, white = walk slow, etc.)? """ def __init__( self, left_motor_port: Port = Port.B, right_motor_port: Port = Port.C, jaw_motor_port: Port = Port.A, touch_sensor_port: Port = Port.S1, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.left_motor = Motor(port=left_motor_port, positive_direction=Direction.CLOCKWISE) self.right_motor = Motor(port=right_motor_port, positive_direction=Direction.CLOCKWISE) self.jaw_motor = Motor(port=jaw_motor_port, positive_direction=Direction.CLOCKWISE) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def calibrate_legs(self): self.left_motor.run(speed=100) self.right_motor.run(speed=200) while self.touch_sensor.pressed(): pass self.left_motor.hold() self.right_motor.hold() self.left_motor.run(speed=400) while not self.touch_sensor.pressed(): pass self.left_motor.hold() self.left_motor.run_angle( rotation_angle=-0.2 * 360, speed=500, then=Stop.HOLD, wait=True) self.right_motor.run(speed=400) while not self.touch_sensor.pressed(): pass self.right_motor.hold() self.right_motor.run_angle( rotation_angle=-0.2 * 360, speed=500, then=Stop.HOLD, wait=True) self.left_motor.reset_angle(angle=0) self.right_motor.reset_angle(angle=0) def roar(self): self.speaker.play_file(file=SoundFile.T_REX_ROAR) self.jaw_motor.run_angle( speed=400, rotation_angle=-60, then=Stop.HOLD, wait=True) # FIXME: jaw doesn't close for i in range(12): self.jaw_motor.run_time( speed=-400, time=0.05 * 1000, then=Stop.HOLD, wait=True) self.jaw_motor.run_time( speed=400, time=0.05 * 1000, then=Stop.HOLD, wait=True) self.jaw_motor.run(speed=200) sleep(0.5) def close_mouth(self): self.jaw_motor.run(speed=200) sleep(1) self.jaw_motor.stop() def walk_until_blocked(self): self.left_motor.run(speed=-400) self.right_motor.run(speed=-400) while self.ir_sensor.distance() >= 25: pass self.left_motor.stop() self.right_motor.stop() def run_away(self): self.left_motor.run_angle( speed=750, rotation_angle=3 * 360, then=Stop.BRAKE, wait=False) self.right_motor.run_angle( speed=750, rotation_angle=3 * 360, then=Stop.BRAKE, wait=True) def jump(self): """ Dinor3x Mission 02 Challenge: make it jump """ ... # TRANSLATED FROM EV3-G MY BLOCKS # ------------------------------- def leg_adjust( self, cyclic_degrees: float, speed: float = 1000, leg_offset_percent: float = 0, mirrored_adjust: bool = False, brake: bool = True): ... def leg_to_pos( self, speed: float = 1000, left_position: float = 0, right_position: float = 0): self.left_motor.brake() self.right_motor.brake() self.left_motor.run_angle( speed=speed, rotation_angle=left_position - cyclic_position_offset( rotation_sensor=self.left_motor.angle(), cyclic_degrees=360), then=Stop.BRAKE, wait=True) self.right_motor.run_angle( speed=speed, rotation_angle=right_position - cyclic_position_offset( rotation_sensor=self.right_motor.angle(), cyclic_degrees=360), then=Stop.BRAKE, wait=True) def turn(self, speed: float = 1000, n_steps: int = 1): ... def walk(self, speed: float = 1000): ... def walk_steps(self, speed: float = 1000, n_steps: int = 1): ...
class Puppy: # Constants for leg angle HALF_UP_ANGLE = 25 STAND_UP_ANGLE = 65 STRETCH_ANGLE = 125 # Loads the different eyes HEART_EYES = Image(ImageFile.LOVE) SQUINTY_EYES = Image(ImageFile.TEAR) def __init__(self): # Sets up the brick self.ev3 = EV3Brick() # Identifies which motor is connected to which ports self.left_leg_motor = Motor(Port.D, Direction.COUNTERCLOCKWISE) self.right_leg_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) # Sets up the gear ratio (automatically translates it to the correct angle) self.head_motor = Motor(Port.C, Direction.COUNTERCLOCKWISE, gears=[[1, 24], [12, 36]]) # Sets up touch sensor self.touch_sensor = TouchSensor(Port.S1) # Sets up constants for the eye self.eyes_timer_1 = StopWatch() self.eyes_timer_1_end = 0 self.eyes_timer_2 = StopWatch() self.eyes_timer_2_end = 0 self.eyes_closed = False def movements(self): self.ev3.screen.load_image(ImageFile.EV3_ICON) self.ev3.light.on(Color.ORANGE) dog_pat = 0 # Movement interactions while True: buttons = self.ev3.buttons.pressed() if Button.CENTER in buttons: break elif Button.UP in buttons: self.head_motor.run(20) elif Button.DOWN in buttons: self.head_motor.run(-20) elif self.touch_sensor.pressed(): self.ev3.speaker.play_file(SoundFile.DOG_BARK_1) self.eyes = self.HEART_EYES self.sit_down() dog_pat += 1 print(dog_pat) elif dog_pat == 3: self.go_to_bathroom() dog_pat -= 3 print(dog_pat) else: self.head_motor.stop() wait(100) self.head_motor.stop() self.head_motor.reset_angle(0) self.ev3.light.on(Color.GREEN) # Below this line I honestly have no clue how it works. It came from the boiler plate of functions def move_head(self, target): self.head_motor.run_target(20, target) @property def eyes(self): return self._eyes @eyes.setter def eyes(self, value): if value != self._eyes: self._eyes = value self.ev3.screen.load_image(value) def update_eyes(self): if self.eyes_timer_1.time() > self.eyes_timer_1_end: self.eyes_timer_1.reset() if self.eyes == self.SLEEPING_EYES: self.eyes_timer_1_end = urandom.randint(1, 5) * 1000 self.eyes = self.TIRED_RIGHT_EYES else: self.eyes_timer_1_end = 250 self.eyes = self.SLEEPING_EYES if self.eyes_timer_2.time() > self.eyes_timer_2_end: self.eyes_timer_2.reset() if self.eyes != self.SLEEPING_EYES: self.eyes_timer_2_end = urandom.randint(1, 10) * 1000 if self.eyes != self.TIRED_LEFT_EYES: self.eyes = self.TIRED_LEFT_EYES else: self.eyes = self.TIRED_RIGHT_EYES def stand_up(self): self.left_leg_motor.run_target(100, self.HALF_UP_ANGLE, wait=False) self.right_leg_motor.run_target(100, self.HALF_UP_ANGLE) while not self.left_leg_motor.control.done(): wait(100) self.left_leg_motor.run_target(50, self.STAND_UP_ANGLE, wait=False) self.right_leg_motor.run_target(50, self.STAND_UP_ANGLE) while not self.left_leg_motor.control.done(): wait(100) wait(500) def sit_down(self): self.left_leg_motor.run(-50) self.right_leg_motor.run(-50) wait(1000) self.left_leg_motor.stop() self.right_leg_motor.stop() wait(100) def go_to_bathroom(self): self.eyes = self.SQUINTY_EYES self.stand_up() wait(100) self.right_leg_motor.run_target(100, self.STRETCH_ANGLE) wait(800) self.ev3.speaker.play_file(SoundFile.HORN_1) wait(1000) for _ in range(3): self.right_leg_motor.run_angle(100, 20) self.right_leg_motor.run_angle(100, -20) self.right_leg_motor.run_target(100, self.STAND_UP_ANGLE) def run(self): self.movements() self.eyes = self.SLEEPING_EYES
brick.light(Color.GREEN) # Brick LED becomes green # Run the motors up to 'motorSpeed' degrees per second: motorL.run(motorSpeed) motorR.run(motorSpeed) while started is True: # Check if there is enough space; if not, stop, turn 90 degrees, then start running again if ultrasonic.distance() < minDistance: # Stop the motors: tempSpeed = motorSpeed for i in range(1, 100): tempSpeed = tempSpeed - 1 motorL.run(tempSpeed) motorR.run(tempSpeed) #wait(100) motorL.stop() motorR.stop() # Turn 90 degrees until there is enough space to start running again: death = 4 while ultrasonic.distance() < minDistance: #motorL.set_run_settings(maxSpeed, 400) #motorR.set_run_settings(maxSpeed, 400) motorL.run_angle(steeringSpeed, -174, Stop.COAST, False) motorR.run_angle(steeringSpeed, +174) death -= 1 if (death == 0): started = False break # Run the motors up to 'motorSpeed' degrees per second: if death != 0:
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor) from pybricks.parameters import (Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align) from pybricks.tools import print, wait, StopWatch from pybricks.robotics import DriveBase # Write your program here brick.sound.beep() brick.light(Color.RED) motorR = Motor(Port.B) motorL = Motor(Port.C) ultraSense = UltrasonicSensor(Port.S3) dist = 69 sped = 69 motorL.run(sped) motorR.run(sped) while dist < ultraSense.distance(): brick.sound.beep(69) motorR.stop(Stop.BRAKE) motorL.stop(Stop.BRAKE)
class R3ptar: """ R3ptar can be driven around by the IR Remote Control, strikes when the Beacon button is pressed, and hisses when the Touch Sensor is pressed (inspiration from LEGO Mindstorms EV3 Home Edition: R3ptar: Tutorial #4) """ def __init__(self, steering_motor_port: Port = Port.A, driving_motor_port: Port = Port.B, striking_motor_port: Port = Port.D, touch_sensor_port: Port = Port.S1, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.ev3_brick = EV3Brick() self.steering_motor = Motor(port=steering_motor_port, positive_direction=Direction.CLOCKWISE) self.driving_motor = Motor(port=driving_motor_port, positive_direction=Direction.CLOCKWISE) self.striking_motor = Motor(port=striking_motor_port, positive_direction=Direction.CLOCKWISE) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def drive_by_ir_beacon( self, speed: float = 1000, # mm/s ): ir_beacons_pressed = \ set(self.ir_sensor.buttons(channel=self.ir_beacon_channel)) if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}: self.driving_motor.run(speed=speed) elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}: self.driving_motor.run(speed=-speed) elif ir_beacons_pressed == {Button.LEFT_UP}: self.steering_motor.run(speed=-500) self.driving_motor.run(speed=speed) elif ir_beacons_pressed == {Button.RIGHT_UP}: self.steering_motor.run(speed=500) self.driving_motor.run(speed=speed) elif ir_beacons_pressed == {Button.LEFT_DOWN}: self.steering_motor.run(speed=-500) self.driving_motor.run(speed=-speed) elif ir_beacons_pressed == {Button.RIGHT_DOWN}: self.steering_motor.run(speed=500) self.driving_motor.run(speed=-speed) else: self.steering_motor.hold() self.driving_motor.stop() def strike_by_ir_beacon(self, speed: float = 1000): if Button.BEACON in \ self.ir_sensor.buttons(channel=self.ir_beacon_channel): self.striking_motor.run_time(speed=speed, time=1000, then=Stop.COAST, wait=True) self.striking_motor.run_time(speed=-speed, time=1000, then=Stop.COAST, wait=True) while Button.BEACON in \ self.ir_sensor.buttons(channel=self.ir_beacon_channel): pass def hiss_if_touched(self): if self.touch_sensor.pressed(): self.ev3_brick.speaker.play_file(file=SoundFile.SNAKE_HISS)
robot.drive(800, 0) move = 1 # Keep driving until the random time has passed or an object is # detected. If an object is detected the "checking" variable # will be set to "False." while checking and timer.time() < random_time: checking = ultrasonic_sensor.distance() > 400 wait(10) # Stop driving. robot.drive(0, 0) # Check if the object is closer than 250 mm. if ultrasonic_sensor.distance() < 250: # Roar and move the head forward to bite. head_motor.dc(-100) ev3.speaker.play_file(SoundFile.T_REX_ROAR) wait(250) head_motor.stop() wait(1000) else: # Move the head and hiss. head_motor.dc(-100) wait(100) head_motor.stop() ev3.speaker.play_file(SoundFile.SNAKE_HISS) # Reset the head motor to its initial position. head_motor.run_target(1200, 0)
ev3.speaker.beep() US = UltrasonicSensor(Port.S1) LeftMotor = Motor(Port.A, positive_direction=Direction.CLOCKWISE, gears=None) RightMotor = Motor(Port.B, positive_direction=Direction.CLOCKWISE, gears=None) SpinMotor = Motor(Port.C, positive_direction=Direction.CLOCKWISE, gears=None) drive = DriveBase(LeftMotor, RightMotor, 55, 203) var = 1 escape_dist = 800 escape_ang = 360 while var > 0: if US.distance() < 500: drive.stop() while US.distance() < escape_dist: drive.drive(100, 180) wait(10) # ev3.screen.print(US.distance()) # print(US.distance(),',',drive.angle()) SpinMotor.run(1080) ev3.speaker.beep() # if drive.angle() > escape_ang: # escape_dist = escape_dist - 200 # escape_ang = escape_ang + 360 # ev3.speaker.say('adjusting parameters') SpinMotor.stop() # escape_dist = 800 # reset_angle # ev3.speaker.play_notes('C5/4_','C6/4') drive.drive(500, 0) print(US.distance())
class Bri6Pack: def __init__(self): # Initialize the EV3 Brick. self.ev3 = EV3Brick() self.ev3.speaker.set_speech_options(voice='f3') # large motors self.left_motor = Motor(Port.A, positive_direction=Direction.COUNTERCLOCKWISE, gears=None) self.right_motor = Motor(Port.B, positive_direction=Direction.COUNTERCLOCKWISE, gears=None) # small (medium) motors self.small_motor_left = Motor(Port.C, positive_direction=Direction.CLOCKWISE, gears=None) self.small_motor_right = Motor(Port.D, positive_direction=Direction.CLOCKWISE, gears=None) # gyro sensor self.gyro_sensor = GyroSensor(Port.S3, Direction.CLOCKWISE) # Initialize the drive base. self.drive_base = DriveBase(self.left_motor, self.right_motor, wheel_diameter=93.5, axle_track=120) return """ A proportional–integral–derivative controller """ def drive_pid(self, speed, distance): isDebug = False # the sign of the input speed is ignored speed = abs(speed) # direction is indicated by the sign of the input distance # postive distance ==> move forward # negative distance ==> move backward if (distance < 0): speed = -speed distance = -distance drive_base = self.drive_base gyro_sensor = self.gyro_sensor # resets the estimated driven distance and angle to 0 drive_base.reset() # reset gyro sensor # a = gyro_sensor.speed() # b = gyro_sensor.angle() gyro_sensor.reset_angle(0) err_p = 0.0 err_i = 0.0 err_d = 0.0 err_prev = 0.0 i = 0 # do not use factor useSpeedFactor = False factor = 1.0 if useSpeedFactor: factor = abs(speed / 300.0) deltaT = 0.02 total_distance = abs(distance) gyro_PID_distance = total_distance * 0.8 # use gyro_PID for the first half while abs(drive_base.distance()) <= gyro_PID_distance: err_p = gyro_sensor.angle() err_i += err_p * deltaT err_d = (err_p - err_prev) / deltaT kp = 1 * factor ki = 4 * factor kd = 0.01 * factor # limit the speed err_i can grow (exponential decay) if abs(err_i) >= 100: err_i *= math.exp(-abs(err_i / 100)) turn_rate = -(kp * err_p + ki * err_i + kd * err_d) drive_base.drive(speed, turn_rate) time.sleep(0.05) # debugging output if (i % 1 == 0 and isDebug): print( "Count={}, Distance={}, P={}, I={}, D={}, CP={}, CI={}, CD={}, turn rate={}" .format(i, drive_base.distance(), err_p, err_i, err_d, kp * err_p, ki * err_i, kd * err_d, turn_rate)) i += 1 err_prev = err_p # finish the distance with motor_PID motor_PID_distance = total_distance - abs(drive_base.distance()) drive_base.stop() drive_base.settings(straight_speed=speed) if speed < 0: motor_PID_distance *= -1 drive_base.straight(motor_PID_distance) if isDebug: print("Final: Distance={} gyroAngle={}, speed={}, mDist={}".format( drive_base.distance(), gyro_sensor.angle(), speed, motor_PID_distance)) ## stall_tolerances(speed, time) ## If the controller cannot reach this "speed" ## for some "time" even with maximum "actuation", it is stalled. # robot.distance_control.stall_tolerances(speed, 100) # robot.distance_control.limits(actuation = 40) def check_stall(self): if self.drive_base.distance_control.stalled(): print("STALLED") self.drive_base.stop() return True def reset_motors(self): self.drive_base.stop() self.small_motor_left.stop() self.small_motor_right.stop() def count_down(self, sec): for i in range(sec, 0, -1): #print(i) if i == 5: # this is not accurate as speaking takes time self.ev3.speaker.say("5 seconds left") else: time.sleep(1) def say(self, words): self.ev3.speaker.say(words)
""" return inches*25.4 # Initialize our drive motors and a drive base # For the arm motor, we want positive values to be up and negative to be down. # The way that we're configured, we have to reverse the direction passed to # the motor initialization to achieve this. arm_motor = Motor(ARM_MOTOR_PORT, Direction.COUNTERCLOCKWISE) left_motor = Motor(LEFT_MOTOR_PORT) right_motor = Motor(RIGHT_MOTOR_PORT) # Let's initialize our arm motor by putting it all the way down until it # stalls. With our build, it will stall when it hits the supporting arms # for the color sensor and touch sensor (so it doesn't hit the ground - it's # still slightly in the air.) arm_motor.stop() arm_motor.run_until_stalled(-180) arm_motor.stop() # Initialize our drive base and drive 24.0 inches at 5 inches/sec. robot = MyDriveBase(left_motor=left_motor, right_motor=right_motor, wheel_diameter=WHEEL_DIAMETER_MM, axle_track=AXLE_TRACK_MM) robot.drive_distance(inches=24.0, speed=5.0, steering=0) # Raise the arm until we stall, which will hopefully raise the lever # at the base of the crane. arm_motor.stop() arm_motor.run_until_stalled(90) arm_motor.stop() # Sleep for a little bit while the arm is raised so that the block
reset_all() while True: PID(-700,0,1,1,1) stop1() robot.stop() elif Button.DOWN in brick.buttons(): while not Button.CENTER in brick.buttons(): first_gyro.reset_angle(0) second_gyro.reset_angle(0) small_left_m = Motor(Port.A) i=0 while i!= 1500: small_left_m.dc(40) i+=1 stop1() small_left_m.stop() time.sleep(0.5) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=740: PID(150,0,8,1,1) stop1() robot.stop() time.sleep(0.1) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=720: followline(100,0.5,1,1) stop1() robot.stop() time.sleep(0.5)
#!/usr/bin/env pybricks-micropython from pybricks.hubs import EV3Brick from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor) from pybricks.parameters import (Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align) from pybricks.tools import print, wait, StopWatch from pybricks.robotics import DriveBase ev3 = EV3Brick() ultrasonicSensor = UltrasonicSensor(Port.S2) left_motor = Motor(Port.A) right_motor = Motor(Port.D) program_running = True distance = ultrasonicSensor.distance() left_motor.run(800) right_motor.run(800) while distance > 195: distance = ultrasonicSensor.distance() print(distance) left_motor.stop() right_motor.stop()
def stop2(): if Button.CENTER in brick.buttons(): left_M.stop() right_M.stop() '''small_left_m.stop()''' '''small_right_m.stop() ''' while True: if first_cls.color() == Color.RED: while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=1200: PID(150,0,5,1,1) stop1() robot.stop() reset_all() time.sleep(1) while motor_avarge()>=-270: PID(-400,0,1,1,1) stop1() robot.stop() reset_all() while True: PID(-500,-210,1,1,1) stop1() elif Button.LEFT in brick.buttons(): while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=970: PID(200,0,8,1,1) stop1() robot.stop() reset_all() while motor_avarge()>=-450: PID(-200,0,1,1,1) stop1() robot.stop() reset_all() while True: PID(-500,-200,1,1,1) stop1() elif Button.UP in brick.buttons(): while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=780: PID(150,0,8,1,1) stop1() robot.stop() left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=470: followline(100,1.2,1,1) stop1() robot.stop() time.sleep(0.1) first_gyro.reset_angle(0) second_gyro.reset_angle(0) while first_gyro.angle()<=25: robot.drive(10,-100) stop1() robot.stop() reset_all() while motor_avarge()<=1000: PID(160,0,8,1,1) stop1() robot.stop() time.sleep(0.2) left_M.reset_angle(0) right_M.reset_angle(0) if second_cls.reflection()>=65: while motor_avarge()<=500: followline(100,0.6,1,1) stop1() robot.stop() elif second_cls.reflection()<=10: while motor_avarge()<=460: followline(100,0.6,1,1) stop1() robot.stop() else: while motor_avarge()<=420: PID(100,-4,1,1,1) stop1() robot.stop() time.sleep(0.3) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()>=-1500: PID(-300,0,1,1,1) stop1() robot.stop() while first_gyro.angle()>=-17: robot.drive(10,100) stop1() robot.stop() reset_all() while True: PID(-700,0,1,1,1) stop1() robot.stop() elif Button.DOWN in brick.buttons(): while not Button.CENTER in brick.buttons(): first_gyro.reset_angle(0) second_gyro.reset_angle(0) small_left_m = Motor(Port.A) i=0 while i!= 1500: small_left_m.dc(40) i+=1 stop1() small_left_m.stop() time.sleep(0.5) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=740: PID(150,0,8,1,1) stop1() robot.stop() time.sleep(0.1) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=720: followline(100,0.5,1,1) stop1() robot.stop() time.sleep(0.5) i=0 while i!= 2000: small_left_m.dc(-40) i+=1 stop1() small_left_m.stop() reset_all() while motor_avarge()<=1000: PID(150,0,1,1,1) stop1() robot.stop() time.sleep(1) reset_all() while True: PID(-500,0,1,1,1) stop1() elif Button.RIGHT in brick.buttons(): while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=740: PID(150,0,1,1,1) stop1() robot.stop() time.sleep(0.1) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=870: followline(150,0.6,1,1) stop1() robot.stop() reset_all() while motor_avarge()<=400: PID(150,0,1,1,1) stop1() robot.stop() reset_all() while first_gyro.angle()<=100: robot.drive(10,-100) stop1() robot.stop() i=0 while i<=110: robot.drive(-100,0) i+=1 stop1() robot.stop() reset_all() while motor_avarge()<=270: PID(150,0,1,1,1) stop1() robot.stop() reset_all() while first_gyro.angle()<=27: robot.drive(10,-100) stop1() robot.stop() reset_all() while motor_avarge()<=600: followline(100,1,1,1) stop1() robot.stop() reset_all() while first_gyro.angle()<=170: robot.drive(10,-100) stop1() robot.stop() while motor_avarge()>=-1230: robot.drive(-1000,0) stop1() robot.stop() reset_all() while True: if motor_avarge()>0: while motor_avarge()>0: robot.drive(-10,0) stop1() robot.stop() elif motor_avarge()<0: while motor_avarge()<0: robot.drive(-10,0) stop1() robot.stop() while motor_avarge()>=-1300: robot.drive(-2000,0) stop1() robot.stop() reset_all()
class Kraz33Hors3: def __init__(self, back_foot_motor_port: Port = Port.C, front_foot_motor_port: Port = Port.B, gear_motor_port: Port = Port.A, touch_sensor_port: Port = Port.S1, color_sensor_port: Port = Port.S3, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.front_foot_motor = Motor(port=front_foot_motor_port, positive_direction=Direction.CLOCKWISE) self.back_foot_motor = Motor( port=back_foot_motor_port, positive_direction=Direction.COUNTERCLOCKWISE) self.gear_motor = Motor(port=gear_motor_port) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.color_sensor = ColorSensor(port=color_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def drive_once_by_ir_beacon( self, speed: float = 1000 # deg/s ): ir_beacons_pressed = set( self.ir_sensor.buttons(channel=self.ir_beacon_channel)) # forward if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}: self.front_foot_motor.run_time( speed=speed, time=1000, # ms then=Stop.COAST, wait=False) self.back_foot_motor.run_time( speed=speed, time=1000, # ms then=Stop.COAST, wait=True) # backward elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}: self.front_foot_motor.run_time( speed=-speed, time=1000, # ms then=Stop.COAST, wait=False) self.back_foot_motor.run_time( speed=-speed, time=1000, # ms then=Stop.COAST, wait=True) # move crazily elif ir_beacons_pressed == {Button.BEACON}: self.gear_motor.run(speed=speed) self.front_foot_motor.run_time( speed=speed / 3, time=1000, # ms then=Stop.COAST, wait=False) self.back_foot_motor.run_time( speed=-speed / 3, time=1000, # ms then=Stop.COAST, wait=True) else: self.gear_motor.stop() def keep_driving_by_ir_beacon( self, speed: float = 1000 # deg/s ): while True: self.drive_once_by_ir_beacon(speed=speed) def back_whenever_touched( self, speed: float = 1000 # deg/s ): while True: if self.touch_sensor.pressed(): self.front_foot_motor.run_time( speed=-speed, time=1000, # ms then=Stop.COAST, wait=False) self.back_foot_motor.run_time( speed=-speed, time=1000, # ms then=Stop.COAST, wait=True) def main( self, speed: float = 1000 # deg/s ): Thread(target=self.back_whenever_touched).start() self.keep_driving_by_ir_beacon(speed=speed)
waitBC() # make sure motors are not moving while oc < 4: oc += 1 cycles = 0 while (cycles < 2): cycles += 1 print("Cycles: %d,%d" % (oc,cycles)) t1 = watch.time() / 1000 robot.drive_time(100, 45, 8000) t2 = watch.time() / 1000 driveTime = t2-t1 print("CW drive time: %5.3f" % (driveTime)) DTFlag = True sleep(1) # just in case not quite done mB.stop(Stop.HOLD) mC.stop(Stop.HOLD) waitBC() while (cycles < 4): cycles += 1 print("Cycles: %d,%d" % (oc,cycles)) t1 = watch.time() / 1000 robot.drive_time(100, -45, 8000) t2 = watch.time() / 1000 driveTime = t2-t1 print("CCW drive time: %5.3f" % (driveTime)) DTFlag = True sleep(1) # just in case not quite done mB.stop(Stop.HOLD) mC.stop(Stop.HOLD)
robot.settings(250, 300, 100, 600) StopAtWhiteLine() robot.straight(175) #robot turns toward the wall and reveres: robot.turn(-120) robot.straight(-180) #goes off the wall and turns to the wheel robot.straight(200) robot.turn(-65) robot.stop() #Goes foreward to flip the wheel medium_left.run_time(-200, 5000) robot.straight(80) medium_left.run_time(150, 5000) medium_left.stop() #Large Tire Flip #StopAtWhiteLineRightMotorBackwards() robot.turn(30) robot.straight(-50) medium_left.run_time(-200, 4000) robot.straight(110) medium_left.run_time(150, 6500) #back after both tire flip robot.turn(20) robot.straight(-550) robot.straight(150) robot.turn(-125) robot.straight(300) robot.stop()
def main(): ev3 = EV3Brick() ev3.speaker.beep() ev3.screen.clear() ev3.light.on(Color.YELLOW) colors = ColorSensor(Port.S3) left_motor = Motor(Port.B, Direction.CLOCKWISE) right_motor = Motor(Port.C, Direction.CLOCKWISE) startup = True direction = 1 ev3.light.off() while (not ev3.buttons.pressed()): if (not checkForLine(ev3, colors)): left_motor.stop() right_motor.stop() # no more line! either: # a) the line turned # b) we were not running straight and went to side of line # c) there was never a line to begin with # (b) newDir = findLineSmallMotion(ev3, colors, left_motor, right_motor, direction) if (0 != newDir): direction = newDir else: # (a) newDir = findLineLargeMotion(ev3, colors, left_motor, right_motor, direction) if (0 != newDir): direction = -1 * newDir else: startup = True # (c) if (startup): ev3.speaker.beep() ev3.speaker.say("put me on the line") while (not checkForLine(ev3, colors)): wait(100) ev3.speaker.say("press any button to start") while (not ev3.buttons.pressed()): wait(10) continue startup = False # go left_motor.run(400) right_motor.run(400) wait(10)
class Gripp3r(IRBeaconRemoteControlledTank, EV3Brick): WHEEL_DIAMETER = 26 AXLE_TRACK = 115 def __init__(self, left_motor_port: Port = Port.B, right_motor_port: Port = Port.C, grip_motor_port: Port = Port.A, touch_sensor_port: Port = Port.S1, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): super().__init__(wheel_diameter=self.WHEEL_DIAMETER, axle_track=self.AXLE_TRACK, left_motor_port=left_motor_port, right_motor_port=right_motor_port, ir_sensor_port=ir_sensor_port, ir_beacon_channel=ir_beacon_channel) self.drive_base.settings( straight_speed=750, # milimeters per second straight_acceleration=750, turn_rate=90, # degrees per second turn_acceleration=90) self.grip_motor = Motor(port=grip_motor_port, positive_direction=Direction.CLOCKWISE) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def grip_or_release_by_ir_beacon(self, speed: float = 500): while True: if Button.BEACON in self.ir_sensor.buttons( channel=self.ir_beacon_channel): if self.touch_sensor.pressed(): self.speaker.play_file(file=SoundFile.AIR_RELEASE) self.grip_motor.run_time(speed=speed, time=1000, then=Stop.BRAKE, wait=True) else: self.speaker.play_file(file=SoundFile.AIRBRAKE) self.grip_motor.run(speed=-speed) while not self.touch_sensor.pressed(): pass self.grip_motor.stop() while Button.BEACON in self.ir_sensor.buttons( channel=self.ir_beacon_channel): pass def main(self, speed: float = 1000): self.grip_motor.run_time(speed=-500, time=1000, then=Stop.BRAKE, wait=True) run_parallel(self.grip_or_release_by_ir_beacon, self.keep_driving_by_ir_beacon)
class Spik3r(EV3Brick): def __init__( self, sting_motor_port: Port = Port.D, go_motor_port: Port = Port.B, claw_motor_port: Port = Port.A, touch_sensor_port: Port = Port.S1, color_sensor_port: Port = Port.S3, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.sting_motor = Motor(port=sting_motor_port, positive_direction=Direction.CLOCKWISE) self.go_motor = Motor(port=go_motor_port, positive_direction=Direction.CLOCKWISE) self.claw_motor = Motor(port=claw_motor_port, positive_direction=Direction.CLOCKWISE) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel self.touch_sensor = TouchSensor(port=touch_sensor_port) self.color_sensor = ColorSensor(port=color_sensor_port) def sting_by_ir_beacon(self): while True: ir_buttons_pressed = set(self.ir_sensor.buttons(channel=self.ir_beacon_channel)) if ir_buttons_pressed == {Button.BEACON}: self.sting_motor.run_angle( speed=-750, rotation_angle=220, then=Stop.HOLD, wait=False) self.speaker.play_file(file=SoundFile.EV3) self.sting_motor.run_time( speed=-1000, time=1000, then=Stop.HOLD, wait=True) self.sting_motor.run_time( speed=1000, time=1000, then=Stop.HOLD, wait=True) while Button.BEACON in self.ir_sensor.buttons(channel=self.ir_beacon_channel): pass def be_noisy_to_people(self): while True: if self.color_sensor.reflection() > 30: for i in range(4): self.speaker.play_file(file=SoundFile.ERROR_ALARM) def pinch_if_touched(self): while True: if self.touch_sensor.pressed(): self.claw_motor.run_time( speed=500, time=1000, then=Stop.HOLD, wait=True) self.claw_motor.run_time( speed=-500, time=0.3 * 1000, then=Stop.HOLD, wait=True) def keep_driving_by_ir_beacon(self): while True: ir_buttons_pressed = set(self.ir_sensor.buttons(channel=self.ir_beacon_channel)) if ir_buttons_pressed == {Button.RIGHT_UP, Button.LEFT_UP}: self.go_motor.run(speed=910) elif ir_buttons_pressed == {Button.RIGHT_UP}: self.go_motor.run(speed=-1000) else: self.go_motor.stop() def main(self): self.screen.load_image(ImageFile.EVIL) Thread(target=self.be_noisy_to_people).start() Thread(target=self.sting_by_ir_beacon).start() Thread(target=self.pinch_if_touched).start() self.keep_driving_by_ir_beacon()
class Robot: """Classe do Robo""" """Todos os métodos tem como primeiro parâmetro a palavra-chave 'self' e é ela que faz a referência a outras variáveis e métodos dentro da classe 'Robot'. Os outros parametros funcionam da mesma forma que em funcoes normais. """ def __init__(self, portaMotorEsquerdo, portaMotorDireito): """Esse metodo é a função de inicialização de um novo dado do tipo 'Robot'. Podemos dizer, então, que é o método de inicialização de um novo objeto da classe 'Robot'. Passamos os parametros: 'self', por ser um método, e as portas que serão associadas aos elementos do robo:""" self.motorEsquerdo = Motor(portaMotorEsquerdo) self.motorDireito = Motor(portaMotorDireito) self.sensor = None # Como nao sabemos qual tipo de sensor será conectado, vamos deixar a variável <sensor> sem nenhum valor por enquanto self.tipoSensor = "nenhum" # Podemos usar essa string para verificar qual o tipo de sensor conectado antes de utilizar ele no codigo """ Cada tipo de sensor diferente que pode ser conectado tem uma inicialização diferente Por isso podemos definir uma inicialização para cada tipo que poderemos utilizar Se soubessemos exatamente quais sensores estariam no robo, eles poderiam ser inicializados direto no metodo <__init__>, assim como foram os motores. Nesse caso, nao seria necessario verificar o tipo de sensor, pois sempre saberiamos qual o tipo e de que forma utiliza-lo.""" def iniciaSensorCor(self, portaSensor): # Para o sensor de cor self.sensor = ColorSensor(portaSensor) self.tipoSensor = "cor" def iniciaSensorUltra(self, portaSensor): # Para o sensor ultrassonico self.sensor = UltrasonicSensor(portaSensor) self.tipoSensor = "ultra" def iniciaSensorInfra(self, portaSensor): # Para o sensor infravermelho self.sensor = InfraredSensor(portaSensor) self.tipoSensor = "infra" def iniciaSensorGiro(self, portaSensor): # Para o sensor giroscopio self.sensor = GyroSensor(portaSensor) self.tipoSensor = "giro" """Metodos para utilizacao dos recursos do robo:""" def andarTempo(self, velocEsquerda, velocDireita, tempo): cronometro = StopWatch() # Definimos um cronometro cronometro.reset() # Resetamos o tempo marcado no cronometro while cronometro.time() < tempo: self.motorDireito.run(velocEsquerda) self.motorEsquerdo.run(velocDireita) self.motorDireito.stop() self.motorEsquerdo.stop() def andarRetoGraus(self, veloc, graus): while (self.motorEsquerdo.angle() < graus) and (self.motorDireito.angle() < graus): self.motorDireito.run(veloc) self.motorEsquerdo.run(veloc) self.motorDireito.stop() self.motorEsquerdo.stop() def curvaGiro(self, veloc, graus): # Curva com os dois motores utilizando o giroscopio if self.tipoSensor != "giro": # Verifica se o sensor do tipo certo esta conectado print("ERRO: GIROSCOPIO NAO CONECTADO.") return False # Interrompe o metodo self.sensor.reset_angle(0) while self.sensor.angle() < graus: self.motorDireito.run(-veloc) self.motorEsquerdo.run(veloc) self.motorDireito.stop() self.motorEsquerdo.stop() def andaAteObstaculo(self, veloc, distancia): if self.tipoSensor != "ultra" and self.tipoSensor != "infra": # O mesmo codigo funciona pro ultrassonico e pro infravermelho print("ERRO: SENSOR DE DISTANCIA NAO CONECTADO") return False # Interrompe o metodo while self.sensor.distance() < distancia: self.motorEsquerdo.run(veloc) self.motorDireito.run(veloc) self.motorEsquerdo.stop() self.motorDireito.stop() def andaAteCor(self, veloc, cor): if self.tipoSensor != "cor": print("ERRO: SENSOR DE COR NAO CONECTADO") return False # Interrompe o metodo while self.sensor.color() != cor: self.motorEsquerdo.run(veloc) self.motorDireito.run(veloc) self.motorEsquerdo.stop() self.motorDireito.stop() def angPoligono(self, lados): if lados <= 2 : print("NAO EH UM POLIGONO") return False if self.tipoSensor != "giro": print("ERRO: SENSOR GIROSCOPIO NAO CONECTADO") return False # Interrompe o metodo angint = (((lados - 2)*180)/lados) #calculo para angulos internos de um polígono" return angint
class Pilot: """Pilot class allowing user to drive the robot""" def __init__(self): self.speed = 0 # speed of the robot in deg/s self.dist_proportion = 1 # proportion of the speed put in the left motor self.angleSpeed = 0 # speed to substract to one wheel to make the robot turn self.turnItRight = 0 # determine the current angle speed, the higher it is, the more it turns to the right self.turnItLeft = 0 # determine the current angle speed, the higher it is, the more it turns to the left self.left_motor = Motor(Port.B) self.right_motor = Motor(Port.C) def forward(self, speed): """Make the robot go forward at 'speed' in deg/s""" self.speed = speed self.dist_proportion = 1 self.angleSpeed = 0 self.left_motor.run(speed) self.right_motor.run(speed) def forwardTurnRightLog(self, speedPercentage, angleAcc): """Make the robot turns right using a logaritmic function. The longer the robot turns, the more it will turn right until the robot reaches MAX_ANGLE_SPEED. This function use a logarimtic function to increase the angle speed. speedPercentage: relative speed the robot should be using while turning angleAcc: relative acceleration of the turn (from 0 to 1) """ self.dist_proportion = 1 self.speed = round(MAX_SPEED * (speedPercentage/100)) self.turnItLeft = 0 self.turnItRight = self.turnItRight + angleAcc self.angleSpeed = min(MAX_ANGLE_SPEED, round(((math.log(self.turnItRight)+3)/4)*MAX_ANGLE_SPEED)) self.left_motor.run(self.speed) self.right_motor.run(self.speed-self.angleSpeed) return self def forwardTurnLeftLog(self, speedPercentage, angleAcc): """Make the robot turns left using a logaritmic function. The longer the robot turns, the more it will turn left until the robot reaches MAX_ANGLE_SPEED. This function use a logarimtic function to increase the angle speed. speedPercentage: relative speed the robot should be using while turning angleAcc: relative acceleration of the turn (from 0 to 1) """ self.speed = round(MAX_SPEED * (speedPercentage/100)) self.dist_proportion = 1 self.turnItRight = 0 self.turnItLeft = self.turnItRight + angleAcc self.angleSpeed = min(MAX_ANGLE_SPEED, round(((math.log(self.turnItRight)+3)/4)*MAX_ANGLE_SPEED)) self.left_motor.run(self.speed) self.right_motor.run(self.speed-self.angleSpeed) return self def forwardTurnLeftExp(self, speedPercentage, angleAcc): """Make the robot turns left using a reverse exponential function. The longer the robot turns, the more it will turn left until the robot reaches MAX_ANGLE_SPEED. This function use a reverse exponential function to increase the angle speed. speedPercentage: relative speed the robot should be using while turning angleAcc: relative acceleration of the turn (from 0 to 1) """ self.speed = round(MAX_SPEED * (speedPercentage/100)) self.dist_proportion = 1 self.turnItRight = 0 self.turnItLeft = self.turnItRight + angleAcc self.angleSpeed = min(MAX_ANGLE_SPEED, round((1/math.exp(self.turnItRight*(-2)))*MAX_ANGLE_SPEED)) self.left_motor.run(self.speed) self.right_motor.run(self.speed-self.angleSpeed) return self def forwardTurnRightExp(self, speedPercentage, angleAcc): """Make the robot turns right using a reverse exponential function. The longer the robot turns, the more it will turn right until the robot reaches MAX_ANGLE_SPEED. This function use a reverse exponential function to increase the angle speed. speedPercentage: relative speed the robot should be using while turning angleAcc: relative acceleration of the turn (from 0 to 1) """ self.speed = round(MAX_SPEED * (speedPercentage/100)) self.dist_proportion = 1 self.turnItLeft = 0 self.turnItRight = self.turnItRight + angleAcc self.angleSpeed = min(MAX_ANGLE_SPEED, round((1/math.exp(self.turnItRight*(-2)))*MAX_ANGLE_SPEED)) self.left_motor.run(self.speed) self.right_motor.run(self.speed-self.angleSpeed) return self def forwardTurn2(self, speedPercentage, angle): """Make the robot turns making a curve speedPercentage : the speed in percent of the robot angle : (-100 to 100) an relative angle, the more it is high, the more the robot turn""" self.speed = MAX_SPEED * (speedPercentage/100) relativeAngle = (self.speed * angle) / 100 if(angle<0): if(self.speed > 0 ): self.dist_proportion = (self.speed+2*relativeAngle)/self.speed self.left_motor.run(self.speed+2*relativeAngle) self.right_motor.run(self.speed) elif(angle>0): self.dist_proportion = 1 self.left_motor.run(self.speed) self.right_motor.run(self.speed-2*relativeAngle) def forwardRelative(self, speedPercentage): """Make the robot move forward using relative speed (0 to 100).""" self.speed = MAX_SPEED * (speedPercentage/100) #MAX_SPEED vitesse maximale du robot self.angleSpeed = 0 self.dist_proportion = 1 self.left_motor.run(self.speed) self.right_motor.run(self.speed) def stop(self): """Make the robot stop moving by stoping all the motors.""" self.speed = 0 self.dist_proportion = 1 self.left_motor.stop() self.right_motor.stop()
class Kraz33Hors3: def __init__(self, back_foot_motor_port: Port = Port.C, front_foot_motor_port: Port = Port.B, gear_motor_port: Port = Port.A, touch_sensor_port: Port = Port.S1, color_sensor_port: Port = Port.S3, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.front_foot_motor = Motor(port=front_foot_motor_port, positive_direction=Direction.CLOCKWISE) self.back_foot_motor = Motor( port=back_foot_motor_port, positive_direction=Direction.COUNTERCLOCKWISE) self.gear_motor = Motor(port=gear_motor_port) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.color_sensor = ColorSensor(port=color_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def drive_once_by_ir_beacon( self, speed: float = 1000 # deg/s ): ir_beacons_pressed = set( self.ir_sensor.buttons(channel=self.ir_beacon_channel)) # forward if ir_beacons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}: self.front_foot_motor.run_time( speed=speed, time=1000, # ms then=Stop.COAST, wait=False) self.back_foot_motor.run_time( speed=speed, time=1000, # ms then=Stop.COAST, wait=True) # backward elif ir_beacons_pressed == {Button.LEFT_DOWN, Button.RIGHT_DOWN}: self.front_foot_motor.run_time( speed=-speed, time=1000, # ms then=Stop.COAST, wait=False) self.back_foot_motor.run_time( speed=-speed, time=1000, # ms then=Stop.COAST, wait=True) # move crazily elif ir_beacons_pressed == {Button.BEACON}: self.gear_motor.run(speed=speed) self.front_foot_motor.run_time( speed=speed / 3, time=1000, # ms then=Stop.COAST, wait=False) self.back_foot_motor.run_time( speed=-speed / 3, time=1000, # ms then=Stop.COAST, wait=True) else: self.gear_motor.stop() def keep_driving_by_ir_beacon( self, speed: float = 1000 # deg/s ): while True: self.drive_once_by_ir_beacon(speed=speed) def back_whenever_touched( self, speed: float = 1000 # deg/s ): while True: if self.touch_sensor.pressed(): self.front_foot_motor.run_time( speed=-speed, time=1000, # ms then=Stop.COAST, wait=False) self.back_foot_motor.run_time( speed=-speed, time=1000, # ms then=Stop.COAST, wait=True) def main( self, speed: float = 1000 # deg/s ): Process(target=self.back_whenever_touched).start() # FIXME: as soon as Touch Sensor pressed # OSError: [Errno 5] EIO: # Unexpected hardware input/output error with a motor or sensor: # --> Try unplugging the sensor or motor and plug it back in again. # --> To see which sensor or motor is causing the problem, # check the line in your script that matches # the line number given in the 'Traceback' above. # --> Try rebooting the hub/brick if the problem persists. self.keep_driving_by_ir_beacon(speed=speed)
class Puppy: # These constants are used for positioning the legs. HALF_UP_ANGLE = 25 STAND_UP_ANGLE = 65 STRETCH_ANGLE = 125 # These constants are for positioning the head. HEAD_UP_ANGLE = 0 HEAD_DOWN_ANGLE = -40 # These constants are for the eyes. #replaced HURT, HEART, SQUINTY with AWAKE,DIZZY,PINCHED_MIDDLE respectively NEUTRAL_EYES = Image(ImageFile.NEUTRAL) TIRED_EYES = Image(ImageFile.TIRED_MIDDLE) TIRED_LEFT_EYES = Image(ImageFile.TIRED_LEFT) TIRED_RIGHT_EYES = Image(ImageFile.TIRED_RIGHT) SLEEPING_EYES = Image(ImageFile.SLEEPING) HURT_EYES = Image(ImageFile.AWAKE) ANGRY_EYES = Image(ImageFile.ANGRY) HEART_EYES = Image(ImageFile.DIZZY) SQUINTY_EYES = Image(ImageFile.PINCHED_MIDDLE) def __init__(self): # Initialize the EV3 brick. self.ev3 = EV3Brick() # Initialize the motors connected to the back legs. self.left_leg_motor = Motor(Port.D, Direction.COUNTERCLOCKWISE) self.right_leg_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE) # Initialize the motor connected to the head. # Worm gear moves 1 tooth per rotation. It is interfaced to a 24-tooth # gear. The 24-tooth gear is connected to parallel 12-tooth gears via # an axle. The 12-tooth gears interface with 36-tooth gears. self.head_motor = Motor(Port.C, Direction.COUNTERCLOCKWISE, gears=[[1, 24], [12, 36]]) # Initialize the Color Sensor. self.color_sensor = ColorSensor(Port.S4) # Initialize the touch sensor. self.touch_sensor = TouchSensor(Port.S1) # This attribute is used by properties. self._eyes = None # These attributes are used by the playful behavior. self.playful_timer = StopWatch() self.playful_bark_interval = None def adjust_head(self): """Use the up and down buttons on the EV3 brick to adjust the puppy's head up or down. """ self.ev3.screen.show_image(ImageFile.EV3_ICON) self.ev3.light.on(Color.ORANGE) while True: buttons = self.ev3.buttons.pressed() if Button.CENTER in buttons: break elif Button.UP in buttons: self.head_motor.run(20) elif Button.DOWN in buttons: self.head_motor.run(-20) else: self.head_motor.stop() wait(100) self.head_motor.stop() self.head_motor.reset_angle(0) self.ev3.light.on(Color.GREEN) def move_head(self, target): """Move the head to the target angle. Arguments: target (int): The target angle in degrees. 0 is the starting position, negative values are below this point and positive values are above this point. """ self.head_motor.run_target(20, target) def reset(self): # must be called when puppy is sitting down. self.left_leg_motor.reset_angle(0) self.right_leg_motor.reset_angle(0) self.behavior = self.idle # The next 10 methods define the 10 behaviors of the puppy. def idle(self): """The puppy is idle.""" self.stand_up() self.eyes = self.NEUTRAL_EYES def go_to_sleep(self): """Makes the puppy go to sleep. Press the center button and touch sensor at the same time to awaken the puppy.""" self.eyes = self.TIRED_EYES self.sit_down() self.move_head(self.HEAD_DOWN_ANGLE) self.eyes = self.SLEEPING_EYES self.ev3.speaker.play_file(SoundFile.SNORING) if self.touch_sensor.pressed( ) and Button.CENTER in self.ev3.buttons.pressed(): self.behavior = self.wake_up def wake_up(self): """Makes the puppy wake up.""" self.eyes = self.TIRED_EYES self.ev3.speaker.play_file(SoundFile.DOG_WHINE) self.move_head(self.HEAD_UP_ANGLE) self.sit_down() self.stretch() wait(1000) self.stand_up() self.behavior = self.idle def act_playful(self): """Makes the puppy act playful.""" self.eyes = self.NEUTRAL_EYES self.stand_up() self.playful_bark_interval = 0 if self.playful_timer.time() > self.playful_bark_interval: self.ev3.speaker.play_file(SoundFile.DOG_BARK_2) self.playful_timer.reset() self.playful_bark_interval = randint(4, 8) * 1000 def act_angry(self): """Makes the puppy act angry.""" self.eyes = self.ANGRY_EYES self.ev3.speaker.play_file(SoundFile.DOG_GROWL) self.stand_up() wait(1500) self.ev3.speaker.play_file(SoundFile.DOG_BARK_1) def act_hungry(self): """Makes the puppy act hungry.""" self.eyes = self.HURT_EYES self.sit_down() self.ev3.speaker.play_file(SoundFile.DOG_WHINE) def go_to_bathroom(self): """Makes the puppy go to the bathroom.""" self.eyes = self.SQUINTY_EYES self.stand_up() wait(100) self.right_leg_motor.run_target(100, self.STRETCH_ANGLE) wait(800) self.ev3.speaker.play_file(SoundFile.HORN_1) wait(1000) for _ in range(3): self.right_leg_motor.run_angle(100, 20) self.right_leg_motor.run_angle(100, -20) self.right_leg_motor.run_target(100, self.STAND_UP_ANGLE) self.behavior = self.idle def act_happy(self): """Makes the puppy act happy.""" self.eyes = self.HEART_EYES # self.move_head(self.?) self.sit_down() for _ in range(3): self.ev3.speaker.play_file(SoundFile.DOG_BARK_1) self.hop() wait(500) self.sit_down() self.reset() def sit_down(self): """Makes the puppy sit down.""" self.left_leg_motor.run(-50) self.right_leg_motor.run(-50) wait(1000) self.left_leg_motor.stop() self.right_leg_motor.stop() wait(100) def walk_steps(self): """Makes the puppy walk a certain number of steps. Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs. Change steps to adjuct the number of steps.""" #steps equals number of steps pup takes steps = 10 self.stand_up() for value in range(1, steps + 1): self.left_leg_motor.run_target(-100, 25, wait=False) self.right_leg_motor.run_target(-100, 25) while not self.left_leg_motor.control.target_tolerances(): wait(200) self.left_leg_motor.run_target(100, 65, wait=False) self.right_leg_motor.run_target(100, 65) while not self.left_leg_motor.control.target_tolerances(): wait(200) self.left_leg_motor.run_target(50, 65, wait=False) self.right_leg_motor.run_target(50, 65) wait(100) def walk_timed(self): """Makes the puppy walk a certain time in ms. Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs. Change walk_time to adjust the time the puppy walks in ms.""" #walk_time equals desired walk time in ms walk_time = 6000 elapsed_time = StopWatch() while elapsed_time.time() < walk_time: self.left_leg_motor.run_target(-100, 25, wait=False) self.right_leg_motor.run_target(-100, 25) while not self.left_leg_motor.control.target_tolerances(): wait(200) self.left_leg_motor.run_target(100, 65, wait=False) self.right_leg_motor.run_target(100, 65) while not self.left_leg_motor.control.target_tolerances(): wait(200) self.left_leg_motor.run_target(50, 65, wait=False) self.right_leg_motor.run_target(50, 65) wait(100) elapsed_time.reset() # The next 4 methods define actions that are used to make some parts of # the behaviors above. def stand_up(self): """Makes the puppy stand up.""" self.left_leg_motor.run_target(100, self.HALF_UP_ANGLE, wait=False) self.right_leg_motor.run_target(100, self.HALF_UP_ANGLE) while not self.left_leg_motor.control.target_tolerances(): wait(100) self.left_leg_motor.run_target(50, self.STAND_UP_ANGLE, wait=False) self.right_leg_motor.run_target(50, self.STAND_UP_ANGLE) while not self.left_leg_motor.control.target_tolerances(): wait(100) wait(500) def stretch(self): """Makes the puppy stretch its legs backwards.""" self.stand_up() self.left_leg_motor.run_target(100, self.STRETCH_ANGLE, wait=False) self.right_leg_motor.run_target(100, self.STRETCH_ANGLE) self.ev3.speaker.play_file(SoundFile.DOG_WHINE) self.left_leg_motor.run_target(100, self.STAND_UP_ANGLE, wait=False) self.right_leg_motor.run_target(100, self.STAND_UP_ANGLE) wait(500) def hop(self): """Makes the puppy hop.""" self.left_leg_motor.run(500) self.right_leg_motor.run(500) wait(275) self.left_leg_motor.stop() self.right_leg_motor.stop() wait(275) self.left_leg_motor.run(-50) self.right_leg_motor.run(-50) wait(275) self.left_leg_motor.stop() self.right_leg_motor.stop() @property def eyes(self): """Gets and sets the eyes.""" return self._eyes @eyes.setter def eyes(self, value): if value != self._eyes: self._eyes = value self.ev3.screen.show_image(value) def run(self): """This is the main program run loop.""" self.sit_down() self.adjust_head() self.eyes = self.SLEEPING_EYES self.reset() #self.eyes = self.SLEEPING_EYES """The following code cycles through all of the behaviors, separated by beeps.""" self.act_playful() wait(1000) self.ev3.speaker.beep() self.act_happy() wait(1000) self.ev3.speaker.beep() self.act_hungry() wait(1000) self.ev3.speaker.beep() self.act_angry() wait(1000) self.ev3.speaker.beep() self.go_to_bathroom() wait(1000) self.ev3.speaker.beep() self.go_to_sleep() wait(1000) self.ev3.speaker.beep() self.wake_up() wait(1000) self.ev3.speaker.beep() self.walk_steps() wait(1000) self.ev3.speaker.beep() self.walk_timed() wait(1000) self.ev3.speaker.beep() self.idle() wait(1000) self.ev3.speaker.beep()
class Gripp3r(EV3Brick): WHEEL_DIAMETER = 26 AXLE_TRACK = 115 def __init__(self, left_motor_port: Port = Port.B, right_motor_port: Port = Port.C, grip_motor_port: Port = Port.A, touch_sensor_port: Port = Port.S1, ir_sensor_port: Port = Port.S4, ir_beacon_channel: int = 1): self.drive_base = DriveBase( left_motor=Motor(port=left_motor_port, positive_direction=Direction.CLOCKWISE), right_motor=Motor(port=right_motor_port, positive_direction=Direction.CLOCKWISE), wheel_diameter=self.WHEEL_DIAMETER, axle_track=self.AXLE_TRACK) self.drive_base.settings( straight_speed=750, # milimeters per second straight_acceleration=750, turn_rate=90, # degrees per second turn_acceleration=90) self.grip_motor = Motor(port=grip_motor_port, positive_direction=Direction.CLOCKWISE) self.touch_sensor = TouchSensor(port=touch_sensor_port) self.ir_sensor = InfraredSensor(port=ir_sensor_port) self.ir_beacon_channel = ir_beacon_channel def keep_driving_by_ir_beacon( self, channel: int = 1, speed: float = 1000 # milimeters per second ): while True: ir_beacon_buttons_pressed = set( self.ir_sensor.buttons(channel=channel)) # forward if ir_beacon_buttons_pressed == {Button.LEFT_UP, Button.RIGHT_UP}: self.drive_base.drive( speed=speed, turn_rate=0 # degrees per second ) # backward elif ir_beacon_buttons_pressed == { Button.LEFT_DOWN, Button.RIGHT_DOWN }: self.drive_base.drive( speed=-speed, turn_rate=0 # degrees per second ) # turn left on the spot elif ir_beacon_buttons_pressed == { Button.LEFT_UP, Button.RIGHT_DOWN }: self.drive_base.drive( speed=0, turn_rate=-90 # degrees per second ) # turn right on the spot elif ir_beacon_buttons_pressed == { Button.LEFT_DOWN, Button.RIGHT_UP }: self.drive_base.drive( speed=0, turn_rate=90 # degrees per second ) # turn left forward elif ir_beacon_buttons_pressed == {Button.LEFT_UP}: self.drive_base.drive( speed=speed, turn_rate=-90 # degrees per second ) # turn right forward elif ir_beacon_buttons_pressed == {Button.RIGHT_UP}: self.drive_base.drive( speed=speed, turn_rate=90 # degrees per second ) # turn left backward elif ir_beacon_buttons_pressed == {Button.LEFT_DOWN}: self.drive_base.drive( speed=-speed, turn_rate=90 # degrees per second ) # turn right backward elif ir_beacon_buttons_pressed == {Button.RIGHT_DOWN}: self.drive_base.drive( speed=-speed, turn_rate=-90 # degrees per second ) # otherwise stop else: self.drive_base.stop() def grip_or_release_by_ir_beacon(self, speed: float = 500): while True: if Button.BEACON in self.ir_sensor.buttons( channel=self.ir_beacon_channel): if self.touch_sensor.pressed(): self.speaker.play_file(file=SoundFile.AIR_RELEASE) self.grip_motor.run_time(speed=speed, time=1000, then=Stop.BRAKE, wait=True) else: self.speaker.play_file(file=SoundFile.AIRBRAKE) self.grip_motor.run(speed=-speed) while not self.touch_sensor.pressed(): pass self.grip_motor.stop() while Button.BEACON in self.ir_sensor.buttons( channel=self.ir_beacon_channel): pass def main(self, speed: float = 1000): self.grip_motor.run_time(speed=-500, time=1000, then=Stop.BRAKE, wait=True) Process(target=self.grip_or_release_by_ir_beacon).start() self.keep_driving_by_ir_beacon(speed=speed)
BRICK = EV3Brick() MEDIUM_MOTOR = Motor(port=Port.A, positive_direction=Direction.CLOCKWISE) TAIL_MOTOR = Motor(port=Port.B, positive_direction=Direction.CLOCKWISE) CHEST_MOTOR = Motor(port=Port.D, positive_direction=Direction.CLOCKWISE) IR_SENSOR = InfraredSensor(port=Port.S4) CHEST_MOTOR.run_time(speed=-300, time=1000, then=Stop.HOLD, wait=True) while True: if IR_SENSOR.distance() < 30: BRICK.light.on(color=Color.RED) MEDIUM_MOTOR.stop() TAIL_MOTOR.stop() BRICK.speaker.play_file(file=SoundFile.SNAKE_HISS) CHEST_MOTOR.run_time(speed=1000, time=1000, then=Stop.HOLD, wait=True) MEDIUM_MOTOR.run(speed=1000) TAIL_MOTOR.run(speed=-1000) CHEST_MOTOR.run_time(speed=-300, time=1000, then=Stop.HOLD, wait=True) sleep(2)