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()
