class SpikeManager: def __init__(self): # Initialize all devices self.ev3 = EV3Brick() self.usb_motor = Motor(Port.D) self.bt_motor = Motor(Port.C) self.left_button_motor = Motor(Port.B) self.right_button_motor = Motor(Port.A) # Reset all motor to mechanical stop self.usb_motor.run_until_stalled(-SPEED, duty_limit=50) self.bt_motor.run_until_stalled(-SPEED, duty_limit=20) self.left_button_motor.run_until_stalled(-SPEED, duty_limit=100) self.right_button_motor.run_until_stalled(SPEED, duty_limit=30) wait(500) # Reset the angles self.usb_motor.reset_angle(10) self.bt_motor.reset_angle(-20) self.left_button_motor.reset_angle(-25) self.right_button_motor.reset_angle(20) # Go to neutral position self.reset() def reset(self): self.usb_motor.run_target(SPEED, 0) self.bt_motor.run_target(SPEED, 0) self.left_button_motor.run_target(SPEED, 0) self.right_button_motor.run_target(SPEED, 0) def insert_usb(self): self.usb_motor.run_target(SPEED, 70, then=Stop.COAST) def remove_usb(self): self.usb_motor.run_target(SPEED, 0, then=Stop.COAST) def activate_dfu(self): self.bt_motor.dc(-40) wait(600) self.insert_usb() wait(8000) self.bt_motor.run_target(SPEED, 0) def shutdown(self): self.left_button_motor.run_target(SPEED, 20) wait(4000) self.left_button_motor.run_target(SPEED, 0)
def wiiInput(): ## Wii Specific Buttons Wii_A = 304 Wii_B = 305 Wii_1 = 257 Wii_2 = 258 Wii_Minus = 412 Wii_Plus = 407 Wii_Home = 316 Wii_Up = 103 Wii_Down = 108 Wii_Left = 105 Wii_Right = 106 ev3 = EV3Brick() left_motor = Motor(Port.B) right_motor = Motor(Port.C) wheel_diameter = 56 axle_track = 114 pen_angle = 0 left_speed = 0 right_speed = 0 characters = [ 'mario', 'luigi', 'peach', 'toad', 'yoshi', 'dk', 'wario', 'bowser' ] ev3.screen.clear() ev3.screen.load_image(characters[0] + ".png") ev3.speaker.play_file("selectPlayer.wav") currentCharacter = 0 left_speed = 0 right_speed = 0 forward = False backward = False turning = False boost = False slow = False spin = False global item global characterSelected ## cat /proc/bus/input/devices event = controllerEvent infile_path = "/dev/input/" + controllerEvent in_file = open(infile_path, "rb") FORMAT = 'llHHi' EVENT_SIZE = struct.calcsize(FORMAT) event = in_file.read(EVENT_SIZE) while event: (tv_sec, tv_usec, ev_type, code, value) = struct.unpack(FORMAT, event) print(item) if item == 0: spin = False slow = False boost = False ## Boost if item == 1: spin = False slow = False boost = True ## Slow elif item == 2: spin = False slow = True boost = False ## Spin elif item == 3: spin = True slow = False boost = False # If a button was pressed or released if ev_type == 1: if characterSelected == 0: if code == Wii_Up and value == 1: if currentCharacter > 0: currentCharacter -= 1 ev3.screen.clear() ev3.screen.load_image(characters[currentCharacter] + ".png") wait(500) if code == Wii_Down and value == 1: if currentCharacter < len(characters) - 1: currentCharacter += 1 ev3.screen.clear() ev3.screen.load_image(characters[currentCharacter] + ".png") wait(500) if code == Wii_2 and value == 1: ev3.speaker.play_file(characters[currentCharacter] + ".wav") wait(500) characterSelected = 1 ### "PLUS BUTTON TO START RACE" --> 3 2 1 GO else: ## 2 button Pressed (forward) if code == Wii_2 and value == 1: forward = True backward = False left_speed = 100 * 0.8 right_speed = 100 * 0.8 ## 2 button Released (forward) elif code == Wii_2 and value == 0: forward = False backward = False if turning == False: left_speed = 0 right_speed = 0 ## 1 button Pressed (backward) if code == Wii_1 and value == 1: backward = True forward = False left_speed = -50 right_speed = -50 ## 1 button Pressed (backward) if code == Wii_1 and value == 0: backward = False forward = False if turning == False: left_speed = 0 right_speed = 0 ## Down button pressed (turn right) if code == Wii_Down and value == 1: turning = True if forward == True: left_speed = 100 right_speed = 50 if backward == True: left_speed = -50 right_speed = -25 if backward == False and forward == False: left_speed = 100 right_speed = 0 ## Down button released (turn right) if code == Wii_Down and value == 0: turning = False if forward == True: left_speed = 100 right_speed = 100 elif backward == True: left_speed = -50 right_speed = -50 else: left_speed = 0 right_speed = 0 ## Up button pressed (turn left) if code == Wii_Up and value == 1: turning = True if forward == True: left_speed = 50 right_speed = 100 if backward == True: left_speed = -25 right_speed = -50 if backward == False and forward == False: left_speed = 0 right_speed = 100 ## Up button released (turn left) if code == Wii_Up and value == 0: turning = False if forward == True: left_speed = 100 right_speed = 100 elif backward == True: left_speed = -50 right_speed = -50 else: left_speed = 0 right_speed = 0 ## Spin Test (A Button) ##if code == 304 and value == 1: ##Spin2Win() ## This should stop it from crashing (threads) try: # Set motor speed ## Full Speed if boost == True: left_motor.dc(left_speed) right_motor.dc(right_speed) ## Slow Speed elif slow == True: left_motor.dc(left_speed * 0.6) right_motor.dc(right_speed * 0.6) ## Spin 360 degrees for 2 seconds elif spin == True: robot = DriveBase(Motor(Port.B), Motor(Port.C), 56, 114) robot.drive_time(0, 360, 2000) robot.stop() ## Normal Driving Mode (80% speed) else: left_motor.dc(left_speed * 0.8) right_motor.dc(right_speed * 0.8) except: pass event = in_file.read(EVENT_SIZE) in_file.close()
# open file in binary mode in_file = open(infile_path, "rb") # Read from the file # long int, long int, unsigned short, unsigned short, unsigned int FORMAT = 'llHHI' EVENT_SIZE = struct.calcsize(FORMAT) event = in_file.read(EVENT_SIZE) while event: (tv_sec, tv_usec, ev_type, code, value) = struct.unpack(FORMAT, event) if ev_type == 3 and code == 3: right_stick_x = value if ev_type == 3 and code == 4: right_stick_y = value # Scale stick positions to -100,100 forward = scale(right_stick_y, (0, 255), (100, -100)) left = scale(right_stick_x, (0, 255), (100, -100)) # Set motor voltages. If we're steering left, the left motor # must run backwards so it has a -left component # It has a forward component for going forward too. left_motor.dc(forward - left) right_motor.dc(forward + left) # Finally, read another event event = in_file.read(EVENT_SIZE) in_file.close()
elif (potencia < -potMaxima): potencia = -potMaxima if (potencia >= 0): potenciaReal = potencia / 1.098901098901099 + 9 else: potenciaReal = potencia / 1.098901098901099 - 9 if (abs(potencia) <= DEADPOT): # Testa se esta em uma posição da zona morta contaPot += 1 else: contaPot = 0 if (contaPot <= 10): motorLeft.dc(potenciaReal) motorRight.dc(potenciaReal) else: motorOn = False motorLeft.brake() motorRight.brake() else: potencia = 0.0 motorLeft.brake() motorRight.brake() botoes = ev3.buttons.pressed() if (Button.CENTER in botoes): break if (Button.UP in botoes): # Botão para cima usa o sensor de cima
Set base variables ''' lightStatus = False flickerStatus = False ''' Create main loop ''' while True: # Turn light on if buttons["up"] and lightStatus == False: lightStatus = True motorA.dc(100) elif buttons["down"] and lightStatus == True: lightStatus = False motorA.dc(0) # Move turbine motorB.dc(buttons['rightVertical']) # Stop script when PS button is pressed if buttons["ps"] is True: wait(2000) # Closing sequence
motorLeft = Motor(Port.A) motorRight = Motor(Port.B) infrared = InfraredSensor(Port.S1) # Create a loop to react to buttons while True: # Check for center button events if Button.CENTER in ev3.buttons.pressed(): motorLeft.stop() motorRight.stop() break # React to the left up and down buttons if Button.LEFT_DOWN in infrared.keypad(): motorLeft.dc(-50) elif Button.LEFT_UP in infrared.keypad(): motorLeft.dc(50) else: motorLeft.stop() # React to the right up and down buttons if Button.RIGHT_DOWN in infrared.keypad(): motorRight.dc(-50) elif Button.RIGHT_UP in infrared.keypad(): motorRight.dc(50) else: motorRight.stop() # Uncomment to display the current status of the remote buttons # print("Buttons: ", infrared.buttons(1))
class MbMotor(): """ Control a motor, besides the fact that you can detect if a motor got stalled the main reason for this class is to solve a bug for pybricks.ev3devices.Motor. The bug is that when you set the motor to move in a Direction.COUNTERCLOCKWISE sometimes it failes to detect it. This class is made on top of pybricks.ev3devices.Motor Args: port (Port): The port of the device clockwise_direction (bool): Sets the defualt movement of the motor clockwise or counterclockwise, True for clockwise else counterclockwise exit_exec (Function): Function that returns True or False, the motor will stop if returns True """ def __init__(self, port, clockwise_direction=True, exit_exec=lambda: False): self.core = Motor(port) self.port = port self.direction = 1 if clockwise_direction else -1 self.exit_exec = exit_exec def angle(self): """ Get the distance the robot has moved in degrees Returns: angle (int): The distance the robot has moved in degrees """ return self.core.angle() * self.direction def speed(self): """ Get the speed of the motor Returns: speed (int): The current speed of the motor """ return self.core.speed() * self.direction def stalled(self, min_speed=0): if abs(self.speed()) <= abs(min_speed): return True return False def run_angle(self, speed, angle, wait=True, detect_stall=False): """ Run the motor to a specific angle Args: speed (int): The speed of the motor angle (int): Degrees to run the motor at wait (bool): Sets if the robot is going to stop for the motor to complete this method or not """ def exec(self, speed, angle): moved_enough = False self.reset_angle() self.run(speed) while True: if abs(self.angle()) > 50: moved_enough = True if moved_enough and detect_stall: if self.stalled(): break if abs(self.angle()) > abs(angle) or self.exit_exec(): break self.hold() if wait: exec(self, speed, angle) else: threading.Thread(target=exec, args=[self, speed, angle]).start() def run_time(self, speed, msec, wait=True): """ Run the motor to a amount of time Args: speed (int): The speed of the motor msec (int): Time to move the robot wait (bool): Sets if the robot is going to stop for the motor to complete this method or not """ def exec(self, speed, msec): self.reset_angle() self.run(speed) s = time() while True: if round(time() - s, 2) * 1000 >= abs(msec) or self.exit_exec(): break self.hold() if wait: exec(self, speed, msec) else: threading.Thread(target=exec, args=[self, speed, msec]).start() def run(self, speed): """ Run the motor to a constant speed Args: speed (int): Speed to run at Note: speed parameter should be between -800 and 800 """ self.core.run(speed * self.direction) def dc(self, speed): """ Run the motor to a constant speed Args: speed (int): Speed to run at Note: speed parameter should be between -100 and 100 """ self.core.dc(speed * self.direction) def hold(self): """ Stop the motor and hold its position """ self.core.hold() def brake(self): """ Passively stop the motor """ self.core.brake() def stop(self): """ No current is being aplied to the robot, so its gonna stop due to friction """ self.core.stop() def reset_angle(self, angle=0): """ Set the motor angle Args: angle (int): Angle to set the motor at """ self.core.reset_angle(angle) def is_stalled(self, min_speed=0): """ Check if the motor got stalled Args: min_speed (int): The minim speed the motor should be moving at """ if abs(self.speed()) <= abs(min_speed): return True return False def __repr__(self): return "Motor Properties:\nPort: " + str( self.port) + "\nDefault Direction: " + str(self.direction)
def takeSecond(elem): return elem[0] def KNN(categories, distance, x, k): dist = [] for (i, val) in enumerate(distance): dist.append((abs(val-x),categories[i])) dist.sort(key=takeSecond) sum = 0 for i in range(3): sum += dist[i][1] return(sum/3) #this is actual k means # return 0 if sum < (k-sum) else 1 # return 0 if mostly 0s categories = [75,75,75,80,80,80,85,85,85,90,90,90,95,95,95] distance = [32,49,68,139,157,145,199,209,197,243,259,250,298,290,302] while True: speed = KNN(categories,distance,ultra.distance() + 100 ,3) if touch.pressed(): for i in range(1000): if abs(throw.angle())< abs(115): throw.dc(speed) else: break wait(0.1) print('done') throw.run_angle(2,2,stop_type=Stop.COAST, wait=True)
motorB.run_target(speed=500, target_angle=90, then=Stop.HOLD, wait=True) printMotor(motorB, ev3.screen) waiter(ir) ev3.screen.print('=angle 90=') motorB.run_angle(speed=500, rotation_angle=90, then=Stop.HOLD, wait=True) printMotor(motorB, ev3.screen) waiter(ir) ev3.screen.print('=track target 90=') motorB.track_target(target_angle=90) waiter(ir) ev3.screen.print('=until stalled=') motorB.run_until_stalled(15, then=Stop.COAST, duty_limit=10) printMotor(motorB, ev3.screen) waiter(ir) ev3.screen.print('=duty 100=') motorB.dc(100) waiter(ir) printMotor(motorB, ev3.screen) motorB.stop() waiter(ir) ev3.screen.print('=duty -100=') motorB.dc(-100) waiter(ir) printMotor(motorB, ev3.screen) motorB.stop() waiter(ir)
#!/usr/bin/env pybricks-micropython from pybricks import ev3brick as brick from pybricks.ev3devices import Motor from pybricks.parameters import Port, Button from pybricks.tools import print, wait motor = Motor(Port.B) cycle = 50 while True: bts = brick.buttons() if Button.LEFT in bts: cycle = max(-100, cycle - 10) elif Button.RIGHT in bts: cycle = min(100, cycle + 10) elif Button.CENTER in bts: break motor.dc(cycle) print(cycle, motor.speed(), motor.angle()) wait(100)
class motorControl: def __init__(self, state): """ Constructor """ self.motor_left = Motor(Port.B) self.motor_right = Motor(Port.C) self.state = state self.db = DriveBase(self.motor_left, self.motor_right, self.state.wheelDiameter, self.state.turnDiameter) def forward(self, *params): if (len(params) > 0): if (params[0] >= 0 and params[0] <= 100): speed = self.state.maxspeed * params[0] / 100 self.motor_left.run(speed) else: print("error,param too big") else: self.motor_right.run(self.state.speed) return def forwardPercent(self, percent): self.motor_left.dc(percent) self.motor_right.dc(percent) return def forwardDrive(self, percent): speed = self.state.maxSpeed * percent / 100 self.db.drive(speed, 0) return def rotate_right(self, *speed): #360 , 10 = 15/8 tours print("entered rotate") if (len(speed) >= 1): self.stop() self.motor_left.run(-speed[0]) self.motor_right.run(speed[0]) else: self.stop() self.motor_left.run(self.speed) self.motor_right.run(-self.speed) wait(10000) self.stop() return def rotate_left(self, *speed): if (len(speed) >= 1): self.stop() self.motor_left.run(-speed[0]) self.motor_right.run(speed[0]) else: self.stop() self.motor_left.run(-self.speed) self.motor_right.run(self.speed) wait(10000) self.stop() return def rotate(self, angle, *params): """ Rotate the robot on himself to a given angle with a rotation speed of 360 :param angle: the rotation angle, left turn is positiv, right turn is negativ """ if (len(params) > 1): print('Too many parameters, enter just an angle and a time (s)') else: # Time not in parameter if (len(params) == 0): time = abs(angle) * math.pi * self.state.turnDiameter / ( self.state.wheelDiameter * math.pi * self.state.speed) time = time * 1000 speed = self.state.speed # Time is in parameter else: time = params[0] speed = (int)(abs(angle) * math.pi * self.state.turnDiameter / (self.state.wheelDiameter * math.pi * time)) print(speed) print(time) if (angle >= 0): turnLeft = 1 else: turnLeft = -1 # Execution self.stop() self.motor_left.run(-1 * turnLeft * speed) self.motor_right.run(turnLeft * speed) wait(time) self.stop() return def stop(self): self.db.stop() return def calibrage(self): return
right_motor = Motor(Port.C) gyro = GyroSensor(Port.S3) # Resetando valores left_motor.reset_angle(0) right_motor.reset_angle(0) gyro.reset_angle(0) # Variáveis utilizadas no processo porcentagem = 60 distancia = cm_to_degree(100) target = 45 kp, ki, kd = 1, 0.01, 2 integral, derivate, error, last_error = 0, 0, 0, 0 while distancia > media_wheels(left_motor, right_motor): error = target - gyro.angle() integral += error derivate = error - last_error correcao = kp * (error + ki * integral + kd * derivate) if (correcao >= 0): left_motor.dc(porcentagem + correcao) right_motor.dc(porcentagem) else: right_motor.dc(porcentagem + correcao * -1) left_motor.dc(porcentagem) left_motor.hold() right_motor.hold()
#!/usr/bin/env pybricks-micropython from math import sin from pybricks import ev3brick as brick from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor, InfraredSensor, UltrasonicSensor, GyroSensor) from pybricks.parameters import (Port, Stop, Direction, Button, Color, SoundFile, ImageFile, Align) from pybricks.tools import print, wait, StopWatch from pybricks.robotics import DriveBase left_motor = Motor(Port.B) right_motor = Motor(Port.C) ''' left_motor.dc(30) right_motor.dc(30) while not any(brick.buttons()): brick.display.text('motor speed:' + str(left_motor.speed())) wait(10) while any(brick.buttons()): wait(10) left_motor.stop(Stop.BRAKE) right_motor.stop(Stop.COAST) while not any(brick.buttons()): brick.display.text('motor angle:' + str(left_motor.angle())) wait(10) while any(brick.buttons()): wait(10)
rPad_x = value if ev_type == 3 and code == 1: rPad_y = value # Scale stick positions to -100,100 right = scale(rPad_x, (0, 255), (100, -100)) left = scale(rPad_x, (0, 255), (100, -100)) # Set motor voltages. If we're steering left, the left motor # must run backwards so it has a -left component # It has a forward component for going forward too. # steer_motor.dc=(45-steer_motor.angle()) # main_motor.angle(45) right = round(right, 1) main_motor.dc(right - left) main_motor.dc(right + left) # Finally, read another event event = in_file.read(EVENT_SIZE) in_file.close() # evdev takes care of polling the controller in a loop for event in gamepad.read_loop(): if event.type == ecodes.EV_KEY: if event.value == 1: if event.code == yBtn: print("Y") elif event.code == bBtn: print("B")
# D = -7 P = 1.6 I = 0.3 D = 0 limit = 100 print("GO") while True: if Button.RIGHT in brick.buttons(): P = P + .1 elif Button.LEFT in brick.buttons(): P = P - .1 elif Button.UP in brick.buttons(): I = I + .01 elif Button.DOWN in brick.buttons(): I = I - .01 #print("P:", P, "I:", I) angle = gyro_sensor.speed() #angle_delta = angle_0 - angle angle_sum += angle # power = P * angle_delta + I * angle_sum + D * (angle_delta - angle_delta_prev) power = P * angle + I * angle_sum #angle_prev = angle #angle_delta_prev = angle_delta if power > limit: power = limit if power < -limit: power = -limit # print("s:", angle, "d:", angle_delta, "p:", power) #print("s:", angle, "p:", power, "P:", P, "I:", I, "sum:", angle_sum) left_motor.dc(power) right_motor.dc(power) # wait(5)
robot.drive(10,100) stop1() robot.stop() reset_all() while True: PID(-700,0,1,1,1) stop1() robot.stop() elif Button.DOWN in brick.buttons(): while not Button.CENTER in brick.buttons(): first_gyro.reset_angle(0) second_gyro.reset_angle(0) small_left_m = Motor(Port.A) i=0 while i!= 1500: small_left_m.dc(40) i+=1 stop1() small_left_m.stop() time.sleep(0.5) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=740: PID(150,0,8,1,1) stop1() robot.stop() time.sleep(0.1) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=720: followline(100,0.5,1,1)
def stop2(): if Button.CENTER in brick.buttons(): left_M.stop() right_M.stop() '''small_left_m.stop()''' '''small_right_m.stop() ''' while True: if first_cls.color() == Color.RED: while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=1200: PID(150,0,5,1,1) stop1() robot.stop() reset_all() time.sleep(1) while motor_avarge()>=-270: PID(-400,0,1,1,1) stop1() robot.stop() reset_all() while True: PID(-500,-210,1,1,1) stop1() elif Button.LEFT in brick.buttons(): while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=970: PID(200,0,8,1,1) stop1() robot.stop() reset_all() while motor_avarge()>=-450: PID(-200,0,1,1,1) stop1() robot.stop() reset_all() while True: PID(-500,-200,1,1,1) stop1() elif Button.UP in brick.buttons(): while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=780: PID(150,0,8,1,1) stop1() robot.stop() left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=470: followline(100,1.2,1,1) stop1() robot.stop() time.sleep(0.1) first_gyro.reset_angle(0) second_gyro.reset_angle(0) while first_gyro.angle()<=25: robot.drive(10,-100) stop1() robot.stop() reset_all() while motor_avarge()<=1000: PID(160,0,8,1,1) stop1() robot.stop() time.sleep(0.2) left_M.reset_angle(0) right_M.reset_angle(0) if second_cls.reflection()>=65: while motor_avarge()<=500: followline(100,0.6,1,1) stop1() robot.stop() elif second_cls.reflection()<=10: while motor_avarge()<=460: followline(100,0.6,1,1) stop1() robot.stop() else: while motor_avarge()<=420: PID(100,-4,1,1,1) stop1() robot.stop() time.sleep(0.3) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()>=-1500: PID(-300,0,1,1,1) stop1() robot.stop() while first_gyro.angle()>=-17: robot.drive(10,100) stop1() robot.stop() reset_all() while True: PID(-700,0,1,1,1) stop1() robot.stop() elif Button.DOWN in brick.buttons(): while not Button.CENTER in brick.buttons(): first_gyro.reset_angle(0) second_gyro.reset_angle(0) small_left_m = Motor(Port.A) i=0 while i!= 1500: small_left_m.dc(40) i+=1 stop1() small_left_m.stop() time.sleep(0.5) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=740: PID(150,0,8,1,1) stop1() robot.stop() time.sleep(0.1) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=720: followline(100,0.5,1,1) stop1() robot.stop() time.sleep(0.5) i=0 while i!= 2000: small_left_m.dc(-40) i+=1 stop1() small_left_m.stop() reset_all() while motor_avarge()<=1000: PID(150,0,1,1,1) stop1() robot.stop() time.sleep(1) reset_all() while True: PID(-500,0,1,1,1) stop1() elif Button.RIGHT in brick.buttons(): while not Button.CENTER in brick.buttons(): reset_all() while motor_avarge()<=740: PID(150,0,1,1,1) stop1() robot.stop() time.sleep(0.1) left_M.reset_angle(0) right_M.reset_angle(0) while motor_avarge()<=870: followline(150,0.6,1,1) stop1() robot.stop() reset_all() while motor_avarge()<=400: PID(150,0,1,1,1) stop1() robot.stop() reset_all() while first_gyro.angle()<=100: robot.drive(10,-100) stop1() robot.stop() i=0 while i<=110: robot.drive(-100,0) i+=1 stop1() robot.stop() reset_all() while motor_avarge()<=270: PID(150,0,1,1,1) stop1() robot.stop() reset_all() while first_gyro.angle()<=27: robot.drive(10,-100) stop1() robot.stop() reset_all() while motor_avarge()<=600: followline(100,1,1,1) stop1() robot.stop() reset_all() while first_gyro.angle()<=170: robot.drive(10,-100) stop1() robot.stop() while motor_avarge()>=-1230: robot.drive(-1000,0) stop1() robot.stop() reset_all() while True: if motor_avarge()>0: while motor_avarge()>0: robot.drive(-10,0) stop1() robot.stop() elif motor_avarge()<0: while motor_avarge()<0: robot.drive(-10,0) stop1() robot.stop() while motor_avarge()>=-1300: robot.drive(-2000,0) stop1() robot.stop() reset_all()
# Write your program here brick.sound.beep() # создаём объекты класса Motor # для управления реальными моторами left_motor = Motor(Port.B) right_motor = Motor(Port.C) # для подачи тока на моторы (от -100 до 100) # используем метод dc объектов класса Motor # крутиться вправо 1 секунду: left_motor.dc(50) right_motor.dc(-50) wait(1000) # сочиняем функцию для поворота налево: def kib_turn_left(): left_motor.dc(50) right_motor.dc(-50) wait(1000) left_motor.dc(0) right_motor.dc(0) # вызываем её на исполнение:
touch_sensor = TouchSensor(Port.S3) # Using a very large font big_font = Font(size=24) ev3.screen.set_font(big_font) # Initialize the rear structure. In order to move the structure both # the rear motor and lift motor must run in sync. First, the rear # motor moves the robot backward while the lift motor moves the rear # structure up until the Touch Sensor is pressed. Second, the rear # motor moves the robot forward while the lift motor moves the rear # structure down for a set amount of degrees to move to its starting # position. Finally, the lift motor resets the angle to "0." This # means that when it moves to "0" later on, it returns to this starting # position. rear_motor.dc(-20) lift_motor.dc(100) while not touch_sensor.pressed(): wait(10) lift_motor.dc(-100) rear_motor.dc(40) wait(50) lift_motor.run_angle(-145, 510) rear_motor.hold() lift_motor.run_angle(-30, 44) lift_motor.reset_angle(0) gyro_sensor.reset_angle(0) # Initialize the steps variable to 0. steps = 0
class Charlie(): ''' Charlie is the class responsible for driving, Robot-Movement and general Real-world interaction of the robot with Sensors and motors. Args: config (dict): The parsed config brick (EV3Brick): EV3Brick for getting button input logger (Logger): Logger for logging ''' def __init__(self, config, brick, logger): logger.info(self, 'Starting initialisation of Charlie') self.__config = config self.brick = brick self.logger = logger self.__conf2port = { 1: Port.S1, 2: Port.S2, 3: Port.S3, 4: Port.S4, 'A': Port.A, 'B': Port.B, 'C': Port.C, 'D': Port.D } self.__initSensors() self.__initMotors() self.min_speed = 35 # lage motor 20, medium motor 30 self.__gyro.reset_angle(0) self.__screenRoutine = False self.showDetails() self.logger.info(self, 'Driving for Charlie initialized') ##TODO def __repr__(self): outputString = "(TODO)\n Config: " + self.__config + "\n Brick: " + self.brick + "\n Logger: " + self.logger outputString += "\n--Debug--\n Minimum Speed: " + str( self.min_speed) + "\n " return "TODO" def __str__(self): return "Charlie" def __initSensors(self): '''Sub-method for initializing Sensors.''' self.logger.debug(self, "Starting sensor initialisation...") try: self.__gyro = GyroSensor( self.__conf2port[self.__config['gyroSensorPort']], Direction.CLOCKWISE if not self.__config['gyroInverted'] else Direction.COUNTERCLOCKWISE ) if self.__config['gyroSensorPort'] != 0 else 0 self.logger.debug( self, 'Gyrosensor initialized sucessfully on port %s' % self.__config['gyroSensorPort']) except Exception as exception: self.__gyro = 0 self.logger.error( self, "Failed to initialize the Gyro-Sensor - Are u sure it's connected to Port %s?" % exception, exception) try: self.__rLight = ColorSensor( self.__conf2port[self.__config['rightLightSensorPort']] ) if self.__config['rightLightSensorPort'] != 0 else 0 self.logger.debug( self, 'Colorsensor initialized sucessfully on port %s' % self.__config['rightLightSensorPort']) except Exception as exception: self.logger.error( self, "Failed to initialize the right Color-Sensor - Are u sure it's connected to Port %s?" % exception, exception) try: self.__lLight = ColorSensor( self.__conf2port[self.__config['leftLightSensorPort']] ) if self.__config['leftLightSensorPort'] != 0 else 0 self.logger.debug( self, 'Colorsensor initialized sucessfully on port %s' % self.__config['leftLightSensorPort']) except Exception as exception: self.logger.error( self, "Failed to initialize the left Color-Sensor - Are u sure it's connected to Port %s?" % exception, exception) try: self.__touch = TouchSensor( self.__conf2port[self.__config['backTouchSensor']] ) if self.__config['backTouchSensor'] != 0 else 0 self.logger.debug( self, 'Touchsensor initialized sucessfully on port %s' % self.__config['backTouchSensor']) except Exception as exception: self.logger.error( self, "Failed to initialize the Touch-Sensor - Are u sure it's connected to Port %s?" % exception, exception) self.logger.debug(self, "Sensor initialisation done") def __initMotors(self): '''Sub-method for initializing Motors.''' self.logger.debug(self, "Starting motor initialisation...") if self.__config['robotType'] == 'NORMAL': try: self.__lMotor = Motor( self.__conf2port[self.__config['leftMotorPort']], Direction.CLOCKWISE if (not self.__config['leftMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['leftMotorGears']) self.__rMotor = Motor( self.__conf2port[self.__config['rightMotorPort']], Direction.CLOCKWISE if (not self.__config['rightMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['rightMotorGears']) except Exception as exception: self.logger.error( self, "Failed to initialize movement motors for robot type NORMAL - Are u sure they\'re all connected?", exception) if self.__config['useGearing']: try: self.__gearingPortMotor = Motor( self.__conf2port[ self.__config['gearingSelectMotorPort']], Direction.CLOCKWISE if (not self.__config['gearingSelectMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['gearingSelectMotorGears']) self.__gearingTurnMotor = Motor( self.__conf2port[ self.__config['gearingTurnMotorPort']], Direction.CLOCKWISE if (not self.__config['gearingTurnMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['gearingTurnMotorGears']) except Exception as exception: self.logger.error( self, "Failed to initialize action motors for the gearing - Are u sure they\'re all connected?", exception) else: try: self.__aMotor1 = Motor( self.__conf2port[ self.__config['firstActionMotorPort']], Direction.CLOCKWISE if (not self.__config['firstActionMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['firstActionMotorGears']) if ( self.__config['firstActionMotorPort'] != 0) else 0 self.__aMotor2 = Motor( self.__conf2port[ self.__config['secondActionMotorPort']], Direction.CLOCKWISE if (not self.__config['secondActionMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['secondActionMotorGears']) if ( self.__config['secondActionMotorPort'] != 0) else 0 except Exception as exception: self.logger.error( self, "Failed to initialize action motors - Are u sure they\'re all connected?", exception) else: try: self.__fRMotor = Motor( self.__conf2port[self.__config['frontRightMotorPort']], Direction.CLOCKWISE if (not self.__config['frontRightMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['frontRightMotorGears']) if ( self.__config['frontRightMotorPort'] != 0) else 0 self.__bRMotor = Motor( self.__conf2port[self.__config['backRightMotorPort']], Direction.CLOCKWISE if (not self.__config['backRightMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['backRightMotorGears']) if ( self.__config['backRightMotorPort'] != 0) else 0 self.__fLMotor = Motor( self.__conf2port[self.__config['frontLeftMotorPort']], Direction.CLOCKWISE if (not self.__config['frontLeftMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['frontLeftMotorGears']) if ( self.__config['frontLeftMotorPort'] != 0) else 0 self.__bLMotor = Motor( self.__conf2port[self.__config['backLeftMotorPort']], Direction.CLOCKWISE if (not self.__config['backLeftMotorInverted']) else Direction.COUNTERCLOCKWISE, gears=self.__config['backLeftMotorGears']) if ( self.__config['backLeftMotorPort'] != 0) else 0 except Exception as exception: self.logger.error( self, "Failed to initialize movement motors for robot type %s - Are u sure they\'re all connected? Errored at Port" % self.__config['robotType'], exception) self.logger.debug(self, "Motor initialisation done") self.logger.info(self, 'Charlie initialized') def showDetails(self): ''' Processes sensor data in a separate thread and shows ''' threadLock = _thread.allocate_lock() def __screenPrintRoutine(): while True: if self.__gyro.angle() > 360: ang = self.__gyro.angle() - 360 else: ang = self.__gyro.angle() speedRight = self.__rMotor.speed() if self.__config[ 'robotType'] == 'NORMAL' else self.__fRMotor.speed() speedRight = speedRight / 360 # from deg/s to revs/sec speedRight = speedRight * (self.__config['wheelDiameter'] * math.pi) # from revs/sec to cm/sec speedLeft = self.__lMotor.speed() if self.__config[ 'robotType'] == 'NORMAL' else self.__fLMotor.speed() speedLeft = speedLeft / 360 # from deg/s to revs/sec speedLeft = speedLeft * (self.__config['wheelDiameter'] * math.pi) # from revs/sec to cm/sec #self.brick.screen.set_font(Font(family = 'arial', size = 16)) if self.__screenRoutine: print(self.__gyro.angle()) self.brick.screen.draw_text(5, 10, 'Robot-Angle: %s' % ang, text_color=Color.BLACK, background_color=Color.WHITE) self.brick.screen.draw_text(5, 40, 'Right Motor Speed: %s' % ang, text_color=Color.BLACK, background_color=Color.WHITE) self.brick.screen.draw_text(5, 70, 'Left Motor Speed: %s' % ang, text_color=Color.BLACK, background_color=Color.WHITE) time.sleep(0.1) with threadLock: _thread.start_new_thread(__screenPrintRoutine, ()) def execute(self, params): ''' This function interprets the number codes from the given array and executes the driving methods accordingly Args: params (array): The array of number code arrays to be executed ''' if self.brick.battery.voltage() <= 7600: if (self.__config["ignoreBatteryWarning"] == True): self.logger.warn( "Please charge the battery. Only %sV left. We recommend least 7.6 Volts for accurate and repeatable results. ignoreBatteryWarning IS SET TO True, THIS WILL BE IGNORED!!!" % self.brick.battery.voltage() * 0.001) else: self.logger.warn( "Please charge the battery. Only %sV left. We recommend least 7.6 Volts for accurate and repeatable results." % self.brick.battery.voltage() * 0.001) return if self.__gyro == 0: self.logger.error(self, "Cannot drive without gyro", '') return methods = { 3: self.absTurn, 4: self.turn, 5: self.action, 7: self.straight, 9: self.intervall, 11: self.curve, 12: self.toColor, 15: self.toWall } self.__gyro.reset_angle(0) self.__gyro.reset_angle(0) time.sleep(0.1) self.__screenRoutine = True while params != [] and not any(self.brick.buttons.pressed()): pparams = params.pop(0) mode, arg1, arg2, arg3 = pparams.pop(0), pparams.pop( 0), pparams.pop(0), pparams.pop(0) methods[mode](arg1, arg2, arg3) self.breakMotors() if self.__config['useGearing']: self.__gearingPortMotor.run_target(300, 0, Stop.HOLD, True) # reset gearing time.sleep(0.3) self.__screenRoutine = False def turn(self, speed, deg, port): ''' Used to turn the motor on the spot using either one or both Motors for turning (2 or 4 in case of ALLWHEEL and MECANUM) Args: speed (int): the speed to drive at deg (int): the angle to turn port (int): the motor(s) to turn with ''' startValue = self.__gyro.angle() speed = self.min_speed if speed < self.min_speed else speed # turn only with left motor if port == 2: # right motor off self.__rMotor.dc(0) # turn the angle if deg > 0: while self.__gyro.angle() - startValue < deg: self.turnLeftMotor(speed) # slow down to not overshoot if not self.__gyro.angle() - startValue < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() - startValue > deg: self.turnLeftMotor(-speed) # slow down to not overshoot if not self.__gyro.angle() - startValue > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return # turn only with right motor elif port == 3: # left motor off self.__lMotor.dc(0) # turn the angle if deg > 0: while self.__gyro.angle() - startValue < deg: self.turnRightMotor(-speed) # slow down to not overshoot if not self.__gyro.angle() - startValue < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() - startValue > deg: self.turnRightMotor(speed) # slow down to not overshoot if not self.__gyro.angle() - startValue > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1 ) if speed > self.min_speed else self.min_speed + 5 #cancel if button pressed if any(self.brick.buttons.pressed()): return # turn with both motors elif port == 23: dualMotorbonus = 19 speed = speed * 2 # turn the angle if deg > 0: while self.__gyro.angle() - startValue < deg: self.turnLeftMotor(speed / 2) self.turnRightMotor(-speed / 2) # slow down to not overshoot if not self.__gyro.angle() - startValue < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.01 ) if speed - self._map( deg, 1, 360, 10, 0.01 ) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus # cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() - startValue > deg: self.turnLeftMotor(-speed / 2) self.turnRightMotor(speed / 2) # slow down to not overshoot if not self.__gyro.angle() - startValue > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.01 ) if speed - self._map( deg, 1, 360, 10, 0.01 ) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus # cancel if button pressed if any(self.brick.buttons.pressed()): return def absTurn(self, speed, deg, port): ''' Used to turn the motor on the spot using either one or both Motors for turning (2 or 4 in case of ALLWHEEL and MECANUM) This method turns in contrast to the normal turn() method to an absolute ange (compared to starting point) Args: speed (int): the speed to drive at deg (int): the angle to turn to port (int): the motor(s) to turn with ''' speed = self.min_speed if speed < self.min_speed else speed # turn only with left motor if port == 2: # right motor off self.__rMotor.dc(0) # turn the angle if deg > 0: while self.__gyro.angle() < deg: self.turnLeftMotor(speed) # slow down to not overshoot if not self.__gyro.angle() < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() > deg: self.turnLeftMotor(-speed) # slow down to not overshoot if not self.__gyro.angle() > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return # turn only with right motor elif port == 3: # left motor off self.__lMotor.dc(0) # turn the angle if deg > 0: while self.__gyro.angle() < deg: self.turnRightMotor(-speed) # slow down to not overshoot if not self.__gyro.angle() < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1) if speed > self.min_speed else self.min_speed #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() > deg: self.turnRightMotor(speed) # slow down to not overshoot if not self.__gyro.angle() > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.1 ) if speed > self.min_speed else self.min_speed + 5 #cancel if button pressed if any(self.brick.buttons.pressed()): return # turn with both motors elif port == 23: dualMotorbonus = 19 speed = speed * 2 # turn the angle if deg > 0: while self.__gyro.angle() < deg: self.turnLeftMotor(speed / 2) self.turnRightMotor(-speed / 2) # slow down to not overshoot if not self.__gyro.angle() < deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.01 ) if speed - self._map( deg, 1, 360, 10, 0.01 ) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus # cancel if button pressed if any(self.brick.buttons.pressed()): return else: while self.__gyro.angle() > deg: self.turnLeftMotor(-speed / 2) self.turnRightMotor(speed / 2) # slow down to not overshoot if not self.__gyro.angle() > deg * 0.6: speed = speed - self._map( deg, 1, 360, 10, 0.01 ) if speed - self._map( deg, 1, 360, 10, 0.01 ) > self.min_speed * 2 - dualMotorbonus else self.min_speed * 2 - dualMotorbonus # cancel if button pressed if any(self.brick.buttons.pressed()): return def straight(self, speed, dist, ang): ''' Drives the Robot in a straight line. Also it self-corrects while driving with the help of a gyro-sensor. This is used to make the Robot more accurate Args: speed (int): the speed to drive at dist (int): the distance in cm to drive ''' if self.__config['robotType'] != 'MECANUM': correctionStrength = 2.5 # how strongly the self will correct. 2 = default, 0 = nothing startValue = self.__gyro.angle() # convert the input (cm) to revs revs = dist / (self.__config['wheelDiameter'] * math.pi) motor = self.__rMotor if self.__config[ 'robotType'] == 'NORMAL' else self.__fRMotor # drive motor.reset_angle(0) if revs > 0: while revs > (motor.angle() / 360): # if not driving staright correct it if self.__gyro.angle() - startValue > 0: lSpeed = speed - abs(self.__gyro.angle() - startValue) * correctionStrength rSpeed = speed elif self.__gyro.angle() - startValue < 0: rSpeed = speed - abs(self.__gyro.angle() - startValue) * correctionStrength lSpeed = speed else: lSpeed = speed rSpeed = speed self.turnLeftMotor(lSpeed) self.turnRightMotor(rSpeed) #cancel if button pressed if any(self.brick.buttons.pressed()): return else: while revs < motor.angle() / 360: # if not driving staright correct it if self.__gyro.angle() - startValue < 0: rSpeed = speed + abs(self.__gyro.angle() - startValue) * correctionStrength lSpeed = speed elif self.__gyro.angle() - startValue > 0: lSpeed = speed + abs(self.__gyro.angle() - startValue) * correctionStrength rSpeed = speed else: lSpeed = speed rSpeed = speed self.turnLeftMotor(-lSpeed) self.turnRightMotor(-rSpeed) # cancel if button pressed if any(self.brick.buttons.pressed()): return else: self.__fRMotor.reset_angle(0) # convert the input (cm) to revs revs = dist / (self.__config['wheelDiameter'] * math.pi) speed = speed * 1.7 * 6 # convert speed form % to deg/min # driving the robot into the desired direction if ang >= 0 and ang <= 45: multiplier = _map(ang, 0, 45, 1, 0) self.__fRMotor.run_angle(speed, revs * 360, Stop.COAST, False) self.__bRMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__bLMotor.run_angle(speed, revs * 360, Stop.COAST, True) elif ang >= -45 and ang < 0: multiplier = _map(ang, -45, 0, 0, 1) self.__fRMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__bRMotor.run_angle(speed, revs * 360, Stop.COAST, False) self.__bLMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed, revs * 360, Stop.COAST, True) elif ang > 45 and ang <= 90: multiplier = _map(ang, 45, 90, 0, 1) self.__fRMotor.run_angle(speed, revs * 360, Stop.COAST, False) self.__bRMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__bLMotor.run_angle(speed, revs * 360, Stop.COAST, True) elif ang < -45 and ang >= -90: multiplier = _map(ang, -45, -90, 0, 1) self.__fRMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__bRMotor.run_angle(speed, revs * 360, Stop.COAST, False) self.__bLMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed, revs * 360, Stop.COAST, True) elif ang > 90 and ang <= 135: multiplier = _map(ang, 90, 135, 1, 0) self.__fRMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__bRMotor.run_angle(speed, revs * -360, Stop.COAST, False) self.__bLMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed, revs * -360, Stop.COAST, True) elif ang < -90 and ang >= -135: multiplier = _map(ang, -90, -135, 1, 0) self.__fRMotor.run_angle(speed, revs * -360, Stop.COAST, False) self.__bRMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed * multiplier + 1, revs * 360 * multiplier, Stop.COAST, False) self.__bLMotor.run_angle(speed, revs * -360, Stop.COAST, True) elif ang > 135 and ang <= 180: multiplier = _map(ang, 135, 180, 0, 1) self.__fRMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__bRMotor.run_angle(speed, revs * -360, Stop.COAST, False) self.__bLMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed, revs * -360, Stop.COAST, True) elif ang < -135 and ang >= -180: multiplier = _map(ang, -135, -180, 0, 1) self.__fRMotor.run_angle(speed, revs * -360, Stop.COAST, False) self.__bRMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__fLMotor.run_angle(speed * multiplier + 1, revs * -360 * multiplier, Stop.COAST, False) self.__bLMotor.run_angle(speed, revs * -360, Stop.COAST, True) def intervall(self, speed, dist, count): ''' Drives forwads and backwards x times. Args: speed (int): the speed to drive at revs (int): the distance (in cm) to drive count (int): how many times it should repeat the driving ''' # convert the input (cm) to revs revs = dist / (self.__config['wheelDiameter'] * math.pi) / 2 speed = speed * 1.7 * 6 # speed in deg/s to % # move count times forwards and backwards for i in range(count + 1): if self.__config['robotType'] == 'NORMAL': ang = self.__lMotor.angle() # drive backwards self.__rMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) self.__lMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) # return to cancel if any button is pressed while self.__lMotor.angle() > revs * -360: if any(self.brick.buttons.pressed()): return # drive forwards self.__lMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) self.__rMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) # return to cancel if any button is pressed while self.__rMotor.angle() <= ang: if any(self.brick.buttons.pressed()): return elif self.__config['robotType'] == 'ALLWHEEL' or self.__config[ 'robotType'] == 'MECANUM': ang = self.__lMotor.angle() # drive backwards self.__fRMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) self.__bRMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) self.__fLMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) self.__bLMotor.run_angle(speed, revs * -360, Stop.BRAKE, False) # return to cancel if any button is pressed while self.__lMotor.angle() > revs * -360: if any(self.brick.buttons.pressed()): return # drive forwards self.__fRMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) self.__bRMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) self.__fLMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) self.__bLMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) # return to cancel if any button is pressed while self.__rMotor.angle() <= ang: if any(self.brick.buttons.pressed()): return def curve(self, speed, dist, deg): ''' Drives forwads and backwards x times. Args: speed (int): the speed to drive at revs1 (int): the distance (in motor revolutions) for the outer wheel to drive deg (int): how much of a circle it should drive ''' speed = speed * 1.7 * 6 # speed to deg/s from % # gyro starting point startValue = self.__gyro.angle() revs1 = dist / (self.__config['wheelDiameter'] * math.pi) # claculate revs for the second wheel pathOutside = self.__config['wheelDiameter'] * math.pi * revs1 rad1 = pathOutside / (math.pi * (deg / 180)) rad2 = rad1 - self.__config['wheelDistance'] pathInside = rad2 * math.pi * (deg / 180) revs2 = pathInside / (self.__config['wheelDiameter'] * math.pi) # claculate the speed for the second wheel relation = revs1 / revs2 speedSlow = speed / relation if deg > 0: # asign higher speed to outer wheel lSpeed = speed rSpeed = speedSlow self.__rMotor.run_angle(rSpeed, revs2 * 360, Stop.COAST, False) self.__lMotor.run_angle(lSpeed, revs1 * 360 + 5, Stop.COAST, False) #turn while self.__gyro.angle() - startValue < deg and not any( self.brick.buttons.pressed()): pass else: # asign higher speed to outer wheel lSpeed = speed rSpeed = speedSlow self.__rMotor.run_angle(rSpeed, revs2 * 360 + 15, Stop.COAST, False) self.__lMotor.run_angle(lSpeed, revs1 * 360, Stop.COAST, False) #turn while self.__gyro.angle() - startValue > deg and not any( self.brick.buttons.pressed()): pass def toColor(self, speed, color, side): ''' Drives forward until the given colorSensor sees a given color. Args: speed (int): the speed to drive at color (int): the color to look for (0 = Black, 1 = White) side (int): which side's color sensor should be used ''' # sets color to a value that the colorSensor can work with if color == 0: color = Color.BLACK else: color = Color.WHITE # Refactor code # only drive till left colorSensor if side == 2: # if drive to color black drive until back after white to not recognize colors on the field as lines if color == Color.BLACK: while lLight.color() != Color.WHITE and not any( self.brick.buttons.pressed()): self.turnBothMotors(speed) while lLight.color() != color and not any( self.brick.buttons.pressed()): self.turnBothMotors(speed) # only drive till right colorSensor elif side == 3: # if drive to color black drive until back after white to not recognize colors on the field as lines if color == Color.BLACK: while rLight.color() != Color.WHITE and not any( self.brick.buttons.pressed()): self.turnBothMotors(speed) while rLight.color() != color and not any( self.brick.buttons.pressed()): self.turnBothMotors(speed) # drive untill both colorSensors elif side == 23: rSpeed = speed lSpeed = speed rWhite = False lWhite = False while (rLight.color() != color or lLight.color() != color ) and not any(self.brick.buttons.pressed()): #if drive to color black drive until back after white to not recognize colors on the field as lines if color == Color.BLACK: if rLight.color() == Color.WHITE: rWhite = True if lLight.color() == Color.WHITE: lWhite = True self.__rMotor.dc(rSpeed) self.__lMotor.dc(lSpeed) # if right at color stop right Motor if rLight.color() == color and rWhite: rSpeed = 0 # if left at color stop left Motor if lLight.color() == color and lWhite: lSpeed = 0 def toWall(self, speed, *args): ''' Drives until a pressure sensor is pressed Args: speed (int): the speed to drive at ''' while not touch.pressed(): self.turnBothMotors(-abs(speed)) if any(self.brick.buttons()): break self.turnBothMotors(0) def action(self, speed, revs, port): ''' Doesn't drive the robot, but drives the action motors Args: speed (int): the speed to turn the motor at revs (int): how long to turn the motor for port (int): which one of the motors should be used ''' speed = abs(speed) * 1.7 * 6 # speed to deg/s from % if self.__config['useGearing']: self.__gearingPortMotor.run_target(300, port * 90, Stop.HOLD, True) # select gearing Port ang = self.__gearingTurnMotor.angle() self.__gearingTurnMotor.run_angle(speed, revs * 360, Stop.BRAKE, False) # start turning the port # cancel, if any brick button is pressed if revs > 0: while self.__gearingTurnMotor.angle() < revs * 360 - ang: if any(self.brick.buttons.pressed()): self.__gearingTurnMotor.dc(0) return else: while self.__gearingTurnMotor.angle() > revs * 360 + ang: if any(self.brick.buttons.pressed()): self.__gearingTurnMotor.dc(0) return else: # turn motor 1 if port == 1: ang = self.__aMotor1.angle() self.__aMotor1.run_angle(speed, revs * 360, Stop.HOLD, False) if revs > 0: while self.__aMotor1.angle() < revs * 360 - ang: if any(self.brick.buttons.pressed()): self.__aMotor1.dc(0) return else: while self.__aMotor1.angle() > revs * 360 + ang: if any(self.brick.buttons.pressed()): self.__aMotor1.dc(0) return # turm motor 2 elif port == 2: ang = self.__aMotor2.angle() self.__aMotor2.run_angle(speed, revs * 360, Stop.HOLD, False) if revs > 0: while self.__aMotor2.angle() < revs * 360 - ang: if any(self.brick.buttons.pressed()): self.__aMotor2.dc(0) return else: while self.__aMotor2.angle() > revs * 360 + ang: if any(self.brick.buttons.pressed()): self.__aMotor2.dc(0) return def turnLeftMotor(self, speed): ''' Sub-method for driving the left Motor(s) Args: speed (int): the speed to drive the motor at ''' if self.__config['robotType'] == 'NORMAL': self.__lMotor.dc(speed) else: self.__fLMotor.dc(speed) self.__bLMotor.dc(speed) def turnRightMotor(self, speed): ''' Sub-method for driving the right Motor(s) Args: speed (int): the speed to drive the motor at ''' if self.__config['robotType'] == 'NORMAL': self.__rMotor.dc(speed) else: self.__fRMotor.dc(speed) self.__bRMotor.dc(speed) def turnBothMotors(self, speed): ''' Submethod for setting a motor.dc() to all motors Args: speed (int): the speed (in percent) to set the motors to ''' if self.__config['robotType'] == 'NORMAL': self.__rMotor.dc(speed) self.__lMotor.dc(speed) else: self.__fRMotor.dc(speed) self.__bRMotor.dc(speed) self.__fLMotor.dc(speed) self.__bLMotor.dc(speed) def breakMotors(self): '''Sub-method for breaking all the motors''' if self.__config['robotType'] == 'NORMAL': self.__lMotor.hold() self.__rMotor.hold() else: self.__fRMotor.hold() self.__bRMotor.hold() self.__fLMotor.hold() self.__bLMotor.hold() time.sleep(0.2) def _map(self, x, in_min, in_max, out_min, out_max): ''' Converts a given number in the range of two numbers to a number in the range of two other numbers Args: x (int): the input number that should be converted in_min (int): The minimal point of the range of input number in_max (int): The maximal point of the range of input number out_min (int): The minimal point of the range of output number out_max (int): The maximal point of the range of output number Returns: int: a number between out_min and out_max, de ''' return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min def getGyroAngle(self): return self.__gyro.angle() def setRemoteValues(self, data): x = data['x'] y = data['y'] if x == 0: x = 0.0001 if data['y'] == 0 and data['x'] == 0: self.breakMotors() else: radius = int(math.sqrt(x**2 + y**2)) # distance from center ang = math.atan(y / x) # angle in radians a = int(self._map(radius, 0, 180, 0, 100)) b = int(-1 * math.cos(2 * ang) * self._map(radius, 0, 180, 0, 100)) if x < 0: temp = a a = b b = temp if y < 0: temp = a a = -b b = -temp self.turnLeftMotor(int(self._map(a, 0, 100, 0, data['maxSpeed']))) self.turnRightMotor(int(self._map(b, 0, 100, 0, data['maxSpeed']))) if data['a1'] == 0: self.__aMotor1.hold() else: a1Speed = data['a1'] self.__aMotor1.dc(a1Speed)
# Code 310 - L1 trigger # Code 311 - R1 trigger # Code 314 - Share button # Code 315 - Options button # Code 316 - PS button elif ev_type == 3: # Type 3 event - sticks # Code 3 - right stick horizontal # Code 2 - L2 trigger # Code 5 - R2 trigger # Code 0 - left stick horizontal (left is 0) # Code 1 - left stick vertical (forward is 0) # Code 4 - r stick vertical # Code 17 - dpad vertical # Code 16 - dpad horizontal if code == 3: left = scale(value, (0, 255), (40, -40)) if code == 1: # Righ stick vertical forward = scale(value, (0, 255), (100, -100)) # Set motor voltages. left_motor.dc(-forward) right_motor.dc(-forward) # Track the steering angle steer_motor.track_target(left) # Finally, read another event event = in_file.read(EVENT_SIZE) in_file.close()
# Code 3 - right stick horizontal # Code 2 - L2 trigger # Code 5 - R2 trigger # Code 0 - left stick horizontal (left is 0) # Code 1 - left stick vertical (forward is 0) # Code 4 - r stick vertical # Code 17 - dpad vertical # Code 16 - dpad horizontal if code == 1: # Vasen tatti ylos/alas forward_l = scale(value, (0, 255), (100, -100)) brick.light(Color.RED) if code == 4: # Oikea tatti ylos/alas forward_r = scale(value, (0, 255), (100, -100)) brick.light(Color.RED) # Asetetaan tattien skaalatut signaalit moottreiden jannitteeksi left_motor1.dc(-forward_l) right_motor1.dc(-forward_r) left_motor2.dc(forward_l) right_motor2.dc(forward_r) # Finally, read another event event = in_file.read(EVENT_SIZE) in_file.close() # PS4 control code ends # Clear screen, print instructions brick.display.clear() brick.display.image('/home/robot/robomest/pics/logo3.bmp') brick.display.text("Press any button to exit", (1, 115))
if ev_type == 3 and code == 0: # Left Stick Horz. Axis left_stick_x = transform_stick(value) elif ev_type == 3 and code == 1: # Left Stick Vert. Axis left_stick_y = transform_stick(value) elif xbox and ev_type == 3 and code == 9: # Right Trigger right_trigger = value / 1024 elif not xbox and ev_type == 3 and code == 5: # R2 paddle right_trigger = value / 256 elif ev_type == 1 and code == 304 and value == 1: # A pressed play_horn() elif ev_type == 1 and code == 305 and value == 1: # B pressed play_sound_effect() #print(left_stick_y, left_stick_x) # Set motor voltages. If we're steering left, the left motor # must run backwards so it has a -X component # It has a Y component for going forward too. left_motor.dc(left_stick_y - left_stick_x * (1 - right_trigger / 1.1)) right_motor.dc(left_stick_y + left_stick_x * (1 - right_trigger / 1.1)) # Finally, read another event event = in_file.read(EVENT_SIZE) in_file.close()
robot.drive(800, 0) move = 1 # Keep driving until the random time has passed or an object is # detected. If an object is detected the "checking" variable # will be set to "False." while checking and timer.time() < random_time: checking = ultrasonic_sensor.distance() > 400 wait(10) # Stop driving. robot.drive(0, 0) # Check if the object is closer than 250 mm. if ultrasonic_sensor.distance() < 250: # Roar and move the head forward to bite. head_motor.dc(-100) ev3.speaker.play_file(SoundFile.T_REX_ROAR) wait(250) head_motor.stop() wait(1000) else: # Move the head and hiss. head_motor.dc(-100) wait(100) head_motor.stop() ev3.speaker.play_file(SoundFile.SNAKE_HISS) # Reset the head motor to its initial position. head_motor.run_target(1200, 0)
del motor_position_change[-1] wheel_angle += change - drive_speed * average_control_loop_period wheel_rate = sum( motor_position_change) / 4 / average_control_loop_period # This is the main control feedback calculation. output_power = (-0.01 * drive_speed) + ( 0.8 * robot_body_rate + 15 * robot_body_angle + 0.08 * wheel_rate + 0.12 * wheel_angle) if output_power > 100: output_power = 100 if output_power < -100: output_power = -100 # Drive the motors. left_motor.dc(output_power - 0.1 * steering) right_motor.dc(output_power + 0.1 * steering) # Check if robot fell down. If the output speed is +/-100% for more # than one second, we know that we are no longer balancing properly. if abs(output_power) < 100: fall_timer.reset() elif fall_timer.time() > 1000: break # This runs update_action() until the next "yield" statement. action = next(action_task) if action is not None: drive_speed, steering = action # Make sure loop time is at least TARGET_LOOP_PERIOD. The output power
left_speed = 100 # React to the R2 button elif code == 313 and value == 0: right_speed = 0 elif code == 313 and value == 1: right_speed = 100 elif ev_type == 3: # The sticks often trigger non-stop events, comment this out if you are # not using the sticks as part of your project, or it becomes hard to # read other data # React to the left stick vertical if code == 1: left_speed = scale(value, (0, 255), (-60, 60)) # React to the right stick vertical elif code == 4: right_speed = scale(value, (0, 255), (-60, 60)) # Set motor speed left_motor.dc(left_speed) right_motor.dc(right_speed) # Read the next event event = in_file.read(EVENT_SIZE) in_file.close()
# server = BluetoothMailboxServer() # server.wait_for_connection(2) # client1 = TextMailbox('client1', server) # client2 = TextMailbox('client2', server) # saySomething('Clients found', 3) # Initialize motors motorA = Motor(Port.A) # motorB = Motor(Port.B) motorC = DCMotor(Port.C) motorD = DCMotor(Port.D) motorA.dc(0) # motorB.dc(0) motorC.dc(0) motorD.dc(0) # Initialize sensors # touchSensor1 = TouchSensor(Port.S1) # colortouchSensor2 = ColorSensor(Port.S2) # This color sensor is being used as a light # ambient() is Blue # reflection() is Red # rgb() and color() uses all three lights # colortouchSensor2.color() # motorA.reset_angle(0)
if code == 3: # Right stick horizontal if vscale == 0.5: rotate = scale(value, (0, 255), (50, -50)) elif vscale == 0.25: rotate = scale(value, (0, 255), (35, -35)) else: rotate = scale(value, (0, 255), (80, -80)) if code == 4: # Right stick vertical if vscale == 0.5: tilt = scale(value, (0, 255), (50, -50)) elif vscale == 0.25: tilt = scale(value, (0, 255), (35, -35)) else: tilt = scale(value, (0, 255), (80, -80)) # Set motor voltages. antenna_motor.dc(-antenna) # Moottori D radar_motor.dc(radar) # Moottori C rotate_motor.dc(rotate) # Moottori A tilt_motor.dc(tilt) # Moottori B # Finally, read another event event = in_file.read(EVENT_SIZE) in_file.close() # PS4 control code ends # Clear screen, print instructions brick.display.clear() brick.display.image('/home/robot/robomest/pics/logo3.bmp') brick.display.text("Press any button to exit", (5, 115)) # Wait until a button is pressed
#Pause button, this grabs the TOUCHED function from the pause thread then puts this function #into an endless loop until unpaused global changeSong if SelectButton.pressed(): break global TOUCHED while TOUCHED: wait(5) #Program begins here #Start music thread m = Thread(target=Music) m.start() #Start pause thread p = Thread(target=pause) p.start() #Start motor while selectionMade == 0: wait(5) left.dc(-60) right.dc(-60) while True: while TOUCHED: left.stop() right.stop() left.dc(-60) right.dc(-60) wait(5) #Program Ends
def ps4Input(): ## PS4 Specific Input PS4_X = 304 PS4_O = 305 PS4_Triangle = 307 PS4_Square = 308 PS4_L1 = 310 PS4_L2 = 312 PS4_R1 = 311 PS4_R2 = 313 ## Dpad uses -1, 0, 1 in value for each axis PS4_DX = 16 PS4_DY = 17 PS4_LAnX = 0 PS4_LAnY = 1 PS4_RAnX = 3 PS4_RAnY = 4 ev3 = EV3Brick() left_motor = Motor(Port.B) right_motor = Motor(Port.C) wheel_diameter = 56 axle_track = 114 pen_angle = 0 left_speed = 0 right_speed = 0 characters = [ 'mario', 'luigi', 'peach', 'toad', 'yoshi', 'dk', 'wario', 'bowser' ] ev3.screen.clear() ev3.screen.load_image(characters[0] + ".png") ev3.speaker.play_file("selectPlayer.wav") currentCharacter = 0 left_speed = 0 right_speed = 0 forward = False backward = False turning = False boost = False slow = False spin = False global item global characterSelected ## cat /proc/bus/input/devices event = controllerEvent infile_path = "/dev/input/" + controllerEvent in_file = open(infile_path, "rb") FORMAT = 'llHHi' EVENT_SIZE = struct.calcsize(FORMAT) event = in_file.read(EVENT_SIZE) while event: (tv_sec, tv_usec, ev_type, code, value) = struct.unpack(FORMAT, event) if item == 0: spin = False slow = False boost = False ## Boost if item == 1: spin = False slow = False boost = True ## Slow elif item == 2: spin = False slow = True boost = False ## Spin elif item == 3: spin = True slow = False boost = False if characterSelected == 0: ## Dpad is considered an axis if ev_type == 3: ## Left if code == PS4_DX and value == -1: if currentCharacter > 0: currentCharacter -= 1 ev3.screen.clear() ev3.screen.load_image(characters[currentCharacter] + ".png") wait(500) ## Right if code == PS4_DX and value == 1: if currentCharacter < len(characters) - 1: currentCharacter += 1 ev3.screen.clear() ev3.screen.load_image(characters[currentCharacter] + ".png") wait(500) ## Check buttons elif ev_type == 1: if code == PS4_X and value == 1: ev3.speaker.play_file(characters[currentCharacter] + ".wav") wait(500) characterSelected = 1 ## Character has been selected, prepare movement else: if ev_type == 1: ## R2 button Pressed (forward) if code == PS4_R2 and value == 1: forward = True backward = False left_speed = 100 * 0.8 right_speed = 100 * 0.8 ## R2 button Released (forward) elif code == PS4_R2 and value == 0: forward = False backward = False if turning == False: left_speed = 0 right_speed = 0 ## L2 button Pressed (backward) elif code == PS4_L2 and value == 1: backward = True forward = False left_speed = -50 right_speed = -50 ## L2 button Released (backward) elif code == PS4_L2 and value == 0: backward = False forward = False if turning == False: left_speed = 0 right_speed = 0 ## Analog Stick Control elif ev_type == 3: if code == PS4_LAnX: xAxis = scale(value, (0, 255), (100, -100)) ## Left = 100 ## Right = -100 ## Neutral = 0 ## Left Turn (50% deadzone so it's not too sensitive) if xAxis > 50: turning = True if forward == True: left_speed = abs(xAxis) * 0.5 right_speed = abs(xAxis) if backward == True: left_speed = -abs(xAxis) * 0.25 right_speed = -abs(xAxis) * 0.5 if backward == False and forward == False: left_speed = 0 right_speed = abs(xAxis) ## Right Turn (50% deadzone so it's not too sensitive) elif xAxis < -50: turning = True if forward == True: left_speed = abs(xAxis) right_speed = abs(xAxis) * 0.5 if backward == True: left_speed = -abs(xAxis) * 0.5 right_speed = -abs(xAxis) * 0.25 if backward == False and forward == False: left_speed = abs(xAxis) right_speed = 0 ## Stick in Neutral Zone else: turning = False if forward == True: left_speed = 100 right_speed = 100 elif backward == True: left_speed = -50 right_speed = -50 else: left_speed = 0 right_speed = 0 ## D-Pad Control Turn Left elif code == PS4_DX and value == -1: turning = True if forward == True: left_speed = 50 right_speed = 100 if backward == True: left_speed = -25 right_speed = -50 if backward == False and forward == False: left_speed = 0 right_speed = 100 ## D-Pad Control Turn Right elif code == PS4_DX and value == 1: turning = True if forward == True: left_speed = 100 right_speed = 50 if backward == True: left_speed = -50 right_speed = -25 if backward == False and forward == False: left_speed = 100 right_speed = 0 ## Released Dpad elif code == PS4_DX and value == 0: turning = False if forward == True: left_speed = 100 right_speed = 100 elif backward == True: left_speed = -50 right_speed = -50 else: left_speed = 0 right_speed = 0 ## This should stop it from crashing (threads) try: ## Full Speed if boost == True: left_motor.dc(left_speed) right_motor.dc(right_speed) ## Slow Speed elif slow == True: left_motor.dc(left_speed * 0.6) right_motor.dc(right_speed * 0.6) ## Spin 360 degrees for 2 seconds elif spin == True: robot = DriveBase(Motor(Port.B), Motor(Port.C), 56, 114) robot.drive_time(0, 360, 2000) robot.stop() ## Normal Driving Mode (80% speed) else: left_motor.dc(left_speed * 0.8) right_motor.dc(right_speed * 0.8) except: pass event = in_file.read(EVENT_SIZE) in_file.close()