def follow_path(path):
current_x = 0
current_y = 0
sensor_motor = MediumMotor(OUTPUT_D)
tank = MoveTank(OUTPUT_B, OUTPUT_C)
print("Length of path: " + str(len(path)))
y_off = path[-1].y - path[0].y + 1
tank.on_for_degrees(-10, -10, 38 * y_off)
i = 0
for p in path:
print(str(i) + "'th: " + str(current_x), str(current_y))
x_off = p.x - current_x
y_off = p.y - current_y
if x_off > 0:
sensor_motor.on_for_degrees(20, 55)
elif x_off < 0:
sensor_motor.on_for_degrees(-20, 55)
if y_off > 0:
tank.on_for_degrees(10, 10, 38)
elif y_off < 0:
tank.on_for_degrees(-10, -10, 38)
current_x = p.x
current_y = p.y
i += 1
class AutoDrive:
def __init__(self, left_motor_port, right_motor_port, infra_mode,
ultra_mode):
self.__moveSteering = MoveSteering(left_motor_port, right_motor_port)
self.__mediumMotor = MediumMotor()
self.__ir = InfraredSensor()
self.__ir.mode = infra_mode
self.__us = UltrasonicSensor()
self.__us.mode = ultra_mode
def __run_forward(self):
self.__moveSteering.on(0, 30)
def __run_backward_rotations(self, rotations):
self.__moveSteering.on_for_rotations(0, 20, -rotations)
def __stop(self):
self.__moveSteering.off()
def __back_and_turn(self, steering):
self.__moveSteering.on_for_rotations(-steering, 20, -1)
def __scan_around_distance(self):
proximities = {}
distance = 0
index = 0
for deg in [-90, 30, 30, 30, 30, 30, 30]:
self.__mediumMotor.on_for_degrees(10, deg)
distance = self.__ir.proximity
proximities[-90 + index * 30] = distance
index += 1
time.sleep(1)
self.__mediumMotor.on_for_degrees(10, -90)
# print("%s" % proximities)
return proximities
def __find_clear_direction(self, proximitiesDic):
max_distance = max(list(proximitiesDic.values()))
direction = list(proximitiesDic.keys())[list(
proximitiesDic.values()).index(max_distance)]
#print("%d" % direction)
steering = self.__convert_direction_to_steering(direction)
return steering
def __convert_direction_to_steering(self, direction):
return 5 * direction / 9
def __ultrasonic_distance(self):
return self.__us.distance_centimeters
def run(self):
self.__run_forward()
block_distance = self.__ultrasonic_distance()
if (block_distance < 20):
self.__stop()
around_distance = self.__scan_around_distance()
steering = self.__find_clear_direction(around_distance)
self.__run_backward_rotations(0.5)
self.__back_and_turn(steering)
class Claw:
def __init__(self, output):
self.claw = MediumMotor(output)
def up(self):
self.claw.on_for_degrees(30, 360)
def down(self):
self.claw.on_for_degrees(30, -360)
class El3ctricGuitar:
NOTES = [1318, 1174, 987, 880, 783, 659, 587, 493, 440, 392, 329, 293]
N_NOTES = len(NOTES)
def __init__(
self, lever_motor_port: str = OUTPUT_D,
touch_sensor_port: str = INPUT_1, ir_sensor_port: str = INPUT_4):
self.lever_motor = MediumMotor(address=lever_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.leds = Leds()
self.speaker = Sound()
def start_up(self):
self.leds.animate_flash(
color='ORANGE',
groups=('LEFT', 'RIGHT'),
sleeptime=0.5,
duration=3,
block=True)
self.lever_motor.on_for_seconds(
speed=5,
seconds=1,
brake=False,
block=True)
self.lever_motor.on_for_degrees(
speed=-5,
degrees=30,
brake=True,
block=True)
sleep(0.1)
self.lever_motor.reset()
def play_music(self):
if self.touch_sensor.is_released:
raw = sum(self.ir_sensor.proximity for _ in range(4)) / 4
self.speaker.tone(
self.NOTES[min(round(raw / 5), self.N_NOTES - 1)]
- 11 * self.lever_motor.position,
100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
def main(self):
self.start_up()
while True:
self.play_music()
def scan():
board = []
final_line = False
color_sensor = ColorSensor(INPUT_2)
sensor_motor = MediumMotor(OUTPUT_D)
tank = MoveTank(OUTPUT_B, OUTPUT_C)
begin_node = None
end_node = None
row_num = 0
while not final_line:
row = []
for col_num in range(10):
# input("row: %d, col: %d" % (row_num, col_num))
node = Node(col_num, row_num)
r, g, b = color_sensor.rgb
cv = color_sensor.color
print(r, g, b)
if r < 50 and g >= 80 and b < 100:
begin_node = node
elif r >= 80 and g < 70 and b < 70:
if row_num >= 1 and board[row_num - 1][col_num] is not None:
board[row_num - 1][col_num].add_neighbor(node)
if col_num >= 1 and row[col_num - 1] is not None:
row[col_num - 1].add_neighbor(node)
end_node = node
final_line = True
elif r > 100 and g > 100 and b > 100:
if row_num >= 1 and board[row_num - 1][col_num] is not None:
board[row_num - 1][col_num].add_neighbor(node)
if col_num >= 1 and row[col_num - 1] is not None:
row[col_num - 1].add_neighbor(node)
else:
node = None
row.append(node)
sleep(0.1)
#TODO: move sensor here
if col_num < 9:
sensor_motor.on_for_degrees(20, 55)
sleep(0.1)
board.append(row)
row_num += 1
tank.on_for_degrees(10, 10, 38)
sleep(0.1)
sensor_motor.on_for_degrees(-20, 55 * 9)
return begin_node, end_node, board
def main():
program_running = 0
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("outD")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
MA.on_for_degrees(SpeedPercent(50), 9000)
def main(): global wheelRadius global robotRadius # Get to position of M03 move(83, 50, wheelRadius, OUTPUT_B, OUTPUT_C) turn(-90, 30, wheelRadius, robotRadius, OUTPUT_B, OUTPUT_C) move(25, -20, wheelRadius, OUTPUT_B, OUTPUT_C) move(50, 50, wheelRadius, OUTPUT_B, OUTPUT_C) turn(-90, 30, wheelRadius, robotRadius, OUTPUT_B, OUTPUT_C) # Move grabber arm down arm = MediumMotor(OUTPUT_A) arm.on_for_degrees(-20, 170) # Move back and scoop up piece move(10, -20, wheelRadius, OUTPUT_B, OUTPUT_C) arm.on_for_degrees(-15, 60) turn(90, 30, wheelRadius, robotRadius, OUTPUT_B, OUTPUT_C) move(40, -50, wheelRadius, OUTPUT_B, OUTPUT_C) turn(90, 30, wheelRadius, robotRadius, OUTPUT_B, OUTPUT_C) move(70, -50, wheelRadius, OUTPUT_B, OUTPUT_C)
class RobotGripper:
def __init__(self):
self.angleToRunTo = 0
self.motorArm = MediumMotor(OUTPUT_B)
self.CurrentAngle = 0
self.motorAngle = 0
self.motorDestinationAngle = 0
self.Hold = False
self.gainAngle = 0
def findMotorAngle(self, angle):
self.angleToRunTo = angle
self.motorDestinationAngle = angle / 0.142857
if angle > 0:
self.gainAngle = (self.angleToRunTo - self.CurrentAngle) / 0.142857
sleep(.1)
elif angle < 0:
self.gainAngle = (self.angleToRunTo + self.CurrentAngle) / 0.142857
sleep(.1)
def extend(self):
self.motorArm.on_for_degrees(-50, self.gainAngle)
sleep(3)
def setHold(self, status):
self.motorArm.on_for_degrees(100, 0, brake=status, block=True)
def retract(self):
self.setHold(True)
self.motorArm.on_for_degrees(50, self.gainAngle)
sleep(3)
class Claw:
def __init__(self, motor_address=OUTPUT_D, us_sensor_address=INPUT_2):
self._claw = MediumMotor(motor_address)
self._us_sensor = UltrasonicSensor(us_sensor_address)
self.is_open = False
def open(self):
if self.is_open:
self.close()
self._claw.on_for_degrees(SpeedPercent(50),
570,
brake=False,
block=True)
self.is_open = True
def close(self):
if not self.is_open:
self.open()
self._claw.on_for_degrees(SpeedPercent(50),
-570,
brake=False,
block=True)
self.is_open = False
def grab_when_within(self,
distance_cm=5.0,
while_waiting=None,
cancel=None):
self.open()
while self._us_sensor.distance_centimeters >= distance_cm:
if cancel and cancel():
return False
if while_waiting:
while_waiting()
self.close()
return True
def release(self):
self._claw.off()
class Robot():
def __init__(self, baseSpeed=500, dt=50):
self.leftMotor = LargeMotor(OUTPUT_C)
self.rightMotor = LargeMotor(OUTPUT_A)
self.steeringDrive = MoveSteering(OUTPUT_C, OUTPUT_A)
self.craneMotor = MediumMotor(OUTPUT_B)
self.baseSpeed = baseSpeed
self.dt = dt
def steer(self, controlSignal):
# ograniczenie sterowania
leftMotorU = max(-1000, min(self.baseSpeed + controlSignal, 1000))
rightMotorU = max(-1000, min(self.baseSpeed - controlSignal, 1000))
# sterowanie silnikami
self.leftMotor.run_timed(time_sp=self.dt,
speed_sp=leftMotorU,
stop_action="coast")
self.rightMotor.run_timed(time_sp=self.dt,
speed_sp=rightMotorU,
stop_action="coast")
sleep(self.dt / 1000)
def rotateLeft(self):
self.steeringDrive.on_for_rotations(-72, 40, 1)
sleep(1)
def rotateRight(self):
self.steeringDrive.on_for_rotations(72, 40, 1)
sleep(1)
def liftCrane(self):
self.craneMotor.on_for_degrees(20, 45)
sleep(1)
def dipCrane(self):
self.craneMotor.on_for_degrees(-20, 45)
sleep(1)
def swing_and_safety():
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
motorA = MediumMotor(OUTPUT_A)
motorD = MediumMotor(OUTPUT_D)
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50),
6.67) #ROBOT MOVES FORWARD FROM BASE
tank_drive.on_for_rotations(SpeedPercent(20), SpeedPercent(20),
.8) # ROBOT MOVES INTO SWING
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30),
0.4) #ROBOT MOVES AWAY FROM SWING
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(0),
1.5) #ROBOT TURNS TO SQUARE ON WALL
motorA.on_for_degrees(SpeedPercent(15), 150) #LEFT ARM TURNS FOR ELEVATOR
tank_drive.on_for_rotations(SpeedPercent(-15), SpeedPercent(-15),
0.45) # ROBOT MOVES BACK INTO WALL
tank_drive.on_for_rotations(SpeedPercent(30), SpeedPercent(30),
1.8) #ROBOT MOVES FORWARD TO ELEVATOR
tank_drive.on_for_rotations(SpeedPercent(30), SpeedPercent(0),
1) #ROBOT TURNS CLOCKWISE TO FACE ELEVATOR
tank_drive.on_for_rotations(SpeedPercent(30), SpeedPercent(30),
1.25) #ROBOT MOVES FORWARD AND HITS ELEVATOR
motorA.on_for_degrees(
SpeedPercent(15),
200) #MEDIUM MOTOR TURNS AWAY SO IT DOESENT UNDO ELEVATOR
tank_drive.on_for_rotations(SpeedPercent(0), SpeedPercent(-30),
0.8) #ROBOT TURNS TO SAFETY FACTOR
tank_drive.on_for_rotations(SpeedPercent(15), SpeedPercent(15),
1.13) #ROBOT MOVES INTO SAFETY FACTOR
tank_drive.on_for_rotations(SpeedPercent(10), SpeedPercent(-10),
0.2) #ROBOT TURNS TO KNOCK DOWN BEAMS
tank_drive.on_for_rotations(
SpeedPercent(-15), SpeedPercent(-15), 0.3
) # ROBOT MOVES BACK TO NOT KNOCK DOWN THE BUILDING IN SAFETY FACTOR
tank_drive.on_for_rotations(SpeedPercent(-10), SpeedPercent(10),
0.5) #ROBOT TURNS TO KNOCK DOWN BEAMS
tank_drive.on_for_rotations(SpeedPercent(-60), SpeedPercent(-60),
12) # ROBOT MOVES BACK TO BASE
class Ev3Car(rpyc.Service):
def __init__(self):
self.My_Tank = MoveTank(OUTPUT_B, OUTPUT_C)
self.Control = MediumMotor(OUTPUT_D)
def status_check(self):
self.Control.position = 0
def exposed_run(self):
self.My_Tank.on(-60, -60)
def exposed_back(self):
self.My_Tank.on(30, 30)
def exposed_stop(self):
self.My_Tank.off()
def exposed_steering(self, degree=0):
self.Control.on_for_degrees(30, degree, True, True)
class RobotRight(rpyc.Service):
ALIASES = ['Right']
def __init__(self, *args, **kwargs):
self.exposed_candy_loader = MediumMotor(OUTPUT_A)
self.exposed_candy_thrower = MediumMotor(OUTPUT_B)
self.exposed_left_cs = ColorSensor(INPUT_1)
self.exposed_right_cs = ColorSensor(INPUT_2)
super().__init__(*args, **kwargs)
def exposed_candy_throw(self):
# TODO fix value
### Load the candy in the thrower
self.exposed_candy_loader.on_for_degrees(80, 75)
self.exposed_candy_loader.on_for_degrees(80, -75)
### Throw the candy
self.exposed_candy_thrower.on_for_degrees(100, 90)
self.exposed_candy_thrower.on_for_degrees(50, -90)
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, MoveDifferential from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS from ev3dev2.wheel import EV3Tire from time import sleep from ev3dev2.motor import MediumMotor, OUTPUT_A medium_motorA = MediumMotor(OUTPUT_A) large_motorB = LargeMotor(OUTPUT_B) large_motorC = LargeMotor(OUTPUT_C) STUD_MM = 8 #created mdiff object mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM) mdiff.on_for_distance(speed=50, distance_mm=-50) mdiff.turn_left(speed=50, degrees=60) mdiff.on_for_distance(speed=50, distance_mm=145) mdiff.turn_right(speed=50, degrees=60) mdiff.on_for_distance(speed=50, distance_mm=675) sleep(2) medium_motorA.on_for_degrees(speed=50, degrees=-110) sleep(2) medium_motorA.on_for_degrees(speed=50, degrees=110)
proximity_sensor = UltrasonicSensor(INPUT_2)
touch_sensor = TouchSensor(INPUT_3)
button = Button()
gyro = GyroSensor(INPUT_1)
'''
spear_head.on_for_rotations(SpeedPercent(60), 3)
spear_head.on_for_rotations(SpeedPercent(-60), 3)
spear_head.off(brake=False)
'''
gyro.mode = GyroSensor.MODE_GYRO_CAL
gyro.mode = GyroSensor.MODE_GYRO_RATE
gyro.mode = GyroSensor.MODE_GYRO_ANG
time.sleep(1)
front_claw.on_for_degrees(SpeedPercent(20), 570) # opens the claw
wheels.on(0, SpeedPercent(50))
while proximity_sensor.distance_centimeters >= 22:
print('First wall: {}'.format(proximity_sensor.distance_centimeters))
wheels.on(-100, SpeedPercent(15))
start_angle = gyro.angle
while gyro.angle > -82:
print(gyro.angle)
wheels.on(0, SpeedPercent(50))
while proximity_sensor.distance_centimeters >= 12:
pass
wheels.off(brake=True)
#!/usr/bin/env python3 from ev3dev2.motor import MediumMotor, OUTPUT_A from ev3dev2.motor import SpeedDPS, SpeedRPS, SpeedRPM from time import sleep medium_motor = MediumMotor(OUTPUT_A) # We'll make the motor turn 180 degrees # at speed 50 (780 dps for the medium motor) medium_motor.on_for_degrees(speed=50, degrees=180) # then wait one second sleep(1) # then – 180 degrees at 500 dps medium_motor.on_for_degrees(speed=SpeedDPS(500), degrees=-180) sleep(1) # then 0.5 rotations at speed 50 (780 dps) medium_motor.on_for_rotations(speed=50, rotations=0.5) sleep(1) # then – 0.5 rotations at 1 rotation per second (360 dps) medium_motor.on_for_rotations(speed=SpeedRPS(1), rotations=-0.5) sleep(1) medium_motor.on_for_seconds(speed=SpeedRPM(60), seconds=5) sleep(1) medium_motor.on(speed=60) sleep(2) medium_motor.off()
#Drawing letter " O " #!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, MediumMotor, MoveTank, MoveSteering, OUTPUT_A, OUTPUT_B, OUTPUT_C Medium_motor = MediumMotor(OUTPUT_A) Large_motor1 = LargeMotor(OUTPUT_B) Large_motor2 = LargeMotor(OUTPUT_C) steer_Pair = MoveSteering(OUTPUT_B, OUTPUT_C) tank = MoveTank(OUTPUT_B, OUTPUT_C) Medium_motor.on_for_degrees(50, 500) tank.on_for_rotations(50, 10, 6) #Drawing letter " q " #!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, MediumMotor, MoveTank, MoveSteering, OUTPUT_A, OUTPUT_B, OUTPUT_C Medium_motor = MediumMotor(OUTPUT_A) Large_motor1 = LargeMotor(OUTPUT_B) Large_motor2 = LargeMotor(OUTPUT_C) steer_Pair = MoveSteering(OUTPUT_B, OUTPUT_C) tank = MoveTank(OUTPUT_B, OUTPUT_C) Medium_motor.on_for_degrees(50, 500) steer_Pair.on_for_degrees(steering = 90, speed = 75, degrees = 1080) tank.on_for_degrees(50, 50, 360) #Drawing letter " F "
class Ev3Robot:
def __init__(self,
wheel1=OUTPUT_B,
wheel2=OUTPUT_C,
wheel3=OUTPUT_A,
wheel4=OUTPUT_D):
self.steer_pair = MoveSteering(wheel1, wheel2)
self.gyro = GyroSensor()
self.tank_pair = MoveTank(wheel1, wheel2)
self.motor1 = LargeMotor(wheel1)
self.motor2 = LargeMotor(wheel2)
self.motor3 = MediumMotor(wheel3)
self.motor4 = MediumMotor(wheel4)
self.color1 = ColorSensor(INPUT_1)
self.color4 = ColorSensor(INPUT_4)
self._black1 = 0
self._black4 = 0
self._white1 = 100
self._white4 = 100
self.gyro.mode = 'GYRO-ANG'
self.led = Leds()
self.btn = Button()
self.btn._state = set()
def write_color(self, file, value):
# opens a file
f = open(file, "w")
# writes a value to the file
f.write(str(value))
f.close()
def read_color(self, file):
# opens a file
f = open(file, "r")
# reads the value
color = int(f.readline().strip())
f.close()
return color
def pivot_right(self, degrees, speed=20):
# makes the robot pivot to the right until the gyro sensor
# senses that it has turned the required number of degrees
self.tank_pair.on(left_speed=speed, right_speed=0)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def pivot_left(self, degrees, speed=20):
# makes the robot pivot to the left until the gyro sensor
# senses that it has turned the required number of degrees
self.tank_pair.on(left_speed=0, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees - 10)
self.tank_pair.off()
def old_spin_right(self, degrees, speed=20):
#old program; don't use
self.gyro.reset()
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed, right_speed=speed * -1)
self.gyro.wait_until_angle_changed_by(degrees)
value2 = self.gyro.angle
self.tank_pair.on(left_speed=-30, right_speed=30)
self.gyro.wait_until_angle_changed_by(value1 - value2 - degrees)
self.stop()
def old_spin_left(self, degrees, speed=20):
#old program; don't use
value1 = self.gyro.angle
self.tank_pair.on(left_speed=speed * -1, right_speed=speed)
self.gyro.wait_until_angle_changed_by(degrees)
value2 = self.gyro.angle
self.tank_pair.on(left_speed=8, right_speed=-8)
self.gyro.wait_until_angle_changed_by(value2 - value1 - degrees + 5)
self.tank_pair.off()
def dog_gear(self, degrees, motor, speed=30):
if motor == 3:
self.motor3.on_for_degrees(degrees=degrees, speed=speed)
self.motor3.off()
else:
self.motor4.on_for_degrees(degrees=degrees, speed=speed)
def go_straight_forward(self, cm, speed=20, wiggle_factor=1):
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05 #divides by circumference of the wheel
# calculates the amount of degrees the robot has turned, then turns the
# opposite direction and repeats until the robot has gone the required
# number of centimeters
while abs(self.motor1.position - value1) / 360 < rotations:
angle = self.gyro.angle - angle0
self.steer_pair.on(steering=angle * wiggle_factor * -1,
speed=speed * -1)
self.steer_pair.off()
def go_straight_backward(self, cm, speed=20, wiggle_factor=1):
# see go_straight_forward
value1 = self.motor1.position
angle0 = self.gyro.angle
rotations = cm / 19.05
while abs(self.motor1.position - value1) / 360 < rotations:
angle = self.gyro.angle - angle0
self.steer_pair.on(steering=angle * wiggle_factor, speed=speed)
self.steer_pair.off()
def calibrate(self):
# turns the led lights orange, and waits for a button to be pressed
# (signal that the robot is on black), then calculates the reflected
# light intensity of the black line, then does the same on the white line
self.led.set_color('LEFT', 'ORANGE')
self.led.set_color('RIGHT', 'ORANGE')
while not self.btn.any():
sleep(0.01)
self.led.set_color('LEFT', 'GREEN')
self.led.set_color('RIGHT', 'GREEN')
self._black1 = self.color1.reflected_light_intensity
self._black4 = self.color4.reflected_light_intensity
sleep(2)
self.led.set_color('LEFT', 'ORANGE')
self.led.set_color('RIGHT', 'ORANGE')
while not self.btn.any():
sleep(0.01)
self.led.set_color('LEFT', 'GREEN')
self.led.set_color('RIGHT', 'GREEN')
self._white1 = self.color1.reflected_light_intensity
self._white4 = self.color4.reflected_light_intensity
sleep(3)
self.write_color("/tmp/black1", self._black1)
self.write_color("/tmp/black4", self._black4)
self.write_color("/tmp/white1", self._white1)
self.write_color("/tmp/white4", self._white4)
def read_calibration(self):
# reads the color files
self._black1 = self.read_color("/tmp/black1")
self._black4 = self.read_color("/tmp/black4")
self._white1 = self.read_color("/tmp/white1")
self._white4 = self.read_color("/tmp/white4")
def align_white(self, speed=20, t=96.8, direction=1):
# goes forward until one of the color sensors sees the white line.
while self.calibrate_RLI(self.color1) < t and self.calibrate_RLI(
self.color4) < t:
self.steer_pair.on(steering=0, speed=speed * direction)
self.steer_pair.off()
# determines which sensor sensed the white line, then moves the opposite
# motor until both sensors have sensed the white line
if self.calibrate_RLI(self.color4) > t:
while self.calibrate_RLI(self.color1) < t:
self.motor1.on(speed=speed * direction)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) < t:
self.motor2.on(speed=speed * direction)
self.motor2.off()
def align_black(self, speed=20, t=4.7, direction=1):
# see align_white
while self.calibrate_RLI(self.color1) > t and self.calibrate_RLI(
self.color4) > t:
self.steer_pair.on(steering=0, speed=speed * direction)
self.steer_pair.off()
if self.calibrate_RLI(self.color4) < t:
while self.calibrate_RLI(self.color1) > t:
self.motor1.on(speed=speed * direction)
self.motor1.off()
else:
while self.calibrate_RLI(self.color4) > t:
self.motor2.on(speed=speed * direction)
self.motor2.off()
def align(self, t, speed=-20, direction=1):
# aligns three times for extra accuracy
self.align_white(speed=speed, t=100 - t, direction=direction)
self.align_black(speed=-5, t=t, direction=direction)
self.align_white(speed=-5, t=100 - t, direction=direction)
def calibrate_RLI(self, color_sensor):
# returns a scaled value based on what black and white are
if (color_sensor.address == INPUT_1):
black = self._black1
white = self._white1
else:
black = self._black4
white = self._white4
return (color_sensor.reflected_light_intensity - black) / (white -
black) * 100
def stop(self):
self.tank_pair.off()
def spin_right(self, degrees, speed=30):
self.turn(degrees, 100, speed)
def spin_left(self, degrees, speed=30):
self.turn(degrees, -100, speed)
def turn(self, degrees, steering, speed=30):
# turns at a decreasing speed until it turns the required amount of degrees
value1 = self.gyro.angle
s1 = speed
d1 = 0
while d1 < degrees - 2:
value2 = self.gyro.angle
d1 = abs(value1 - value2)
slope = speed / degrees
s1 = (speed - slope * d1) * (degrees / 90) + 3
self.steer_pair.on(steering=steering, speed=s1)
self.steer_pair.off()
def Krab():
MA = MediumMotor("outA")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("outD")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
#Setting the Gyro. V
GY.mode = 'GYRO-ANG'
GY.mode = 'GYRO-RATE'
GY.mode = 'GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 1
starting_value = GY.value()
gyro_correct_loops = 0
straight_correct_loops = 0
gyro_correct_straight = 0
# change this to whatever speed what you want. V
left_wheel_speed = 100
right_wheel_speed = 100
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# Wheel alignment. VVVV
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01)
#Pulling out of Launch area. V
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30),
1.625)
#Gyro Turn toowards the red circle. V
while GY.value() < 80:
left_wheel_speed = 100
right_wheel_speed = -100
#MB is left wheel & MC is right wheel
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
#Driving forward towards the safety factor. V
while MB.position > -1700:
if GY.value() == 90:
left_wheel_speed = -500
right_wheel_speed = -500
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 8
gyro_correct_loops = 0
gyro_correct_straight = 0
straight_correct_loops = 0
else:
if GY.value() < 90:
correct_rate = abs(
GY.value()
) # This captures the gyro value at the beginning of the statement
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -500
right_wheel_speed = -500
if abs(
GY.value()
) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
gyro_correct_loops = gyro_correct_loops + 1
if GY.value() == 0 and gyro_correct_straight == 0:
while straight_correct_loops < gyro_correct_loops + 1:
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1
gyro_correct_loops = 0
straight_correct_loops = 0
else:
correct_rate = abs(
GY.value()) # Same idea as the other if statement
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -500
right_wheel_speed = -500
gyro_correct_loops = gyro_correct_loops + 1
if abs(GY.value()) >= correct_rate:
gyro_adjust = gyro_adjust + 1
if GY.value(
) == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line
while straight_correct_loops < gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again
gyro_correct_loops = 0
straight_correct_loops = 0
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
#unlocking arm to get elevator
MD.on_for_degrees(SpeedPercent(-50), 176.26)
#pushing down the beams from safety factor
tank_drive.on_for_rotations(SpeedPercent(-50), SpeedPercent(-50), 2.75)
#going back to home. V
while MB.position < 0: #2244 previously
if GY.value() == 90:
left_wheel_speed = 900
right_wheel_speed = 900
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 8
gyro_correct_loops = 0
gyro_correct_straight = 0
straight_correct_loops = 0
else:
if GY.value() < 90:
correct_rate = abs(
GY.value()
) # This captures the gyro value at the beginning of the statement
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = 900
right_wheel_speed = 900
if abs(
GY.value()
) <= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
gyro_correct_loops = gyro_correct_loops + 1
if GY.value() == 0 and gyro_correct_straight == 0:
while straight_correct_loops > gyro_correct_loops + 1:
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1
gyro_correct_loops = 0
straight_correct_loops = 0
else:
correct_rate = abs(
GY.value()) # Same idea as the other if statement
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = 900
right_wheel_speed = 900
gyro_correct_loops = gyro_correct_loops + 1
if abs(GY.value()) <= correct_rate:
gyro_adjust = gyro_adjust + 1
if GY.value(
) == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line
while straight_correct_loops > gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again
gyro_correct_loops = 0
straight_correct_loops = 0
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
#still going home
tank_drive.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 1.5)
Launchrun()
class LegoBot(MoveDifferential):
def __init__(self, left_motor_port, right_motor_port, rot_motor_port,
wheel_class, wheel_distance_mm,
desc=None, motor_class=LargeMotor):
MoveDifferential.__init__(self, left_motor_port, right_motor_port, wheel_class, wheel_distance_mm)
"""
LegoBot Class inherits all usefull stuff for differential drive
and adds sound, LEDs, IRSensor which is rotated by Medium Motor
"""
self.leds = Leds()
self.sound = Sound()
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
# Startup sequence
self.sound.play_song((('C4', 'e'), ('D4', 'e'), ('E5', 'q')))
self.leds.set_color("LEFT", "GREEN")
self.leds.set_color("RIGHT", "GREEN")
# create IR sensors
self.ir_sensor = InfraredSensor()
self.sensor_rotation_point = Pose( 0.05, 0.0, np.radians(0))
self.sensor_rotation_radius = 0.04
self.sensor_thread_run = False
self.sensor_thread_id = None
# temporary storage for ir readings and poses until half rotation is fully made
self.ir_sensors = None
# initialize motion
self.ang_velocity = (0.0,0.0)
self.rot_motor = MediumMotor(rot_motor_port)
self.rotate_thread_run = False
self.rotate_thread_id = None
self.rotation_degrees = 180
# information about robot for controller or supervisor
self.info = Struct()
self.info.wheels = Struct()
self.info.wheels.radius = self.wheel.radius_mm /1000
self.info.wheels.base_length = wheel_distance_mm /1000
self.info.wheels.max_velocity = 2*pi*170/60 # 170 RPM
self.info.wheels.min_velocity = 2*pi*30/60 # 30 RPM
self.info.pose = None
self.info.ir_sensors = Struct()
self.info.ir_sensors.poses = None
self.info.ir_sensors.readings = None
self.info.ir_sensors.rmax = 0.7
self.info.ir_sensors.rmin = 0.04
# starting odometry thread
self.odometry_start(0,0,0)
# start measuring distance with IR Sensor in another thread while rotating
self.sensor_update_start(self.rot_motor)
# start rotating of medium motor
self.rotate_and_update_sensors_start()
def turn_off(self):
# stop odometry thread
self.odometry_stop()
# stop updating sensors
self.rotate_and_update_sensors_stop()
# return robots head to start position
self.rot_motor.on_to_position(100, 0, True, True)
self.sensor_update_stop()
# Shutdown sequence
self.sound.play_song((('E5', 'e'), ('C4', 'e')))
self.leds.set_color("LEFT", "BLACK")
self.leds.set_color("RIGHT", "BLACK")
def get_pose(self):
"""Get the pose of the object in world coordinates"""
return Pose(self.x_pos_mm/1000, self.y_pos_mm/1000, self.theta)
def move(self,dt):
(vl, vr) = self.get_wheel_speeds()
# actual robot move
self.on_for_seconds(SpeedRPS(vl/2/pi), SpeedRPS(vr/2/pi), dt, False, False)
def get_info(self):
# getting updated info for supervisor
self.info.pose = self.get_pose()
return self.info
def set_inputs(self,inputs):
""" Setting new values of (vl, vr) sent by supervisor and controller """
self.set_wheel_speeds(inputs)
def get_wheel_speeds(self):
return self.ang_velocity
def set_wheel_speeds(self,*args):
if len(args) == 2:
(vl, vr) = args
else:
(vl, vr) = args[0]
left_ms = max(-self.info.wheels.max_velocity, min(self.info.wheels.max_velocity, vl))
right_ms = max(-self.info.wheels.max_velocity, min(self.info.wheels.max_velocity, vr))
self.ang_velocity = (left_ms, right_ms)
def sensor_update_start(self, motor, sleep_time=0.005): # 5ms
"""
A thread is started that will run until the user calls sensor_update_stop()
which will set sensor_thread_run to False
"""
self.ir_sensors = {}
def _sensor_monitor():
while self.sensor_thread_run:
angle = -np.radians(motor.degrees) # convert from degrees to radians
sensor_x = round(self.sensor_rotation_radius*cos(angle) + self.sensor_rotation_point.x, 3)
sensor_y = round(self.sensor_rotation_radius*sin(angle) + self.sensor_rotation_point.y, 3)
# adding to temp dict sensor readings and poses
self.ir_sensors.update({(sensor_x, sensor_y, angle):round(self.ir_sensor.proximity*0.007, 3)})
time.sleep(0.005)
self.sensor_thread_id = None
self.sensor_thread_run = True
self.sensor_thread_id = _thread.start_new_thread(_sensor_monitor, ())
def sensor_update_stop(self):
"""
Signal the sensor update thread to exit and wait for it to exit
"""
if self.sensor_thread_id:
self.sensor_thread_run = False
while self.sensor_thread_id:
pass
def rotate_and_update_sensors_start(self):
self.info.ir_sensors.readings = []
self.info.ir_sensors.poses = []
def _rotate_monitor():
while self.rotate_thread_run:
# writing ir sensor reading and poses from temp dict
self.info.ir_sensors.readings = [*self.ir_sensors.values()]
self.info.ir_sensors.poses = [*self.ir_sensors]
# cleaning up temp dict
self.ir_sensors = {}
# rotate rotation motor with sensor
self.rot_motor.on_for_degrees(50, self.rotation_degrees, True, True)
time.sleep(0.005)
# change orientation of rotation
self.rotation_degrees = -self.rotation_degrees
self.rotate_thread_id = None
self.rot_motor.position = 0
# rotate head to the left at start
self.rot_motor.on_for_degrees(100, -90, True, True)
self.rotate_thread_run = True
self.rotate_thread_id = _thread.start_new_thread(_rotate_monitor, ())
def rotate_and_update_sensors_stop(self):
"""
Signal the sensor update thread to exit and wait for it to exit
"""
if self.rotate_thread_id:
self.rotate_thread_run = False
while self.rotate_thread_id:
pass
#!/usr/bin/env python3 from ev3dev2.motor import MediumMotor, OUTPUT_B from time import sleep mm = MediumMotor() mm.on(speed=50, brake=True, block=False) sleep(1) mm.off() sleep(1) mm.on_for_seconds(speed=50, seconds=2, brake=True, block=True) sleep(1) mm.on_for_rotations(50, 3) sleep(1) mm.on_for_degrees(50, 90) sleep(1) mm.on_to_position(50, 180) sleep(1)
def main():
Sound.speak("").wait()
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
GY.mode='GYRO-ANG'
GY.mode='GYRO-RATE'
GY.mode='GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 1
starting_value = GY.value()
# change this to whatever speed what you want. V
left_wheel_speed = -300
right_wheel_speed = -300
# if Gyro value is the same as the starting value, go straigt. if more turn right. if less turn left. V
# FIX THIS VALUE!!!!!!!!!! V
while MB.position >= -500: # consider adjusting the wheel speeds only if your current gyro value doesn't equal the starting value
if GY.value() == 0:
left_wheel_speed = -300
right_wheel_speed = -300
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 1
else:
if GY.value() > starting_value:
correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate:
gyro_adjust = gyro_adjust + 1 # If gyro value has worsened despite the correction then change the adjust variable for next time
else:
correct_rate = abs (GY.value()) # Same idea as the other if statement
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate:
gyro_adjust = gyro_adjust + 1
# wait for the block to drop. V
sleep(3)
# pulling away from the crane. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 0.25)
# Unlocking attachment. V
MD.on_for_degrees(SpeedPercent(50), 600)
# pulling away from unlocked attachment. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 1)
# gyro 90 degree spin turn
while GY.value() <= 45:
MB.run_forever(speed_sp=300)
MC.run_forever(speed_sp=-300)
# drive into home
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 4)
def Crane():
Sound.speak("").wait()
MA = MediumMotor("outA")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("outD")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
GY.mode='GYRO-ANG'
GY.mode='GYRO-RATE'
GY.mode='GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 12
# change this to whatever speed what you want
left_wheel_speed = -300
right_wheel_speed = -300
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #wheel alignment
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# change the value to how far you want the robot to go. V
while MB.position > -900:
if GY.value() == 0:
left_wheel_speed = -300
right_wheel_speed = -300
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 9
else:
if GY.value() < 0:
correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement
left_wheel_speed = left_wheel_speed + gyro_adjust
right_wheel_speed = right_wheel_speed - gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
else:
correct_rate = abs (GY.value()) # Same idea as the other if statement
right_wheel_speed = right_wheel_speed + gyro_adjust
left_wheel_speed = left_wheel_speed - gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate:
gyro_adjust = gyro_adjust + 1
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
# wait for the block to drop. V
sleep(1.5)
# pulling away from the crane. V
tank_drive.on_for_rotations(SpeedPercent(20), SpeedPercent(10), 0.50)
# Unlocking attachment. V
MD.on_for_degrees(SpeedPercent(50), 360)
# pulling away from unlocked attachment. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 1)
# gyro 90 degree spin turn
while GY.value() < 91:
MB.run_forever(speed_sp=300)
MC.run_forever(speed_sp=-300)
# drive into home
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 4)
program_running = 0
Launchrun()
def Krab():
MA = MediumMotor("outA")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("outD")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
#Setting the Gyro. V
GY.mode='GYRO-ANG'
GY.mode='GYRO-RATE'
GY.mode='GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 1
starting_value = GY.value()
gyro_correct_loops = 0
straight_correct_loops = 0
gyro_correct_straight = 0
# change this to whatever speed what you want. V
left_wheel_speed = 100
right_wheel_speed = 100
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# Wheel alignment. VVVV
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01)
#Pulling out of Launch area. V
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1.625)
#Gyro Turn toowards the red circle. V
while GY.value() < 80:
left_wheel_speed = 100
right_wheel_speed = -100
#MB is left wheel & MC is right wheel
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
#Driving forward towards the red circle. V
while MB.position > -1700: #was -2550, Joshua is changing it to -2300
if GY.value() == 90:
left_wheel_speed = -500
right_wheel_speed = -500
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 8
gyro_correct_loops = 0
gyro_correct_straight = 0
straight_correct_loops = 0
else:
if GY.value() < 90:
correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -500
right_wheel_speed = -500
if abs (GY.value()) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
gyro_correct_loops = gyro_correct_loops + 1
if GY.value() == 0 and gyro_correct_straight == 0:
while straight_correct_loops < gyro_correct_loops + 1:
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1
gyro_correct_loops = 0
straight_correct_loops = 0
else:
correct_rate = abs (GY.value()) # Same idea as the other if statement
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -500
right_wheel_speed = -500
gyro_correct_loops = gyro_correct_loops + 1
if abs (GY.value()) >= correct_rate:
gyro_adjust = gyro_adjust + 1
if GY.value() == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line
while straight_correct_loops < gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again
gyro_correct_loops = 0
straight_correct_loops = 0
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
#unlocking arm to get elevator
MD.on_for_degrees(SpeedPercent(-50), 176.26)
#pushing down the beams from safety factor
tank_drive.on_for_rotations(SpeedPercent(-50), SpeedPercent(-50), 2.75)
#going back to home
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 6)
tank_drive.on_for_rotations(SpeedPercent(40), SpeedPercent(40), 3)
Launchrun()
def Launchrun():
Sound.speak("ready to go")
btn = Button()
#adjust the while statement for however long you want it to go.
while True:
# V if up button is pressed, wait 1 second. if button is still pressed run program 1 if else run program 2 (repeat for each button)
if btn.up:
sleep(1)
if btn.up:
Krab()
else:
Krab()
if btn.down:
sleep(1)
if btn.down:
Spider()
else:
Spider()
if btn.left:
sleep(1)
if btn.left:
Crane()
else:
Crane()
if btn.right:
sleep(1)
if btn.right:
Bulldozer()
else:
Bulldozer()
if btn.enter:
sleep(1)
if btn.enter:
Redcircle()
else:
Traffic()
#running launchrun
Launchrun()
class ColorTank:
def __init__(self, left_motor_port, right_motor_port, infra_sensor_mode,
color_sensor_mode):
self.__movesteering = MoveSteering(left_motor_port, right_motor_port)
self.__mediummotor = MediumMotor()
self.__cs = ColorSensor()
self.__cs.mode = color_sensor_mode
self.__ir = InfraredSensor()
self.__ir.mode = infra_sensor_mode
def __turn_left(self):
self.__movesteering.on(-50, 30)
def __turn_right(self):
self.__movesteering.on(50, 30)
def __run_forward(self):
self.__movesteering.on(0, 50)
def __run_backward(self):
self.__movesteering.on(0, -20)
def __stop(self):
self.__movesteering.off()
def __play_text_sound(self, words):
sound = Sound()
sound.speak(words)
def __lift_up(self):
self.__mediummotor.on_for_degrees(10, 50)
def __lift_down(self):
self.__mediummotor.on_for_degrees(10, -50)
def __get_button_pressed_value(self, buttons):
BUTTON_VALUES = {
0: [],
1: ['top_left'],
2: ['bottom_left'],
3: ['top_right'],
4: ['bottom_right'],
5: ['top_left', 'top_right'],
6: ['top_left', 'bottom_right'],
7: ['bottom_left', 'top_right'],
8: ['bottom_left', 'bottom_right'],
9: ['beacon'],
10: ['top_left', 'bottom_left'],
11: ['top_right', 'bottom_right']
}
return list(BUTTON_VALUES.keys())[list(
BUTTON_VALUES.values()).index(buttons)]
def __run(self, button_value):
if (button_value == 1):
self.__turn_left()
elif (button_value == 3):
self.__turn_right()
elif (button_value == 5):
self.__run_forward()
elif (button_value == 8):
self.__run_backward()
elif (button_value == 2):
self.__lift_up()
elif (button_value == 4):
self.__lift_down()
# elif(button_value == 2):
# self.__play_text_sound("Lily, I love you")
else:
self.__stop()
def __color_detect(self):
color = self.__cs.color
if (color == 1):
self.__play_text_sound("black")
elif (color == 2):
self.__play_text_sound("blue")
elif (color == 3):
self.__play_text_sound("green")
elif (color == 4):
self.__play_text_sound("yellow")
elif (color == 5):
self.__play_text_sound("red")
elif (color == 6):
self.__play_text_sound("white")
elif (color == 7):
self.__play_text_sound("brown")
else:
pass
def process(self):
self.__ir.process()
buttons_pressed = self.__ir.buttons_pressed()
button_value = self.__get_button_pressed_value(buttons_pressed)
self.__run(button_value)
self.__color_detect()
class AthenaRobot(object):
# constructors for the robot with default parameters of wheel radius and ports
def __init__(self,
wheelRadiusCm=4,
leftLargeMotorPort=OUTPUT_B,
rightLargeMotorPort=OUTPUT_C,
leftMediumMotorPort=OUTPUT_A,
rightMediumMotorPort=OUTPUT_D,
leftSensorPort=INPUT_1,
rightSensorPort=INPUT_4,
ultraSonicSensorPort=INPUT_2):
#self is the current object, everything below for self are member variables
self.wheelRadiusCm = wheelRadiusCm
self.wheelCircumferenceCm = 2 * math.pi * wheelRadiusCm
self.leftLargeMotor = LargeMotor(leftLargeMotorPort)
self.rightLargeMotor = LargeMotor(rightLargeMotorPort)
self.leftMediumMotor = MediumMotor(leftMediumMotorPort)
self.rightMediumMotor = MediumMotor(rightMediumMotorPort)
self.leftSensor = ColorSensor(leftSensorPort)
self.rightSensor = ColorSensor(rightSensorPort)
# run a distance in centimeters at speed of centimeters per second
def run(self, distanceCm, speedCmPerSecond, brake=True, block=True):
if speedCmPerSecond < 0:
raise Exception('speed cannot be negative')
# Calculate degrees of distances and SpeedDegreePerSecond
degreesToRun = distanceCm / self.wheelCircumferenceCm * 360
speedDegreePerSecond = speedCmPerSecond / self.wheelCircumferenceCm * 360
print("Run - Degree: {0:.3f} Speed:{1:.3f} MaxSpeed {2}".format(
degreesToRun, speedDegreePerSecond, self.leftLargeMotor.max_speed),
file=sys.stderr)
# run motors based on the calculated results
self.leftLargeMotor.on_for_degrees(
SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, False)
self.rightLargeMotor.on_for_degrees(
SpeedDPS(speedDegreePerSecond) * (-1), degreesToRun, brake, block)
# turn a angle in degrees, positive means turn right and negative means turn left.
def turn(self, degree, speed=10, brake=True, block=True):
# 1.9 is a scale factor from experiments
degreesToRun = degree * 1.275
# Turn at the speed
self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, False)
self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block)
def turnRight(self, degree, speed=10, brake=True, block=True):
self.turn(degree, speed, brake, block)
def turnLeft(self, degree, speed=10, brake=True, block=True):
self.turn(-degree, speed, brake, block)
def turnOnRightWheel(self, degree, speed=10, brake=True, block=True):
degreesToRun = degree * 2.7
self.rightLargeMotor.on_for_degrees(speed, degreesToRun, brake, block)
def turnRightOnRightWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnRightWheel(degree, speed, brake, block)
def turnLeftOnRightWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnRightWheel(-degree, speed, brake, block)
def turnOnLeftWheel(self, degree, speed=10, brake=True, block=True):
degreesToRun = degree * 2.7
self.leftLargeMotor.on_for_degrees(-speed, degreesToRun, brake, block)
def turnRightOnLeftWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnLeftWheel(degree, speed, brake, block)
def turnLeftOnLeftWheel(self, degree, speed=10, brake=True, block=True):
self.turnOnLeftWheel(-degree, speed, brake, block)
#Medium Motor Movement
def moveMediumMotor(self, isLeft, speed, degrees, brake=True, block=True):
#sees which motor is running
if isLeft == False:
self.rightMediumMotor.on_for_degrees(speed, degrees, brake, block)
else:
self.leftMediumMotor.on_for_degrees(speed, degrees, brake, block)
# Following a line with one sensor
def lineFollow(self,
whiteThreshold=98,
blackThreshold=15,
scale=0.2,
useLeftSensor=True,
useLeftEdge=True,
runDistanceCM=300):
self.leftLargeMotor.reset()
self.rightLargeMotor.reset()
# Allow an attached backsensor. Ixf useBackSensor, defining back sensor and revert useLeftEdge since motor is actually going backward
initialPos = self.leftLargeMotor.position # remember initial position
loop = True
while loop:
# use left or right sensor based on passed in useLeftSensor
reflect = self.leftSensor.reflected_light_intensity if useLeftSensor == True else self.rightSensor.reflected_light_intensity
# Allow an attached backsensor. If useBackSensor, use reflected_light_intensity of that sensor
leftPower = abs(reflect - blackThreshold) * scale
rightPower = abs(whiteThreshold - reflect) * scale
# if we follow the right edge, need to swap left and right
if useLeftEdge == False:
oldLeft = leftPower
leftPower = rightPower
rightPower = oldLeft
self.leftLargeMotor.on(-leftPower)
self.rightLargeMotor.on(-rightPower)
# Calculate the distance run in CM
distanceRanInCM = abs((self.leftLargeMotor.position - initialPos) *
(self.wheelCircumferenceCm /
self.leftLargeMotor.count_per_rot))
# Printing the reflected light intensity with the powers of the two motors
print(
"LineFollow - reflect: {0:3d} leftPower: {1:3f} rightPower: {2:3f} lMotorPos: {3:3d} distanceRanInCM {4:3f}"
.format(reflect, leftPower, rightPower,
self.leftLargeMotor.position, distanceRanInCM),
file=sys.stderr)
if distanceRanInCM >= runDistanceCM:
loop = False
# Stopping the motor once the loop is over
self.leftLargeMotor.off()
self.rightLargeMotor.off()
# run until both conditions are met
def onUntilTwoConditions(self,
leftCondition,
rightCondition,
caller,
speed=5,
consecutiveHit=5,
sleepTime=0.01):
# Start motor at passed speonUntilTwoConditionsed.
self.leftLargeMotor.on(-speed)
self.rightLargeMotor.on(-speed)
condLeftCounter = 0
condRightCounter = 0
condLeftMet = False
condRightMet = False
while (not condLeftMet or not condRightMet):
# check left condition
if (leftCondition()):
condLeftCounter += 1
else:
condLeftCounter = 0
# reset to zero
if (condLeftCounter >= consecutiveHit):
if (condRightMet):
sleep(.1)
self.leftLargeMotor.off()
condLeftMet = True
# check right condition
if (rightCondition()):
condRightCounter += 1
else:
condRightCounter = 0
# reset to zero
if (condRightCounter >= consecutiveHit):
if (condLeftMet):
sleep(.1)
self.rightLargeMotor.off()
condRightMet = True
print(
"{4}-onUntilTwoConditions: left_reflected: {0:3d}, right_reflected: {1:3d}, leftHit: {2:3d}, rightHit: {3:3d}"
.format(self.leftSensor.reflected_light_intensity,
self.rightSensor.reflected_light_intensity,
condLeftCounter, condRightCounter, caller),
file=sys.stderr)
sleep(sleepTime)
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def onUntilWhiteLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilTwoConditions(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, lambda: self.rightSensor.reflected_light_intensity
> white_threshold, "onUntilWhiteLine", speed, consecutiveHit,
sleepTime)
def onUntilBlackLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilTwoConditions(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, lambda: self.rightSensor.reflected_light_intensity
< black_threshold, "onUntilBlackLine", speed, consecutiveHit,
sleepTime)
# run until find a game line
def onUntilGameLine(self,
consecutiveHit=5,
speed=5,
sleepTime=0.01,
white_threshold=85,
black_threshold=30):
self.onUntilWhiteLine(consecutiveHit, speed, sleepTime,
white_threshold)
self.onUntilBlackLine(consecutiveHit, speed, sleepTime,
black_threshold)
# run until condition is met
def onUntilCondition(self,
condition,
caller,
leftSpeed=5,
rightSpeed=5,
consecutiveHit=5,
sleepTime=0.01,
revert=False):
# Start motor at passed speonUntilTwoConditionsed.
self.leftLargeMotor.on(-leftSpeed if revert == False else leftSpeed)
self.rightLargeMotor.on(-rightSpeed if revert == False else rightSpeed)
counter = 0
condMet = False
while (not condMet):
# check condition
if (condition()):
counter += 1
else:
counter = 0
# reset to zero
if (counter >= consecutiveHit):
self.leftLargeMotor.off()
self.rightLargeMotor.off()
condMet = True
print(
"{4}-onUntilCondition: ColorSensor(left_reflected: {0:3d}, right_reflected: {1:3d}, hit: {2:3d}), UltraSonicSensor(distance_centimeters: {3:3f})"
.format(self.leftSensor.reflected_light_intensity,
self.rightSensor.reflected_light_intensity, counter,
self.ultraSonicSensor.distance_centimeters, caller),
file=sys.stderr)
sleep(sleepTime)
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def onUntilLeftBlack(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, "onUntilLeftBlack", speed, speed, consecutiveHit,
sleepTime)
def onUntilLeftWhite(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, "onUntilLeftWhite", speed, speed, consecutiveHit,
sleepTime)
def onUntilRightBlack(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
black_threshold=30):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity <
black_threshold, "onUntilRightBlack", speed, speed, consecutiveHit,
sleepTime)
def onUntilRightWhite(self,
speed=5,
consecutiveHit=5,
sleepTime=0.01,
white_threshold=85):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity >
white_threshold, "onUntilRightWhite", speed, speed, consecutiveHit,
sleepTime)
def turnUntilLeftBlack(self,
turnLeft,
speed,
consecutiveHit=2,
black_threshold=30):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity <
black_threshold, "turnUntilLeftBlack",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilLeftWhite(self,
turnLeft,
speed,
consecutiveHit=2,
white_threshold=85):
self.onUntilCondition(
lambda: self.leftSensor.reflected_light_intensity >
white_threshold, "turnUntilLeftWhite",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilRightBlack(self,
turnLeft,
speed,
consecutiveHit=2,
black_threshold=30):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity <
black_threshold, "turnUntilRightBlack",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
def turnUntilRightWhite(self,
turnLeft,
speed,
consecutiveHit=2,
white_threshold=85):
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity >
white_threshold, "turnUntilRightWhite",
0 if turnLeft == True else speed, speed if turnLeft == True else 0,
consecutiveHit)
# Go until sensor reading has a specified offset or reach to the threshhold
def onUntilRightDarkerBy(self,
difference,
black_threshold=20,
speed=10,
consecutiveHit=2):
originalValue = self.rightSensor.reflected_light_intensity
print("onUntilRightDarkerBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity - originalValue
< -difference or self.rightSensor.reflected_light_intensity <
black_threshold,
"onUntilRightSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilRightLighterBy(self,
difference,
white_threshold=80,
speed=10,
consecutiveHit=2):
originalValue = self.rightSensor.reflected_light_intensity
print("onUntilRightLighterBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(
lambda: self.rightSensor.reflected_light_intensity - originalValue
> difference or self.rightSensor.reflected_light_intensity >
white_threshold,
"onUntilRightSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilLeftDarkerBy(self,
difference,
black_threshold=20,
speed=10,
consecutiveHit=2):
originalValue = self.leftSensor.reflected_light_intensity
print(
"onUntilLeftDarkerBy - originalValue: {0:3d}, diff: {1:3d}".format(
originalValue, difference),
file=sys.stderr)
self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity
- originalValue < -difference or self.leftSensor.
reflected_light_intensity < black_threshold,
"onUntilLeftSensorDiff",
consecutiveHit=consecutiveHit)
def onUntilLeftLighterBy(self,
difference,
white_threshold=80,
speed=10,
consecutiveHit=2):
originalValue = self.leftSensor.reflected_light_intensity
print("onUntilLeftLighterBy - originalValue: {0:3d}, diff: {1:3d}".
format(originalValue, difference),
file=sys.stderr)
self.onUntilCondition(lambda: self.leftSensor.reflected_light_intensity
- originalValue > difference or self.leftSensor.
reflected_light_intensity > white_threshold,
"onUntilLeftSensorDiff",
consecutiveHit=consecutiveHit)
#uses Ultrasonic sensor to see wall as going back
def revertSafely(self,
speed=100,
distanceToStop=10,
consecutiveHit=1,
sleepTime=0.01):
self.onUntilCondition(
lambda: self.ultraSonicSensor.distance_centimeters <
distanceToStop, "revertSafely", speed, speed, consecutiveHit,
sleepTime, True)
# Calibrating White for Sensor
def calibrateColorSensor(self, sensorInput):
sensor = ColorSensor(sensorInput)
# Calibration
sensor.calibrate_white()
# Done Signal
sound.beep()
# Calibrating Color for Sensor
def testColorSensor(self,
sensorInput,
sensorPort,
repeatNumber=10,
pauseNumber=0.2,
speed=0):
sensor = ColorSensor(sensorInput)
if (speed > 0):
self.leftLargeMotor.on(speed)
self.rightLargeMotor.on(speed)
times = 0
# For loop
while times != repeatNumber:
# Print
print("testColorSensor- Port: {0:3d}: {1:3d}".format(
sensorPort, sensor.reflected_light_intensity),
file=sys.stderr)
time.sleep(pauseNumber)
times = times + 1
self.leftLargeMotor.off()
self.rightLargeMotor.off()
def resetMediumMotor():
self.moveMediumMotor(isLeft=False, speed=50, degrees=1000)
self.rightMediumMotor.reset()
def testRobot(self):
self.leftLargeMotor.on_for_degrees(20, 180)
sleep(.1)
self.rightLargeMotor.on_for_degrees(20, 180)
sleep(.1)
self.moveMediumMotor(True, 10, 180)
sleep(.1)
self.moveMediumMotor(False, 10, 180)
sleep(.1)
self.run(20, 10)
sleep(.1)
self.run(-20, 10)
sleep(.1)
self.turn(90)
sleep(.1)
self.turn(-90)
sleep(.1)
self.turnOnLeftWheel(90)
sleep(.1)
self.turnOnLeftWheel(-90)
sleep(.1)
self.turnOnRightWheel(90)
sleep(.1)
self.turnOnRightWheel(-90)
sleep(.1)
self.calibrateColorSensor(INPUT_1)
self.calibrateColorSensor(INPUT_4)
self.testColorSensor(INPUT_1, 1)
self.testColorSensor(INPUT_4, 4)
class Catapult(IRBeaconRemoteControlledTank):
def __init__(self,
left_motor_port: str = OUTPUT_B,
right_motor_port: str = OUTPUT_C,
catapult_motor_port: str = OUTPUT_A,
touch_sensor_port: str = INPUT_1,
color_sensor_port: str = INPUT_3,
ir_sensor_port: str = INPUT_4,
ir_beacon_channel: int = 1):
super().__init__(left_motor_port=left_motor_port,
right_motor_port=right_motor_port,
ir_sensor_port=ir_sensor_port,
ir_beacon_channel=ir_beacon_channel)
self.catapult_motor = MediumMotor(address=catapult_motor_port)
self.touch_sensor = TouchSensor(address=touch_sensor_port)
self.color_sensor = ColorSensor(address=color_sensor_port)
self.ir_sensor = InfraredSensor(address=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
self.speaker = Sound()
def scan_colors(self):
while True:
if self.color_sensor.color == ColorSensor.COLOR_YELLOW:
pass
elif self.color_sensor.color == ColorSensor.COLOR_WHITE:
self.speaker.play_file(wav_file='/home/robot/sound/Good.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
def make_noise_when_touched(self):
while True:
if self.touch_sensor.is_pressed:
self.speaker.play_file(wav_file='/home/robot/sound/Ouch.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
def throw_by_ir_beacon(self):
while True:
if self.ir_sensor.beacon(channel=self.ir_beacon_channel):
self.catapult_motor.on_for_degrees(speed=-100,
degrees=150,
brake=True,
block=True)
self.catapult_motor.on_for_degrees(speed=100,
degrees=150,
brake=True,
block=True)
while self.ir_sensor.beacon(channel=self.ir_beacon_channel):
pass
def main(self):
self.speaker.play_file(wav_file='/home/robot/sound/Yes.wav',
volume=100,
play_type=Sound.PLAY_WAIT_FOR_COMPLETE)
Thread(target=self.make_noise_when_touched, daemon=True).start()
Thread(target=self.throw_by_ir_beacon, daemon=True).start()
Thread(target=self.scan_colors, daemon=True).start()
self.keep_driving_by_ir_beacon(speed=100)
#!/usr/bin/env micropython from ev3dev2.motor import OUTPUT_C, OUTPUT_B, OUTPUT_A, OUTPUT_D, MoveSteering, LargeMotor, MoveTank, MediumMotor from ev3dev2.motor import SpeedDPS, SpeedRPS, SpeedRPM from ev3dev2.sensor.lego import GyroSensor from sys import stderr from ev3_robot import Ev3Robot robot = Ev3Robot() robot.read_calibration() tank_pair = MoveTank(OUTPUT_C, OUTPUT_B) steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) medium_motorA = MediumMotor(OUTPUT_A) medium_motorD = MediumMotor(OUTPUT_D) # Mission 4 medium_motorA.on_for_degrees(degrees = 135, speed = -30) robot.go_straight_forward(76) medium_motorD.on_for_degrees(degrees = 360, speed = -100) robot.spin_right(5) robot.go_straight_backward(cm = 4, speed = -20) medium_motorA.on_for_degrees(degrees = 135, speed = 30)
#!/usr/bin/python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank, MediumMotor
from ev3dev2.led import Leds
from ev3dev2.button import Button
from ev3dev2.sound import Sound
from time import sleep
import os
os.system('setfont Lat15-TerminusBold14')
penLift = MediumMotor(OUTPUT_C)
penMove = LargeMotor(OUTPUT_A)
btn = Button()
penLift.on_for_degrees(speed=40, degrees=-850)
penMove.on_for_degrees(speed=20, degrees=500)
sleep(2)
while True:
penMove.on_for_degrees(speed=20, degrees=-1000)
penLift.on_for_degrees(speed=-20, degrees=10)
if btn.enter:
penMove.on_for_degrees(speed=20, degrees=500)
penLift.on_for_degrees(speed=20, degrees=900)
break
penMove.on_for_degrees(speed=20, degrees=1000)
penLift.on_for_degrees(speed=-20, degrees=10)
if btn.enter:
penMove.on_for_degrees(speed=20, degrees=-500)
penLift.on_for_degrees(speed=20, degrees=900)
break
from ev3dev2.motor import (MoveSteering, MediumMotor, OUTPUT_A, OUTPUT_B,
OUTPUT_C)
from ev3dev2.sensor.lego import UltrasonicSensor
from time import sleep
motor_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
medium_motor = MediumMotor(OUTPUT_A)
ultrasonic_sensor = UltrasonicSensor()
# Start robot moving forward
motor_pair.on(steering=0, speed=10)
# Wait until robot less than 3.5cm from cuboid
while ultrasonic_sensor.distance_centimeters > 3.5:
sleep(0.01)
# Stop moving forward
motor_pair.off()
# Lower robot arm over cuboid
medium_motor.on_for_degrees(speed=-10, degrees=90)
# Drag cuboid backwards for 2 seconds
motor_pair.on_for_seconds(steering=0, speed=-20, seconds=2)
# Raise robot arm
medium_motor.on_for_degrees(speed=10, degrees=90)
# Move robot away from cuboid
motor_pair.on_for_seconds(steering=0, speed=-20, seconds=2)
