# ------Input-------- print('Setting input values') power = 40 target = 55 kp = 1 # Proportional gain. Start value 1, float(0.65) kd = 0 # Derivative gain. Start value 0 ,float(0.02) ki = 0 #float(0.01) # Integral gain. Start value 0 direction = -1 minRef = 3 maxRef = 26 # ------------------- # Connect two large motors on output ports B and C and check that # the device is connected using the 'connected' property. print('Connecting motors') left_motor = LargeMotor(OUTPUT_B) right_motor = LargeMotor(OUTPUT_A) # One left and one right motor should be connected # Connect color and touch sensors and check that they # are connected. print('Connecting sensors') #ir = InfraredSensor(); assert ir.connected col = ColorSensor(INPUT_2) us = UltrasonicSensor(INPUT_1) # Change color sensor mode print('Setting color sensor mode') col.mode = 'COL-REFLECT'
from ev3dev2.sensor.lego import TouchSensor from ev3dev2.led import Leds from ev3dev2.sensor.lego import ColorSensor, LightSensor '''Kp = 520 Ki = 0 Kd = 0 offset = 20 Tp = 30 integral = 0 lastError = 0 dervitave = 0''' lcolor1 = LightSensor(INPUT_2) lcolor2 = LightSensor(INPUT_3) #ccolor = ColorSensor(INPUT_1) mr = LargeMotor(OUTPUT_A) ml = LargeMotor(OUTPUT_B) a = .175 def kalibrierung(a, l1, l2): return (l2 - l1) * a d = kalibrierung(a, lcolor1.reflected_light_intensity, lcolor2.reflected_light_intensity) def firstwhile(): global switch1, switch2 lcolor1 = LightSensor(INPUT_2) lcolor2 = LightSensor(INPUT_3) Kp = 140 Ki = 0 Kd = 0 offset = d
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_A, SpeedPercent m = LargeMotor(OUTPUT_A) m.on_for_rotations(SpeedPercent(75), 5)
#!/usr/bin/env python3 # https://github.com/ev3dev/ev3dev-lang-python # https://github.com/ev3dev/ev3dev-lang-python-demo#balanc3r from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, MoveTank from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.sensor.lego import TouchSensor, ColorSensor, GyroSensor, UltrasonicSensor from ev3dev2.led import Leds from ev3dev2.sound import Sound import math import time # Motors l_motor = LargeMotor(OUTPUT_D) r_motor = LargeMotor(OUTPUT_A) # Sensors color = ColorSensor(INPUT_1) gyro = GyroSensor(INPUT_2) ts = TouchSensor(INPUT_3) us = UltrasonicSensor(INPUT_4) # Sound sound = Sound() # sound.speak('Welcome to the E V 3 dev project!') def forward(speed):
def moveGreenPlayer( inDistance ): global GreenCurrent LargeMotor( OUTPUT_B ).on_for_degrees( SpeedPercent( MotorSpeed ), inDistance, True, False ) GreenCurrent += inDistance
def moveYellowPlayer( inDistance ): global YellowCurrent LargeMotor( OUTPUT_D ).on_for_degrees( SpeedPercent( MotorSpeed ), -inDistance, True, False ) YellowCurrent += inDistance
def do_GET(self): print(self.path) if self.path.startswith("/ev3/"): url = urllib.parse.urlparse(self.path) path = url.path.split("/") params = urllib.parse.parse_qs(url.query) if path[2] == "sensor": portN = params['portName'][0] if portN == '1': portN = INPUT_1 elif portN == '2': portN = INPUT_2 elif portN == '3': portN = INPUT_3 elif portN == '4': portN = INPUT_4 type = params['type'][0] if type == 'touch': ts = TouchSensor(portN) return self.send_result(ts.is_pressed) if type == 'light': ts = ColorSensor(portN) return self.send_result(ts.reflected_light_intensity) if type == 'color': ts = ColorSensor(portN) return self.send_result(ts.color) if type == 'gyro': ts = GyroSensor(portN) return self.send_result(ts.angle) if type == 'ultra': ts = UltrasonicSensor(portN) return self.send_result(ts.distance_centimeters) if path[2] == "tank": port1 = params['port1'][0] if port1 == 'A': port1 = OUTPUT_A if port1 == 'B': port1 = OUTPUT_B if port1 == 'C': port1 = OUTPUT_C if port1 == 'D': port1 = OUTPUT_D port2 = params['port2'][0] if port2 == 'A': port2 = OUTPUT_A if port2 == 'B': port2 = OUTPUT_B if port2 == 'C': port2 = OUTPUT_C if port2 == 'D': port2 = OUTPUT_D motor = MoveTank(port1, port2) rotations = float(params['rotations'][0]) speed1 = float(params['speed1'][0]) speed2 = float(params['speed2'][0]) motor.on_for_rotations(speed1, speed2, rotations) return self.send_result("OK") if path[2] == "motor": portN = params['portName'][0] if portN == 'A': portN = OUTPUT_A if portN == 'B': portN = OUTPUT_B if portN == 'C': portN = OUTPUT_C if portN == 'D': portN = OUTPUT_D type = params['type'][0] print(params) if type == 'large': motor = LargeMotor(portN) if 'seconds' in params and 'speed' in params: seconds = float(params['seconds'][0]) speed = float(params['speed'][0]) motor.on_for_seconds(SpeedPercent(speed), seconds) return self.send_result("OK") if 'rotations' in params and 'speed' in params: rotations = float(params['rotations'][0]) speed = float(params['speed'][0]) motor.on_for_rotations(SpeedPercent(speed), rotations) return self.send_result("OK") if 'run' in params and 'speed' in params: speed = float(params['speed'][0]) if params['run'][0] == 'true': motor.on(SpeedPercent(speed)) else: motor.off() return self.send_result("OK") if type == 'medium': motor = MediumMotor(portN) if 'seconds' in params and 'speed' in params: seconds = float(params['seconds'][0]) speed = float(params['speed'][0]) motor.on_for_seconds(SpeedPercent(speed), seconds) return self.send_result("OK") if 'rotations' in params and 'speed' in params: rotations = float(params['rotations'][0]) speed = float(params['speed'][0]) motor.on_for_rotations(SpeedPercent(speed), rotations) return self.send_result("OK") return self.send_result("ERROR") if self.path == "/": self.path = "snap.html" SimpleHTTPRequestHandler.do_GET(self)
from ev3dev2.motor import LargeMotor,MediumMotor, OUTPUT_D, OUTPUT_A, OUTPUT_C, OUTPUT_B, follow_for_ms from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM, MoveTank, MoveSteering, SpeedPercent from ev3dev2.sound import Sound from time import sleep from ev3dev2.sensor.lego import ColorSensor from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.sound import Sound import math import sys import time LeftAction = MediumMotor(OUTPUT_A) RightAction = MediumMotor(OUTPUT_D) TankPair = MoveTank(OUTPUT_B, OUTPUT_C, motor_class=LargeMotor) LeftSensor = ColorSensor(INPUT_1) RightSensor = ColorSensor(INPUT_4) LeftWheel = LargeMotor(OUTPUT_B) RightWheel = LargeMotor(OUTPUT_C) sound = Sound() def LineFollowing(FastSpeed,SlowSpeed,Degree): DegreeSum = 0 AngleOld = 360 * RightWheel.position / RightWheel.count_per_rot while DegreeSum < Degree: if RightSensor.color == ColorSensor.COLOR_WHITE: LeftWheel.on(SpeedDPS(FastSpeed)) RightWheel.on(SpeedDPS(SlowSpeed)) else: RightWheel.on(SpeedDPS(FastSpeed)) LeftWheel.on(SpeedDPS(SlowSpeed)) AngleNew = 360 * RightWheel.position / RightWheel.count_per_rot DegreeSum = DegreeSum + abs(AngleNew - AngleOld)
# some keyboard control setup and vars tty.setcbreak(sys.stdin) x = 0 # set here to make global pilot_mode = 1 # start in human mode, '1'; '-1' means auto-pilot # some Nav control vars initSearch = False # have we completed the initial search circle, looking for ir beacon? beaconLock = False # has the beacon been found and data read? cmpGenHeading = False # do we have a lock on compass heading for North? beaconSearch = False # are we searching for the beacon right now? cmpSearch = False # are we searching for North with the compass right now? navPrint = 10 # print the Nav msgs every nth cycle i = 0 # iterator for testing # setup motors mL = LargeMotor(OUTPUT_A) time.sleep(0.5) mL.reset() time.sleep(2) mLspd = 0 mL.stop_action = 'coast' mL.polarity = 'inversed' mL.position = 0 mR = LargeMotor(OUTPUT_B) time.sleep(0.5) mR.reset() time.sleep(2) mRspd = 0 mR.stop_action = 'coast'
from time import sleep import math ent_motor_medio = OUTPUT_A ent_motor_grande = OUTPUT_B ent_motor_esq = OUTPUT_C ent_motor_dir = OUTPUT_D ent_us_fr = INPUT_1 ent_us_lat = INPUT_2 ent_sc_esq = INPUT_3 ent_sc_dir = INPUT_4 steering_pair = MoveSteering(ent_motor_esq, ent_motor_dir) elevador_g = LargeMotor(ent_motor_grande) garra_m = MediumMotor(ent_motor_medio) # elevador_g.on_for_degrees(40, -180) # negativo sobe # garra_m.on_for_degrees(40, 96) # positivo abre cor_esq = ColorSensor(ent_sc_esq) cor_dir = ColorSensor(ent_sc_dir) us_lat = UltrasonicSensor(ent_us_lat) us_front = UltrasonicSensor(ent_us_fr) sound = Sound() btn = Button() tam_cano = 0
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D import time import threading armSpeed = 22 leftArm = -21.5 rightArm = 21.5 portA = LargeMotor(OUTPUT_A) portB = LargeMotor(OUTPUT_B) portC = LargeMotor(OUTPUT_C) portD = LargeMotor(OUTPUT_D) # C portA.on_for_degrees(armSpeed, rightArm) portA.on_for_degrees(armSpeed, leftArm) # D portA.on_for_degrees(armSpeed, leftArm) portA.on_for_degrees(armSpeed, rightArm) # E portB.on_for_degrees(armSpeed, rightArm) portB.on_for_degrees(armSpeed, leftArm) # F portB.on_for_degrees(armSpeed, leftArm) portB.on_for_degrees(armSpeed, rightArm)
#!/usr/bin/env python3 #Codigo que faz uma curva p/ direita, segue reto, e faz uma curva p/ esquerda #importacao da biblioteca de motores from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, MoveTank, SpeedPercent #definicoes tank = MoveTank(OUTPUT_A, OUTPUT_B) motorA = LargeMotor(OUTPUT_A) motorB = LargeMotor(OUTPUT_B) motorC = MediumMotor(OUTPUT_C) #testes p/ conseguir uma corva de 90* com 1motor + motor C #90* p/ direita def dir90(): motorC.on_for_degrees(SpeedPercent(50), 25, block=True) motorA.on_for_rotations(SpeedPercent(100), 3.8) motorA.stop() motorC.on_for_degrees(SpeedPercent(-30), 25) #90* p/ esquerda def esq90(): motorC.on_for_degrees(SpeedPercent(-50), 25, block=True) motorB.on_for_rotations(SpeedPercent(100), 3.8) motorB.stop() motorC.on_for_degrees(SpeedPercent(30), 25) #Execucao: dir90() #curva p/direita
#!/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 mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM) medium_motorA.on_for_degrees(speed=10, degrees=-110) medium_motorA.on_for_degrees(speed=10, degrees=110)
import os.path # DEFINIÇÕES ent_motor_esq = OUTPUT_C ent_motor_dir = OUTPUT_D ent_motor_grande = OUTPUT_B ent_motor_medio = OUTPUT_A ent_sc_esq = INPUT_3 ent_sc_dir = INPUT_4 ent_us_lat = INPUT_2 ent_us_fr = INPUT_1 steering_pair = MoveSteering(ent_motor_esq, ent_motor_dir) garra_g = LargeMotor(ent_motor_grande) garra_m = MediumMotor(ent_motor_medio) cor_esq = ColorSensor(ent_sc_esq) cor_dir = ColorSensor(ent_sc_dir) usl = UltrasonicSensor(ent_us_lat) usf = UltrasonicSensor(ent_us_fr) sound = Sound() btn = Button() # FUNÇÕES def distancia(sensor): a1 = sensor.distance_centimeters sleep(0.1)
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) MB.stop(stop_action="coast") MC.stop(stop_action="coast") Launchrun()
def run(self): # sensors cs = ColorSensor() cs.mode = 'COL-REFLECT' # measure light intensity # motors lm = LargeMotor('outC') rm = LargeMotor('outB') speed = 360 / 2 # deg/sec, [-1000, 1000] dt = 500 # milliseconds stop_action = "coast" # PID tuning Kp = 1 # proportional gain Ki = 0.002 # integral gain Kd = 0.01 # derivative gain integral = 0 previous_error = 0 # initial measurment target_value = 35 # Start the main loop while not self.shut_down: # Calculate steering using PID algorithm error = target_value - cs.value() if previous_error > 0 and error < 0: integral = 0 integral += (error * dt) derivative = (error - previous_error) / dt # u zero: on target, drive forward # u positive: too bright, turn right # u negative: too dark, turn left u = (Kp * error) + (Ki * integral) + (Kd * derivative) # limit u to safe values: [-1000, 1000] deg/sec if speed + abs(u) > 1000: if u >= 0: u = 1000 - speed else: u = speed - 1000 # run motors if u >= 0: lm.run_timed(time_sp=dt, speed_sp=speed + abs(u), stop_action=stop_action) rm.run_timed(time_sp=dt, speed_sp=0, stop_action=stop_action) sleep(dt / 2000) else: lm.run_timed(time_sp=dt, speed_sp=0, stop_action=stop_action) rm.run_timed(time_sp=dt, speed_sp=speed + abs(u), stop_action=stop_action) sleep(dt / 2000) previous_error = error
def Traffic(): Sound.speak("").wait() MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") T1 = TouchSensor("") 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 left_wheel_speed = 100 right_wheel_speed = 100 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 tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 0.675) # Originally 0.95 - the robot starts out from base while GY.value() < 85: #gyro turns 85 degrees and faces towards the swing 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") while MB.position > -1600: # this is the gyro program, the first line tells the bot to continue loop until it reaches a defined position if GY.value() == 90: #this runs if the gyro is OK and already straight, sets a lot of variables as well 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 = 8 gyro_correct_loops = 0 gyro_correct_straight = 0 straight_correct_loops = 0 else: #if the gyro is off it runs this section of code 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 #changes the wheel speeds accordingly 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: # 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: #runs only if bot isn't already on the original line it started from while straight_correct_loops < gyro_correct_loops: #Basically this messes up the gyro again so the bot can correct back to the line it was orignally on 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 #sets this to 1 so that it doesn't go off the line again gyro_correct_loops = 0 straight_correct_loops = 0 else: correct_rate = abs (GY.value()) # Same idea as the other if statement, just reversed 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 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: while straight_correct_loops < gyro_correct_loops: 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 gyro_correct_loops = 0 straight_correct_loops = 0 # stop all motors MB.stop(stop_action="hold") MC.stop(stop_action="hold") while GY.value() > 19: #this turns the bot towards the circle, need to be less than straight 90 degrees 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") tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1.35) #goes forward (2.5) slightly to move the block into tan tank_drive.on_for_rotations(SpeedPercent(30), SpeedPercent(30), 0.7) #drives back a bit (1) while GY.value() < 145: #turns to face the bridge backwards left_wheel_speed = 100 #originaly 200 right_wheel_speed = -100 #originaly 200 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") # Drives up bridge tank_drive.on_for_rotations(SpeedPercent(70), SpeedPercent(70), 3) #Onto the bridge!! while True: MB.stop(stop_action="hold") MC.stop(stop_action="hold")
def moveRedPlayer( inDistance ): global RedCurrent LargeMotor( OUTPUT_A ).on_for_degrees( SpeedPercent( MotorSpeed ), -inDistance, True, False ) RedCurrent += inDistance
#!/usr/bin/env micropython from ev3dev2.motor import LargeMotor, OUTPUT_B LARGE_MOTOR = LargeMotor(address=OUTPUT_B) LARGE_MOTOR.on_for_seconds(speed=50.0, seconds=4.0, block=True, brake=True) LARGE_MOTOR.on_for_seconds(speed=100.0, seconds=4.0, block=True, brake=True) LARGE_MOTOR.on_for_seconds(speed=-100.0, seconds=4.0, block=True, brake=True) LARGE_MOTOR.on_for_rotations(speed=100.0, rotations=5.0, block=True, brake=True)
def moveBluePlayer( inDistance ): global BlueCurrent LargeMotor( OUTPUT_C ).on_for_degrees( SpeedPercent( MotorSpeed ), inDistance, True, False ) BlueCurrent += inDistance
# Make sure the same address is set in Pixy2 PIXY2_I2C_ADDRESS = 0x54 # Signature of the ball configured in PixyMon BALL_SIGNATURE = 1 # Data for requesting block GET_OBJECTS_COMMAND = [174, 193, 32, 2, BALL_SIGNATURE, 1] # The y position of the ball when it reaches the goal - I found this value by opening PixyMon and hovering # my mouse over the goalie line on the board. PixyMon shows the coordinates of the mouse, so I used # the y-position of the mouse as Ye. Ye = 190 # Sets the lego port of the large motor to port C lm = LargeMotor(OUTPUT_C) #Where the robot starts BALL_START = 170 # Set LEGO port for Pixy2 on input port 1 in1 = LegoPort(INPUT_1) in1.mode = 'other-i2c' # Short wait for the port to get ready time.sleep(0.5) # Settings for I2C (SMBus(3) for INPUT_1) bus = SMBus(3)
class MindCuber(object): scan_order = [ 5, 9, 6, 3, 2, 1, 4, 7, 8, 23, 27, 24, 21, 20, 19, 22, 25, 26, 50, 54, 51, 48, 47, 46, 49, 52, 53, 14, 10, 13, 16, 17, 18, 15, 12, 11, 41, 43, 44, 45, 42, 39, 38, 37, 40, 32, 34, 35, 36, 33, 30, 29, 28, 31 ] hold_cube_pos = 85 rotate_speed = 400 flip_speed = 300 flip_speed_push = 400 def __init__(self): self.shutdown = False self.flipper = LargeMotor(address=OUTPUT_A) self.turntable = LargeMotor(address=OUTPUT_B) self.colorarm = MediumMotor(address=OUTPUT_C) self.color_sensor = ColorSensor() self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW self.infrared_sensor = InfraredSensor() self.init_motors() self.state = ['U', 'D', 'F', 'L', 'B', 'R'] self.rgb_solver = None signal.signal(signal.SIGTERM, self.signal_term_handler) signal.signal(signal.SIGINT, self.signal_int_handler) filename_max_rgb = 'max_rgb.txt' if os.path.exists(filename_max_rgb): with open(filename_max_rgb, 'r') as fh: for line in fh: (color, value) = line.strip().split() if color == 'red': self.color_sensor.red_max = int(value) log.info("red max is %d" % self.color_sensor.red_max) elif color == 'green': self.color_sensor.green_max = int(value) log.info("green max is %d" % self.color_sensor.green_max) elif color == 'blue': self.color_sensor.blue_max = int(value) log.info("blue max is %d" % self.color_sensor.blue_max) def init_motors(self): for x in (self.flipper, self.turntable, self.colorarm): x.reset() log.info("Initialize flipper %s" % self.flipper) self.flipper.on(SpeedDPS(-50), block=True) self.flipper.off() self.flipper.reset() log.info("Initialize colorarm %s" % self.colorarm) self.colorarm.on(SpeedDPS(500), block=True) self.colorarm.off() self.colorarm.reset() log.info("Initialize turntable %s" % self.turntable) self.turntable.off() self.turntable.reset() def shutdown_robot(self): log.info('Shutting down') self.shutdown = True if self.rgb_solver: self.rgb_solver.shutdown = True for x in (self.flipper, self.turntable, self.colorarm): # We are shutting down so do not 'hold' the motors x.stop_action = 'brake' x.off(False) def signal_term_handler(self, signal, frame): log.error('Caught SIGTERM') self.shutdown_robot() def signal_int_handler(self, signal, frame): log.error('Caught SIGINT') self.shutdown_robot() def apply_transformation(self, transformation): self.state = [self.state[t] for t in transformation] def rotate_cube(self, direction, nb): current_pos = self.turntable.position final_pos = 135 * round( (self.turntable.position + (270 * direction * nb)) / 135.0) log.info( "rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" % (direction, nb, current_pos, final_pos)) if self.flipper.position > 35: self.flipper_away() self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed), final_pos) if nb >= 1: for i in range(nb): if direction > 0: transformation = [0, 1, 5, 2, 3, 4] else: transformation = [0, 1, 3, 4, 5, 2] self.apply_transformation(transformation) def rotate_cube_1(self): self.rotate_cube(1, 1) def rotate_cube_2(self): self.rotate_cube(1, 2) def rotate_cube_3(self): self.rotate_cube(-1, 1) def rotate_cube_blocked(self, direction, nb): # Move the arm down to hold the cube in place self.flipper_hold_cube() # OVERROTATE depends on lot on MindCuber.rotate_speed current_pos = self.turntable.position OVERROTATE = 18 final_pos = int(135 * round( (current_pos + (270 * direction * nb)) / 135.0)) temp_pos = int(final_pos + (OVERROTATE * direction)) log.info( "rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s" % (direction, nb, current_pos, temp_pos, final_pos)) self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed), temp_pos) self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4), final_pos) def rotate_cube_blocked_1(self): self.rotate_cube_blocked(1, 1) def rotate_cube_blocked_2(self): self.rotate_cube_blocked(1, 2) def rotate_cube_blocked_3(self): self.rotate_cube_blocked(-1, 1) def flipper_hold_cube(self, speed=300): current_position = self.flipper.position # Push it forward so the cube is always in the same position # when we start the flip if (current_position <= MindCuber.hold_cube_pos - 10 or current_position >= MindCuber.hold_cube_pos + 10): self.flipper.ramp_down_sp = 400 self.flipper.on_to_position(SpeedDPS(speed), MindCuber.hold_cube_pos) sleep(0.05) def flipper_away(self, speed=300): """ Move the flipper arm out of the way """ log.info("flipper_away()") self.flipper.ramp_down_sp = 400 self.flipper.on_to_position(SpeedDPS(speed), 0) def flip(self): """ Motors will sometimes stall if you call on_to_position() multiple times back to back on the same motor. To avoid this we call a 50ms sleep in flipper_hold_cube() and after each on_to_position() below. We have to sleep after the 2nd on_to_position() because sometimes flip() is called back to back. """ log.info("flip()") if self.shutdown: return # Move the arm down to hold the cube in place self.flipper_hold_cube() # Grab the cube and pull back self.flipper.ramp_up_sp = 200 self.flipper.ramp_down_sp = 0 self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190) sleep(0.05) # At this point the cube is at an angle, push it forward to # drop it back down in the turntable self.flipper.ramp_up_sp = 200 self.flipper.ramp_down_sp = 400 self.flipper.on_to_position(SpeedDPS(self.flip_speed_push), MindCuber.hold_cube_pos) sleep(0.05) transformation = [2, 4, 1, 3, 0, 5] self.apply_transformation(transformation) def colorarm_middle(self): log.info("colorarm_middle()") self.colorarm.on_to_position(SpeedDPS(600), -750) def colorarm_corner(self, square_index): """ The lower the number the closer to the center """ log.info("colorarm_corner(%d)" % square_index) position_target = -580 if square_index == 1: position_target -= 10 elif square_index == 3: position_target -= 30 elif square_index == 5: position_target -= 20 elif square_index == 7: pass else: raise ScanError( "colorarm_corner was given unsupported square_index %d" % square_index) self.colorarm.on_to_position(SpeedDPS(600), position_target) def colorarm_edge(self, square_index): """ The lower the number the closer to the center """ log.info("colorarm_edge(%d)" % square_index) position_target = -640 if square_index == 2: position_target -= 20 elif square_index == 4: position_target -= 40 elif square_index == 6: position_target -= 20 elif square_index == 8: pass else: raise ScanError( "colorarm_edge was given unsupported square_index %d" % square_index) self.colorarm.on_to_position(SpeedDPS(600), position_target) def colorarm_remove(self): log.info("colorarm_remove()") self.colorarm.on_to_position(SpeedDPS(600), 0) def colorarm_remove_halfway(self): log.info("colorarm_remove_halfway()") self.colorarm.on_to_position(SpeedDPS(600), -400) def scan_face(self, face_number): log.info("scan_face() %d/6" % face_number) if self.shutdown: return if self.flipper.position > 35: self.flipper_away(100) self.colorarm_middle() self.colors[int(MindCuber.scan_order[self.k])] = self.color_sensor.rgb self.k += 1 i = 1 target_pos = 115 self.colorarm_corner(i) # The gear ratio is 3:1 so 1080 is one full rotation self.turntable.reset() self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed), 1080, block=False) self.turntable.wait_until('running') while True: # 135 is 1/8 of full rotation if self.turntable.position >= target_pos: current_color = self.color_sensor.rgb self.colors[int(MindCuber.scan_order[self.k])] = current_color i += 1 self.k += 1 if i == 9: # Last face, move the color arm all the way out of the way if face_number == 6: self.colorarm_remove() # Move the color arm far enough away so that the flipper # arm doesn't hit it else: self.colorarm_remove_halfway() break elif i % 2: self.colorarm_corner(i) if i == 1: target_pos = 115 elif i == 3: target_pos = 380 else: target_pos = i * 135 else: self.colorarm_edge(i) if i == 2: target_pos = 220 elif i == 8: target_pos = 1060 else: target_pos = i * 135 if self.shutdown: return if i < 9: raise ScanError('i is %d..should be 9' % i) self.turntable.wait_until_not_moving() self.turntable.off() self.turntable.reset() log.info("\n") def scan(self): log.info("scan()") self.colors = {} self.k = 0 self.scan_face(1) self.flip() self.scan_face(2) self.flip() self.scan_face(3) self.rotate_cube(-1, 1) self.flip() self.scan_face(4) self.rotate_cube(1, 1) self.flip() self.scan_face(5) self.flip() self.scan_face(6) if self.shutdown: return log.info("RGB json:\n%s\n" % json.dumps(self.colors)) self.rgb_solver = RubiksColorSolverGeneric(3) self.rgb_solver.enter_scan_data(self.colors) self.rgb_solver.crunch_colors() self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict() log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba)) # This is only used if you want to rotate the cube so U is on top, F is # in the front, etc. You would do this if you were troubleshooting color # detection and you want to pause to compare the color pattern on the # cube vs. what we think the color pattern is. ''' log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier") self.rotate_cube(-1, 1) self.flip() self.flipper_away() self.rotate_cube(1, 1) input('Paused') ''' def move(self, face_down): log.info("move() face_down %s" % face_down) position = self.state.index(face_down) actions = { 0: ["flip", "flip"], 1: [], 2: ["rotate_cube_2", "flip"], 3: ["rotate_cube_1", "flip"], 4: ["flip"], 5: ["rotate_cube_3", "flip"] }.get(position, None) for a in actions: if self.shutdown: break getattr(self, a)() def run_kociemba_actions(self, actions): log.info('Action (kociemba): %s' % ' '.join(actions)) total_actions = len(actions) for (i, a) in enumerate(actions): if self.shutdown: break if a.endswith("'"): face_down = list(a)[0] rotation_dir = 1 elif a.endswith("2"): face_down = list(a)[0] rotation_dir = 2 else: face_down = a rotation_dir = 3 log.info("Move %d/%d: %s%s (a %s)" % (i, total_actions, face_down, rotation_dir, pformat(a))) self.move(face_down) if rotation_dir == 1: self.rotate_cube_blocked_1() elif rotation_dir == 2: self.rotate_cube_blocked_2() elif rotation_dir == 3: self.rotate_cube_blocked_3() log.info("\n") def resolve(self): if self.shutdown: return cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))] output = check_output(cmd).decode('ascii') if 'ERROR' in output: msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd), output) log.error(msg) print(msg) sys.exit(1) actions = output.strip().split() self.run_kociemba_actions(actions) self.cube_done() def cube_done(self): self.flipper_away() def wait_for_cube_insert(self): rubiks_present = 0 rubiks_present_target = 10 log.info('wait for cube...to be inserted') while True: if self.shutdown: break dist = self.infrared_sensor.proximity # It is odd but sometimes when the cube is inserted # the IR sensor returns a value of 100...most of the # time it is just a value less than 50 if dist < 50 or dist == 100: rubiks_present += 1 log.info("wait for cube...distance %d, present for %d/%d" % (dist, rubiks_present, rubiks_present_target)) else: if rubiks_present: log.info('wait for cube...cube removed (%d)' % dist) rubiks_present = 0 if rubiks_present >= rubiks_present_target: log.info('wait for cube...cube found and stable') break time.sleep(0.1)
#!/usr/bin/env python3 import sys import math from ev3dev2.motor import LargeMotor, OUTPUT_C, OUTPUT_D, OUTPUT_B from ev3dev2.sensor.lego import UltrasonicSensor from ev3dev2.sound import Sound from ev3dev2.sensor.lego import GyroSensor import time from ev3dev2.sound import Sound m1 = LargeMotor(OUTPUT_C) m2 = LargeMotor(OUTPUT_D) m3 = LargeMotor(OUTPUT_B) us = UltrasonicSensor() sound = Sound() print(us.distance_centimeters, file=sys.stderr) a_min = m3.position + 10 m3.on(-10, block=False) while m3.position < a_min: a_min = m3.position time.sleep(0.1) m3.off() print("a_min", a_min, file=sys.stderr) a_max = m3.position - 10 m3.on(10, block=False) while m3.position > a_max:
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3 from ev3dev2.sensor.lego import TouchSensor, ColorSensor from ev3dev2.led import Leds from time import sleep # TODO: Add code here colour_s1 = ColorSensor(INPUT_1) # sensor 1 for light intensity colour_s2 = ColorSensor(INPUT_2) # sensor 2 for light intensity lm = LargeMotor(OUTPUT_B); # left motor on PORT B rm = LargeMotor(OUTPUT_C); # right motor on PORT C a = 0.6; d = a * (colour_s2.reflected_light_intensity() - colour_s1.reflected_light_intensity()) while True: if (colour_s2.reflected_light_intensity() - colour_s1.reflected_light_intensity()) > d: lm.on_for_rotations(SpeedPercent(100), 2) # maybe change LEFT AND RIGHT rm.on_for_rotations(SpeedPercent(0), 2) elif (colour_s1.reflected_light_intensity() - colour_s2.reflected_light_intensity()) > d: lm.on_for_rotations(SpeedPercent(0), 2) # maybe change LEFT AND RIGHT rm.on_for_rotations(SpeedPercent(100), 2)
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, MoveTank, MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D from ev3dev2.sensor.lego import TouchSensor, ColorSensor, GyroSensor from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 import xml.etree.ElementTree as ET import threading import time from sys import stderr gyro = GyroSensor(INPUT_1) steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C) tank_block = MoveTank(OUTPUT_B, OUTPUT_C) largeMotor_Left = LargeMotor(OUTPUT_B) largeMotor_Right = LargeMotor(OUTPUT_C) #_________________________________________________________________________________________________________________________________ def StraightGyro_current(stop, speed, rotations): print("In StraightGyro_current", file=stderr) current_degrees = largeMotor_Left.position rotations = rotations * 360 target_rotations = current_degrees + rotations target = gyro.angle current_gyro_reading = target # print("Current Gyro Reading: {}".format(current_gyro_reading)) while float( current_degrees ) < target_rotations: # if the currentm rotations is smaller than the target rotations if stop(): break
#!/usr/bin/env micropython from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_D from ev3dev2.sensor import INPUT_4 from ev3dev2.sensor.lego import InfraredSensor from ev3dev2.sound import Sound MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A) TAIL_MOTOR = LargeMotor(address=OUTPUT_B) CHEST_MOTOR = LargeMotor(address=OUTPUT_D) IR_SENSOR = InfraredSensor(address=INPUT_4) SPEAKER = Sound() def drive_once_by_ir_beacon(channel: int = 1, speed: float = 100): if IR_SENSOR.top_left(channel=channel) and IR_SENSOR.top_right( channel=channel): TAIL_MOTOR.on(speed=speed, brake=False, block=False) elif IR_SENSOR.bottom_left(channel=channel) and IR_SENSOR.bottom_right( channel=channel): TAIL_MOTOR.on(speed=-speed, brake=False, block=False) elif IR_SENSOR.top_left(channel=channel): MEDIUM_MOTOR.on(speed=-50, brake=False, block=False) TAIL_MOTOR.on(speed=speed, brake=False, block=False) elif IR_SENSOR.top_right(channel=channel):
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, MoveTank from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4 from ev3dev2.sensor.lego import TouchSensor, ColorSensor, GyroSensor, UltrasonicSensor from ev3dev2.led import Leds import time leds = Leds() try: A = LargeMotor(OUTPUT_A) except: pass try: B = LargeMotor(OUTPUT_B) except: pass try: C = LargeMotor(OUTPUT_C) except: pass try: D = LargeMotor(OUTPUT_D) except: pass try: ultra = UltrasonicSensor() except: pass try: touch = TouchSensor() except:
def main(): # start sensor and motor definitions Sound.speak("").wait() MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") GY = GyroSensor("") C3 = ColorSensor("") C4 = ColorSensor("") # end sensor and motor definitions # start calibration GY.mode = 'GYRO-ANG' GY.mode = 'GYRO-RATE' GY.mode = 'GYRO-ANG' # end calibration # The following line would be if we used tank_drive # tank_drive = MoveTank(OUTPUT_B, OUTPUT_C) # start definition of driving parameters loop_gyro = 0 starting_value = GY.value() # end definition of driving parameters # set initial speed parameters speed = 20 adjust = 1 # change 999999999999 to however you want to go while loop_gyro < 999999999999: # while Gyro value is the same as the starting value, then go straigt. while GY.value() == starting_value: left_wheel_speed = speed right_wheel_speed = speed MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) return # while greater than starting value, then go left. while GY.value() > starting_value: left_wheel_speed = speed - adjust right_wheel_speed = speed MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) return # while less than starting value, then go right. while GY.value() < starting_value: left_wheel_speed = speed + adjust right_wheel_speed = speed MB.run_forever(speed_sp=left_wheel_speed) MC.run_forever(speed_sp=right_wheel_speed) return return
def main(): MA = MediumMotor("") MB = LargeMotor("outB") MC = LargeMotor("outC") MD = MediumMotor("") 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 # change the value to how far you want the robot to go. V #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 > -2550: #was -2180, Joshua is changing it to -2500 to stay in circle better if GY.value() == 90: left_wheel_speed = -200 right_wheel_speed = -200 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 = -200 right_wheel_speed = -200 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 = -200 right_wheel_speed = -200 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") tank_drive.on_for_seconds(SpeedPercent(-100), SpeedPercent(-100), 1) #Pulling away from the Red circle and driving into home. V tank_drive.on_for_rotations(SpeedPercent(65), SpeedPercent(65), 9)
from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank from enum import Enum from time import sleep import threading import math class ArmPositions(Enum): Up = 1 Settle = 2 Down = 3 Flip = 4 armMotor = LargeMotor(OUTPUT_A) armMotorSpeed = 25 tableMotor = LargeMotor(OUTPUT_B) tableMotorSpeed = 50 tableMotorOvershoot = 50 tableDegreesPerTurn = 270 scannerMotor = MediumMotor(OUTPUT_C) scannerMotorSpeed = 50 scannerPositionHold = -200 scannerPositionCenter = -750 scannerPositionEdge = -625 scannerPositionCorner = -575 def turnTable(turns, doOvershoot=True):
from time import sleep import logging #logging logging.basicConfig(level=logging.DEBUG, format='%(asctime)s %(levelname)5s: %(message)s') log=logging.getLogger(__name__) log.info('Stair climber starting......') # define the touch sensor ts = TouchSensor(INPUT_3) # define the gyro sensor gy = GyroSensor(INPUT_2) # define the large motor on port B lm_move = LargeMotor(OUTPUT_B) # define the large motor on port D lm_lifter = LargeMotor(OUTPUT_D) # define the midium motor on port A mm_move = MediumMotor(OUTPUT_A) # define the button btn = Button() # define lcd display lcd = Display() # define sound snd = Sound() # define the steps to go, initial value is 0