self.reset_Claw()
self.checkFigure()
brick.sound.beep()
random.seed(int(time()))
clangy = robot()
#print(match_pattern(clangy.matrix, globals()["sminus"]))
#clangy.checkFigure()
#clangy.reset_Claw()
clangy.readLobby()
for color in clangy.lista_lobby:
wait(10)
if color == 1:
brick.sound.file(SoundFile.RED, VOLUME)
if color == 2:
brick.sound.file(SoundFile.BLUE, VOLUME)
if color == 3:
brick.sound.file(SoundFile.GREEN, VOLUME)
if color == 4:
brick.sound.file(SoundFile.BLACK, VOLUME)
if color == 5:
brick.sound.file(SoundFile.YELLOW, VOLUME)
#clangy.moveToGame(0,0)
# while(block):
# if Button.CENTER in brick.buttons():
elif step == 3:
turn_rate = -15
# Set the drive base speed and turn rate.
robot.drive(DRIVE_SPEED, turn_rate)
last_deviation = deviation
else:
robot.stop()
end_time = watch.time()
# print("step: "+str(step))
# print("color: "+str(color))
# print("speed: "+str(DRIVE_SPEED))
# print("distance: "+str(distance))
# print("stopOrNot: "+str(stopping))
# print("stop time: "+str(stop_time))
# print("time: "+str(time))
# print("time difference: "+str(end_time-time))
# Store data log.
data.log(time, step, color, DRIVE_SPEED, distance, stopping, deviation, integral, derivative)
# Keep time of each loop constant 100ms.
wait_time = 0
if (end_time-time) < 100:
wait_time = 100-(end_time-time)
wait(wait_time)
def act_happy():
"""Makes the puppy act happy."""
#Replaced HEART eyes with DIZZY
ev3.screen.show_image(ImageFile.DIZZY)
#puppy sits down
left_leg_motor.run(-50)
right_leg_motor.run(-50)
wait(1000)
left_leg_motor.stop()
right_leg_motor.stop()
wait(100)
for _ in range(3):
ev3.speaker.play_file(SoundFile.DOG_BARK_1)
#puppy hops
left_leg_motor.run(500)
right_leg_motor.run(500)
wait(275)
left_leg_motor.stop()
right_leg_motor.stop()
wait(275)
left_leg_motor.run(-50)
right_leg_motor.run(-50)
wait(275)
left_leg_motor.stop()
right_leg_motor.stop()
wait(500)
#puppy sits down
left_leg_motor.run(-50)
right_leg_motor.run(-50)
wait(1000)
left_leg_motor.stop()
right_leg_motor.stop()
wait(100)
left_leg_motor.reset_angle(0)
right_leg_motor.reset_angle(0)
#puppy stands up
left_leg_motor.run_target(100, 25, wait=False)
right_leg_motor.run_target(100, 25)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_motor = Motor(Port.C)
right_motor = Motor(Port.B)
wheel_diameter = 56
axle_track = 114
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
gyro_sensor = GyroSensor(Port.S2)
# calibrate_gyro(gyro_sensor)
# angle_0 = gyro_sensor.angle()
angle_0 = 0
angle_sum = 0
angle_delta_prev = 0
wait(10)
# P = -55
# I = -26
# 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
SoundFile, ImageFile, Align)
from pybricks.tools import print, wait, StopWatch
from pybricks.robotics import DriveBase
motorA = Motor(Port.A)
motorD = Motor(Port.D)
robot = DriveBase(motorA, motorD, 56, 114)
cs = ColorSensor(Port.S1)
gs = GyroSensor(Port.S3)
x = 0
#Drive the rob ot for 500 mm/s, 0 turn rate and for 2 seconds
while x < 4:
robot.drive_time(500, 0, 2000)
robot.stop(stop_type = Stop.BRAKE)
wait(500)
print("Robot moved forward.Square side count = ", x+1)
#Reset Gyro Angle befor starts to turn
gs.reset_angle(0)
print("Gyro Angle :", gs.angle())
#Make the robot to turn
motorA.run(-75)
motorD.run(75)
while gs.angle() >=-75:
wait(0.5)
print("Gyro Angle :", gs.angle())
def move_crane_up(crane_motor, degrees):
log_string('Angle at start ' + str(crane_motor.angle()))
wait(100)
crane_motor.run_angle(90, degrees, Stop.BRAKE)
log_string('Angle at end ' + str(crane_motor.angle()))
#Fuege den Tools Ordner zum PYTHONPATH hinzu. Nicht notwendig wenn TOF.py im selben Ordner ist
import sys
sys.path.append("/home/robot/LEGORoboticsPython/Tools")
from pixy_camera import Camera
ev3=EV3Brick()
#Port des Seonsors festlegen
camera = Camera(Port.S1)
rightMotor = Motor(Port.A)
leftMotor = Motor(Port.B)
speed=150
while not Button.DOWN in ev3.buttons.pressed():
x,y,w,h=camera.getObjectData(1)
result = ' Camera x: ' + str(x)
print(result)
if x<2000:
diff_from_center=x-100
leftMotor.run(speed+diff_from_center)
rightMotor.run(speed-diff_from_center)
else:
leftMotor.stop()
rightMotor.stop()
wait(200)
camera.close()
leftMotor.stop()
rightMotor.stop()
def right_angle():
# This function drives the robot forward, turn a right angle, drive
# forward again, and then turn 180 degrees to drive back along the
# same path and return to its initial position.
# Reset the Gyro Sensor angle.
gyro_sensor.reset_angle(0)
# Drive forward for 750 millimeters
robot.straight(750)
# Turn clockwise until the angle is 90 degrees.
robot.drive(0, steering)
ev3.speaker.beep()
while gyro_sensor.angle() < 90 - overshoot:
wait(1)
robot.drive(0, 0)
wait(1000)
# Drive forward for 750 millimeters
robot.straight(750)
# Turn clockwise until the angle is 270 degrees.
robot.drive(0, steering)
ev3.speaker.beep()
while gyro_sensor.angle() < 270 - overshoot:
wait(1)
robot.drive(0, 0)
wait(1000)
# Drive forward for 750 millimeters
robot.straight(750)
# Turn counterclockwise until the angle is 180 degrees.
robot.drive(0, -steering)
ev3.speaker.beep()
while gyro_sensor.angle() > 180 + overshoot:
wait(1)
robot.drive(0, 0)
wait(1000)
# Drive forward for 750 millimeters
robot.straight(750)
# Turn clockwise until the angle is 360 degrees.
robot.drive(0, steering)
ev3.speaker.beep()
while gyro_sensor.angle() < 360 - overshoot:
wait(1)
robot.drive(0, 0)
wait(1000)
# function makes the car move at the desired speed and turn angle.
# The car keeps moving until you give another drive command.
def drive(drive_motor_speed, steer_angle):
# Start running the drive motors
front.run(drive_motor_speed)
rear.run(drive_motor_speed)
# Limit the steering value for safety, and then start the steer
# motor.
limited_angle = max(-limit, min(steer_angle, limit))
steer.run_target(200, limited_angle, wait=False)
# Drive forward for 5 seconds.
drive(800, 0)
wait(5000)
# Drive backward for 5 seconds.
drive(-800, 0)
wait(5000)
# Drive forward to the right for 5 seconds.
drive(800, 90)
wait(5000)
# Drive backward to the left for 5 seconds.
drive(-800, -90)
wait(5000)
# Stop smoothly and center the steer.
drive(0, 0)
# ExampleHub = TechnicHub PrimeHub MoveHub
from pybricks.hubs import ExampleHub
from pybricks.parameters import Color, Side
from pybricks.tools import wait
# Initialize the hub.
hub = ExampleHub()
# Define colors for each side in a dictionary.
SIDE_COLORS = {
Side.TOP: Color.RED,
Side.BOTTOM: Color.BLUE,
Side.LEFT: Color.GREEN,
Side.RIGHT: Color.YELLOW,
Side.FRONT: Color.MAGENTA,
Side.BACK: Color.BLACK,
}
# Keep updating the color based on detected up side.
while True:
# Check which side of the hub is up.
up_side = hub.imu.up()
# Change the color based on the side.
hub.light.on(SIDE_COLORS[up_side])
# Also print the result.
print(up_side)
wait(50)
def test_max_speed(self):
log.info('--- test_max_speed ---')
SPEED_STEP, MIN_TEST_TIME, CHECK_INTERVAL = 50, 16000, 250
max_speed, speed_left= 0, SPEED_STEP
motor_right, motor_left = Motor(ROBOT['drive_motor_port_left']), Motor(ROBOT['drive_motor_port_right'])
motor_left.reset_angle(0)
motor_right.reset_angle(0)
robot = DriveBase(motor_left, motor_right, ROBOT['wheel_diameter'], ROBOT['wheel_axle_track'])
watch = StopWatch()
robot.drive(speed_left, 0)
angle_left, angle_right = 0,0
while True:
run_time = watch.time()
old_speed = speed_left
speed_left = motor_left.speed()
# timeout
if run_time >= MIN_TEST_TIME:
log.info('reach max test time, speed_left=%d, max_speed=%d' % (speed_left, max_speed))
break
# found higher speed reached
if speed_left > max_speed:
print('motor angle: left=%d, right=%d' % (motor_left.angle(), motor_right.angle()))
print('motor speed: left=%d, right=%d' % (motor_left.speed(), motor_right.speed()))
# stop and run more time with higher speed
robot.stop()
watch.reset()
motor_left.reset_angle(0)
motor_right.reset_angle(0)
angle_left, angle_right = 0,0
max_speed = speed_left
speed_left += SPEED_STEP
print('drive one more time, speed_left=%d, max_speed=%d' % (speed_left, max_speed ))
robot.drive(speed_left, 0)
continue
# continue
a_l = motor_left.angle()
a_r = motor_right.angle()
angle_left += a_l
angle_right += a_r
#print('angle/total angle: %d/%d - %d/%d' % (a_l, angle_left, a_r, angle_right))
steering = (abs(angle_left-angle_right) // 10) * 10
if steering > 30:
steering = 30
if angle_left > angle_right:
steering = 0 - steering
if abs(angle_left - angle_right) > 10:
print('delta/total/steering: %3d/%3d/%3d' % (a_l - a_r,angle_left-angle_right, steering))
motor_left.reset_angle(0)
motor_right.reset_angle(0)
robot.drive((motor_left.speed() + motor_right.speed()) / 2, steering)
else:
print('.')
wait(CHECK_INTERVAL) # wait one second to let motor reach higher speed
print('motor speed: left=%d, right=%d' % (motor_left.speed(), motor_right.speed()))
print('total motor angle: left=%d, right=%d' % (angle_left, angle_right))
log.info('test_max_speed: battery=%d, max_speed=%d' % (brick.battery.voltage(), max_speed))
robot.stop()
#!/usr/bin/env pybricks-micropython from pybricks.hubs import EV3Brick from pybricks.tools import wait from pybricks.parameters import Color # Initialize the EV3 ev3 = EV3Brick() # Turn on a red light ev3.light.on(Color.RED) # Wait wait(4000) ev3.light.on(Color.YELLOW) wait(4000) ev3.light.on(Color.GREEN) wait(4000) # Turn the light off ev3.light.off() wait(4000)
def BlueMission(
): # Blue Run (Step Counter, Pull-Up Bar, Boccia Aim, Slide, Health Unit - 1)
#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
InfraredSensor, UltrasonicSensor,
GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from pybricks.tools import wait, StopWatch, DataLog
from pybricks.robotics import DriveBase
from pybricks.media.ev3dev import SoundFile, ImageFile
# This program requires LEGO EV3 MicroPython v2.0 or higher.
# Click "Open user guide" on the EV3 extension tab for more information.
#define your variables
ev3 = EV3Brick()
left_motor = Motor(Port.C)
right_motor = Motor(Port.B)
medium_motor = Motor(Port.A)
large_motor = Motor(Port.D)
wheel_diameter = 56
axle_track = 115
line_sensor = ColorSensor(Port.S2)
line_sensor1 = ColorSensor(Port.S3)
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
# Go towards the step counter mission from base
robot.settings(800) # Speed Change
robot.straight(650)
robot.stop(Stop.BRAKE)
wait(20)
# Slow the robot down to succesfully push the step counter.
robot.settings(200)
# Slowly pushes the step counter by going backward and forward a couple times to increase reliability.
robot.straight(230)
robot.straight(-20)
robot.straight(50)
robot.stop(Stop.BRAKE)
'''
robot.straight(-45)
robot.stop(Stop.BRAKE)
robot.straight(120)
robot.stop(Stop.BRAKE)
'''
robot.straight(-60)
robot.stop(Stop.BRAKE)
# The robot then turns and goes backwards until the right color sensor detects black.
#robot.settings(250,300,250,300)
robot.turn(45)
robot.straight(-100)
while True:
robot.drive(-100, 0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
#The large motor attatchment comes down at the same time the robot takes a turn towards
#the black line underneath the pull up bar
left_motor.run_angle(50, -300, then=Stop.HOLD, wait=True)
# The robot then goes straight towards the line under the pull-up bar.
robot.straight(120)
robot.stop(Stop.BRAKE)
# Robot continues to go forwards until the left color sensor detects black.
while True:
robot.drive(115, 0)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
right_motor.run_angle(100, 150, then=Stop.HOLD, wait=True)
# The robot turns using the right motor until it detects black.
while True:
right_motor.run(100)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.straight(-90)
large_motor.run_angle(100, 150, then=Stop.HOLD, wait=True)
robot.stop(Stop.BRAKE)
robot.stop(Stop.BRAKE)
while True:
right_motor.run(40)
if line_sensor.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
ev3.speaker.beep()
BLACK = 9
WHITE = 85
threshold = (BLACK + WHITE) / 2
# Set the drive speed at 100 millimeters per second.
DRIVE_SPEED = 100
# Set the gain of the proportional line controller. This means that for every
PROPORTIONAL_GAIN = 1.2
runWhile = True
robot.reset()
ev3.speaker.beep()
while True:
# Calculate the deviation from the threshold.
deviation = line_sensor.reflection() - threshold
# Calculate the turn rate.
turn_rate = PROPORTIONAL_GAIN * deviation
# Set the drive base speed and turn rate.
robot.drive(DRIVE_SPEED, turn_rate)
wait(10)
print(line_sensor1.color())
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
robot.straight(110)
ev3.speaker.beep()
large_motor.run_angle(100, -150, then=Stop.HOLD, wait=False)
robot.stop(Stop.BRAKE)
robot.turn(110)
BLACK = 9
WHITE = 85
threshold = (BLACK + WHITE) / 2
# Set the drive speed at 100 millimeters per second.
DRIVE_SPEED = 100
# Set the gain of the proportional line controller. This means that for every
PROPORTIONAL_GAIN = 1.2
runWhile = True
robot.reset()
ev3.speaker.beep()
while True:
# Calculate the deviation from the threshold.
deviation = line_sensor.reflection() - threshold
# Calculate the turn rate.
turn_rate = PROPORTIONAL_GAIN * deviation
# Set the drive base speed and turn rate.
robot.drive(DRIVE_SPEED, turn_rate)
wait(10)
print(line_sensor.color())
if robot.distance() >= 500:
robot.stop(Stop.BRAKE)
break
ev3.speaker.beep(3)
while True:
robot.drive(40, 0)
if line_sensor1.color() == Color.BLACK:
robot.stop(Stop.BRAKE)
break
robot.stop(Stop.BRAKE)
ev3.speaker.beep()
robot.turn(-100)
robot.straight(60)
large_motor.run_angle(300, 150, then=Stop.HOLD, wait=True)
def reset_YAxis(self):
self.y_Motor.run(300)
while (self.resetY_Sensor.pressed() == False):
wait(10)
self.y_Motor.stop()
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()
def click_center(self, duration=100):
self.press_center(False)
self.press_center(True)
wait(duration)
self.press_center(False)
def move_rack_up(rack_motor, degrees):
log_string('Angle at start ' + str(rack_motor.angle()))
wait(100)
rack_motor.run_angle(90, degrees, Stop.BRAKE)
log_string('Angle at end ' + str(rack_motor.angle()))
def click_bluetooth(self, duration=200):
self.press_bluetooth(False)
self.press_bluetooth(True)
wait(duration)
self.press_bluetooth(False)
def move_crane_down(crane_motor, degrees):
log_string('Down Angle at start ' + str(crane_motor.angle()))
wait(100)
crane_motor.run_angle(90, -1 * degrees)
log_string('down Angle at end ' + str(crane_motor.angle()))
def click_right(self, duration=100):
self.press_right(False)
self.press_right(True)
wait(duration)
self.press_right(False)
BELLY = 365
HEAD = 128
ARMS = 244
NECK = 521
BASE = 697
# Place all the elements
for element in (FEET, BELLY, ARMS, NECK, HEAD):
# Go to the element
belt.run_target(speed=200, target_angle=element)
# Grab the element
arm.run_time(speed=300, time=2000)
# Lift the element by going back to nearly zero
arm.run_target(speed=300, target_angle=55)
# Go to the base
belt.run_target(speed=200, target_angle=BASE)
wait(500)
# Put the element down
arm.run_time(speed=300, time=2000)
# Lift the arm back up
arm.run_target(speed=300, target_angle=0)
# When we are done, eject the result
belt.run_target(speed=200, target_angle=FEET)
def activate_dfu(self):
self.press_bluetooth(True)
wait(600)
self.insert_usb(True)
wait(8000)
self.press_bluetooth(False)
GO_MOTOR.stop()
BRICK.speaker.play_file(file=SoundFile.ERROR_ALARM)
for i in range(3):
GO_MOTOR.run_angle(speed=-1000,
rotation_angle=10,
then=Stop.HOLD,
wait=True)
STING_MOTOR.run_angle(speed=-750,
rotation_angle=220,
then=Stop.HOLD,
wait=True)
wait(time=0.1 * 1000)
STING_MOTOR.run_time(speed=-1000, time=1000, then=Stop.HOLD, wait=True)
STING_MOTOR.run_time(speed=400, time=1000, then=Stop.HOLD, wait=True)
# to avoid jerking
wait(time=1000)
GO_MOTOR.run(speed=1000)
BRICK.speaker.play_file(file=SoundFile.ERROR_ALARM)
for i in range(5):
MEDIUM_MOTOR.run_time(speed=750,
time=0.2 * 1000,
from pybricks.pupdevices import Motor from pybricks.parameters import Port, Direction from pybricks.tools import wait # Initialize a motor on port A with the positive direction as counterclockwise. # Also specify one gear train with a 12-tooth and a 36-tooth gear. The 12-tooth # gear is attached to the motor axle. The 36-tooth gear is at the output axle. geared_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE, [12, 36]) # Make the output axle run at 100 degrees per second. The motor speed # is automatically increased to compensate for the gears. geared_motor.run(100) # Wait for three seconds. wait(3000)
# The wheel diameter of the Robot Educator Driving Base is 56 mm.
wheel_diameter = 56
# The axle track is the distance between the centers of each of the
# wheels. This is 118 mm for the Robot Educator Driving Base.
axle_track = 118
# The Driving Base is comprised of 2 motors. There is a wheel on each
# motor. The wheel diameter and axle track values are used to make the
# motors move at the correct speed when you give a drive command.
robot = DriveBase(left_motor, right_motor, wheel_diameter, axle_track)
# Wait until one of the command buttons is pressed.
while not any(b in brick.buttons() for b in COMMAND_BUTTONS):
wait(10)
# Store the pressed button as the drive command.
drive_command = brick.buttons()[0]
brick.sound.file(SoundFile.CLICK)
# Wait 2 seconds and then play a sound to indicate that the robot is
# about to drive.
wait(2000)
brick.sound.file(SoundFile.GO)
wait(1000)
# Now drive the robot using the drive command. Depending on which
# button was pressed, drive in a different way.
# The robot turns 90 degrees to the left.
t = Thread(target=wiiInput)
t.start()
## Otherwise run PS4Input THread is a PS4 Controller is connected
elif controllerName == "PS4 Controller":
t = Thread(target=ps4Input)
t.start()
while True:
## Wait until the character is selected before begining item selection
if characterSelected == 1:
## Generate item every 10 seconds
wait(10000)
item = random.randint(1, 3)
## Boost is 1, Slow is 2, Spin is 3
if item == 1:
EV3Brick().speaker.play_file('starman.wav')
wait(1000)
## Slow Down for 5 seconds
if item == 2:
EV3Brick().speaker.play_notes(['E2/4', 'E2/4', 'E2/4'], tempo=320)
wait(3000)
## Spin Once (set to 0 to stop spinning)
if item == 3:
left_leg_motor.run(500)
right_leg_motor.run(500)
wait(275)
left_leg_motor.stop()
right_leg_motor.stop()
wait(275)
left_leg_motor.run(-50)
right_leg_motor.run(-50)
wait(275)
left_leg_motor.stop()
right_leg_motor.stop()
"""The following code cycles through all of the behaviors, separated by beeps."""
act_playful()
wait(1000)
ev3.speaker.beep()
act_happy()
wait(1000)
ev3.speaker.beep()
act_hungry()
wait(1000)
ev3.speaker.beep()
act_angry()
wait(1000)
ev3.speaker.beep()
go_to_bathroom()
wait(1000)
ev3.speaker.beep()
go_to_sleep()
wait(1000)
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()
def walk_steps():
"""Makes the puppy walk a certain number of steps.
Modify front wheels to roll by removing anchoring pegs and switching pegs through the axle to non-friction pegs.
Change steps to adjust the number of steps."""
#steps equals number of steps pup takes
steps = 10
#puppy stands up
left_leg_motor.run_target(100, 25, wait=False)
right_leg_motor.run_target(100, 25)
while not left_leg_motor.control.target_tolerances():
wait(100)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
while not left_leg_motor.control.target_tolerances():
wait(100)
wait(500)
#puppy walks specified number of steps
for value in range(1, steps + 1):
left_leg_motor.run_target(-100, 25, wait=False)
right_leg_motor.run_target(-100, 25)
while not left_leg_motor.control.target_tolerances():
wait(300)
left_leg_motor.run_target(100, 65, wait=False)
right_leg_motor.run_target(100, 65)
while not left_leg_motor.control.target_tolerances():
wait(300)
left_leg_motor.run_target(50, 65, wait=False)
right_leg_motor.run_target(50, 65)
wait(100)
def run(self):
"""This is the main program run loop."""
self.sit_down()
self.adjust_head()
self.eyes = self.SLEEPING_EYES
self.reset()
#self.eyes = self.SLEEPING_EYES
"""The following code cycles through all of the behaviors, separated by beeps."""
self.act_playful()
wait(1000)
self.ev3.speaker.beep()
self.act_happy()
wait(1000)
self.ev3.speaker.beep()
self.act_hungry()
wait(1000)
self.ev3.speaker.beep()
self.act_angry()
wait(1000)
self.ev3.speaker.beep()
self.go_to_bathroom()
wait(1000)
self.ev3.speaker.beep()
self.go_to_sleep()
wait(1000)
self.ev3.speaker.beep()
self.wake_up()
wait(1000)
self.ev3.speaker.beep()
self.walk_steps()
wait(1000)
self.ev3.speaker.beep()
self.walk_timed()
wait(1000)
self.ev3.speaker.beep()
self.idle()
wait(1000)
self.ev3.speaker.beep()
