def kalibriere(self):
headmotor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
farbsensor = ColorSensor(Port.S3)
headmotor.run_until_stalled(speed=10, duty_limit=50)
debug('winkel=' + str(headmotor.angle()))
headmotor.run_target(speed=10, target_angle=-120, wait=False)
while (farbsensor.reflection() < 10): # & (headmotor.speed() != 0):
debug('farbwert=' + str(farbsensor.reflection()))
time.sleep(0.1)
headmotor.stop()
headmotor.run_angle(speed=10, rotation_angle=15)
debug(str(farbsensor.reflection()))
# winkel auf 0
headmotor.reset_angle(0)
self.angle_ist = 0
self._schreibe_winkel()
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 doLuggage(wheels, cSensor, pickup=True):
global brick_state, blocks_delivered
if pickup:
stapler = Motor(Port.A, Direction.CLOCKWISE)
stapler.run_until_stalled(20, Stop.BRAKE)
brick.sound.file(SoundFile.HORN_1,1000)
brick_state = Status.CARRYING_LUGGAGE
else:
stapler = Motor(Port.A, Direction.COUNTERCLOCKWISE)
You_spin_me_right_round_baby_Right_round_like_a_record_baby_Right_round_round_round(wheels, 180)
stapler.run_angle(50, 100, Stop.BRAKE)
wheels.drive_time(-50, 0, 1000)
stapler.run_angle(50, 25, Stop.BRAKE)
wheels.drive_time(-113/3, 45/3, 1000*3)
wheels.drive_time(0,45/3,1000*3)
blocks_delivered += 1
brick_state = Status.SEARCHING
class SmallMotor:
def __init__(self, ev3, port):
self.ev3 = ev3
self.motor = Motor(port, Direction.COUNTERCLOCKWISE)
self.motor.reset_angle(0)
def reset(self):
self.motor.run_until_stalled(100)
self.motor.run_angle(800, -300)
self.motor.reset_angle(0)
def moveTo(self, angle, speed = 800, wait = False):
print(self.motor.angle())
self.motor.run_target(speed, angle, Stop.HOLD, wait)
print(self.motor.angle())
def move(self, speed = 20):
self.motor.run(speed)
def brake(self):
self.motor.brake()
ev3.screen.print('=run target 90=')
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()
from pybricks.ev3devices import (Motor, TouchSensor, ColorSensor,
InfraredSensor, UltrasonicSensor, GyroSensor)
from pybricks.parameters import Port, Stop, Direction, Button, Color
from math import sin, radians, pi, atan2, degrees
from connection import SpikePrimeStreamReader, SpikeRCWheel
# Create the connection. See README.md to find the address for your SPIKE hub.
wheel = SpikeRCWheel('38:0B:3C:A3:45:0D')
steer_motor = Motor(Port.A)
drive_l = Motor(Port.B)
drive_r = Motor(Port.C)
# Auto center steering wheels.
steer_motor.run_until_stalled(250)
steer_motor.reset_angle(80)
steer_motor.run_target(300,0)
speed = 0
left = 0
# Now you can simply read values!
while True:
steer_motor.track_target(wheel.steering() * -0.5 + wheel.right_thumb())
if wheel.left_paddle() > 15:
speed = wheel.left_paddle() * -10
else:
speed = wheel.right_paddle() * 10
s.connect(('192.168.86.23',50001))
s.send(bytes('one\n', 'utf-8'))
turn_motor = Motor(Port.C, Direction.COUNTERCLOCKWISE, [12,36])
extend_motor = Motor(Port.B, Direction.CLOCKWISE, [8,48])
grip_motor = Motor(Port.A, Direction.CLOCKWISE, [12,8])
turn_sensor = TouchSensor(Port.S1)
extend_sensor = TouchSensor(Port.S2)
brick.sound.beep()
brick.light(Color.GREEN)
#turn_motor.run_time(255,5000,Stop.BRAKE,True)
turn_motor.run_until_stalled(255, Stop.COAST, 50)
brick.sound.file(SoundFile.TWO)
s.send(bytes('two\n', 'utf-8'))
turn_motor.reset_angle(-90)
turn_home_found = False
while not turn_home_found:
turn_motor.run_until_stalled(-255,Stop.COAST,50)
if turn_sensor.pressed() == True:
turn_home_found = True
brick.sound.file(SoundFile.THREE)
s.send(bytes('three\n', 'utf-8'))
break
arm.run_angle(100, -180)
while obstacle_sensor.distance() < 1000:
while obstacle_sensor.distance() < 300:
ev3.light.on(Color.YELLOW)
arm.run_angle(100, 120)
arm.run_angle(100, -120)
wait(10)
ev3.light.on(Color.GREEN)
arm.run_angle(100, 60)
arm.run_angle(100, -60)
ev3.light.on(Color.RED)
arm.run_angle(100, 150)
ev3.light.on(Color.RED)
# beeper()
# move the arm down before starting
arm.run_until_stalled(400)
arm.run_angle(100, -20)
# do somthing with the arm
wag_arm()
ev3.speaker.say("the end")
while opgavenr == 9: #Spil musik for opgave 9
ev3.light.on(Color.RED)
if Section_number < 11:
USSR_Done = 0
Section_to_play = "USSR" + str(Section_number) + ".rsf"
ev3.speaker.play_file(Section_to_play)
Section_number += 1
else:
USSR_Done = 1
break
Music_Thread = Thread(
target=Play_Music) #Tilføje musikfunktionen til en anden thread
Music_Thread.start() #Start musikfunktionen i en anden thread
cMotor.run_until_stalled(-100) #Sæt gripperen til at være helt åben
#Tjek farver
greyLine = colorS.reflection() + 2 # Definer farven grå
robot.turn(-45) # Drej -45 grader
white = colorS.reflection() # Definer farven hvid
robot.turn(45) # Drej 45 grader
threshold = (
greyLine +
white) / 2 # Definer threshold hvor sensor skal skifte imellem grå og hvid
blackLines = greyLine / 3 # Definer sort streg
def drive(x): # Definerer en general drive funktion
deviation = colorS.reflection() - threshold
arm.stop(Stop.HOLD)
# Initialize the base. First move it until the brick button
# in the base is pressed. Reset the motor angle to make this
# the zero point. Then hold the motor in place so it does not move.
wheels.drive_time(60, 0, 2000)
while not (brick.buttons()):
wait(10)
base_motor_B.reset_angle(0)
base_motor_C.reset_angle(0)
base_motor_B.stop(Stop.HOLD)
base_motor_C.stop(Stop.HOLD)
# Initialize the grabber. First rotate the motor until it stalls.
# Stalling means that it cannot move any further. This position
# corresponds to the closed position. Then rotate the motor
# by -90 degrees such that the gripper is open.
grabber.run_until_stalled(200, Stop.COAST, 50)
grabber.reset_angle(0)
grabber.run_target(200, -90)
###############################################################################
#define the path to area of each block
def area(color):
if color == Color.RED:
wheels.drive_time(-60, 0, 2000)
wheels.drive(-100, 90)
wheels.drive_time(-60, 0, 2000)
elif color == Color.BLUE:
wheels.drive_time(60, 0, 2000)
wheels.drive(-60, 90)
class robot:
#status Variables
position = [0, 0]
Clawdir = -1
lista_lobby = []
posicao_lobby = 0
matrix = [[0, 0, 0, 0, 0], [0, 0, 0, 0, 0], [0, 0, 0, 0, 0],
[0, 0, 0, 0, 0], [0, 0, 0, 0, 0]]
#sensors
resetY_Sensor = None
chromoSensor = None
#motors
claw_Motor = None
y_Motor = None
x_Motor1 = None
x_motor2 = None
xAxisMotors = None
def __init__(self):
self.resetY_Sensor = TouchSensor(Port.S2)
self.chromoSensor = ColorSensor(Port.S1)
self.claw_Motor = Motor(Port.A)
self.y_Motor = Motor(Port.B)
self.x_Motor1 = Motor(Port.C)
self.x_Motor1.set_run_settings(200, 100)
self.x_Motor2 = Motor(Port.D)
self.x_Motor2.set_run_settings(200, 100)
self.xAxisMotors = DriveBase(self.x_Motor1, self.x_Motor2, 50, 100)
self.reset()
def reset_Claw(self):
self.claw_Motor.run_until_stalled(1000, Stop.BRAKE, 38)
self.claw_Motor.run_time(5500 * -1, 1700, Stop.BRAKE)
self.Clawdir = -1
def reset_YAxis(self):
self.y_Motor.run(300)
while (self.resetY_Sensor.pressed() == False):
wait(10)
self.y_Motor.stop()
def reset(self):
self.reset_Claw()
self.reset_YAxis()
print("done reset")
def toggleClaw(self):
self.claw_Motor.run_time(5500 * self.Clawdir, 1300, Stop.BRAKE)
self.Clawdir = -self.Clawdir
def yAxis(self, pos):
self.y_Motor.run_angle(100, -217.7 * pos, Stop.BRAKE, True)
def xAxis(self, pos, dir):
self.xAxisMotors.drive_time(dir * 18.75, 0, 1000 * pos * 4)
def moveTo(self, xPos, yPos, resetY=True):
dir = 1
if (resetY):
self.reset_YAxis()
if (xPos - self.position[0] < 0):
dir = -1
self.xAxis(abs(xPos - self.position[0]), dir)
if (not (resetY)):
self.yAxis(yPos - self.position[1])
else:
self.yAxis(yPos + 0.25)
self.position[0] = xPos
self.position[1] = yPos
def moveToLobby(self, pos, resetY=True):
if (pos % 5 == 0):
resetY = True
self.moveTo(pos // 5 + 5, pos % 5, resetY)
def moveToGame(self, xPos, yPos):
if (xPos > 4 or yPos > 4):
quit()
self.moveTo(xPos, yPos, True)
def readColor(self):
while (True):
tempList = []
for x in range(10):
wait(200)
tempList += [self.testecor(self.chromoSensor.rgb())]
tempList = list(dict.fromkeys(tempList))
print(tempList)
if (len(tempList) == 1):
if (tempList[0] != 0):
return tempList[0]
def readLobby(self):
pos = 0
self.moveToLobby(pos, True)
while (len(self.lista_lobby) == 0
or self.lista_lobby[-1] != 5): #le as peças ate ler amarelo
self.lista_lobby.append(self.readColor())
print(self.lista_lobby[:-1])
pos += 1
if (self.lista_lobby[-1] != 5):
self.moveToLobby(pos, False)
self.lista_lobby = self.lista_lobby[:-1]
self.reset_YAxis()
countOfChars = {i: self.lista_lobby.count(i) for i in self.lista_lobby}
print(countOfChars)
global pecasExistentes
global pecasExistentesOld
pecasExistentes += [countOfChars[1]]
pecasExistentes += [countOfChars[2]]
pecasExistentes += [countOfChars[3]]
pecasExistentes += [countOfChars[4]]
pecasExistentesOld = pecasExistentes.copy()
print(self.lista_lobby)
def testecor(self, a):
#print(a)
if (a[0] > a[1] + a[2] and a[0] + a[1] + a[2] > 25):
return 1 #vermelho
elif (a[2] > a[0] + a[1] and a[0] + a[1] + a[2] > 25):
return 2 #azul
elif (a[0] + a[1] + a[2] < 70):
temp = self.chromoSensor.color()
if (temp == 3):
return 3 #verde
elif (temp == 1):
return 4 #preto
else:
return 0 #sem cor
elif (a[0] + a[1] + a[2] < 180):
return 5 #amarelo
def checkFigure(self):
for pattern in figureSequence:
result = match_pattern(self.matrix, globals()[pattern])
#print(result)
print(self.matrix)
if (result != -1):
for x in range(len(result[1])):
for y in range(len(result[1][x])):
if (result[1][x][y] == self.matrix[x + result[0][0]][
y + result[0][1]]):
pecasExistentes[result[1][x][y] - 1] -= 1
self.matrix[x + result[0][0]][y + result[0][1]] = 0
brick.sound.file(SoundFile.DETECTED, VOLUME)
brick.display.text(pattern)
block = True
while (block):
if Button.CENTER in brick.buttons():
block = False
wait(10)
def movePieceTo(self, xPos, yPos):
self.moveToLobby(self.posicao_lobby)
self.matrix[xPos][yPos] = self.lista_lobby[self.posicao_lobby]
self.posicao_lobby += 1
self.toggleClaw()
self.moveToGame(xPos, yPos)
self.claw_Motor.run_time(1000, 1000, Stop.BRAKE)
self.reset_YAxis()
self.reset_Claw()
self.checkFigure()
base_switch = TouchSensor(Port.S1)
arm_sensor = ColorSensor(Port.S3)
motor_arm.run(15)
while arm_sensor.reflection() < 32:
wait(10)
motor_arm.reset_angle(0)
motor_arm.stop(Stop.HOLD)
motor_base.run(-60)
while not base_switch.pressed():
wait(10)
motor_base.reset_angle(0)
motor_base.stop(Stop.HOLD)
motor_grip.run_until_stalled(200, Stop.COAST, 50)
motor_grip.reset_angle(0)
motor_grip.run_target(200, -90)
brick.sound.beeps(1)
def control_motor(button1, button2, motor, limit_reached, speed, speed_unit,
speed_max):
if limit_reached:
brick.sound.beeps(1)
motor.stop(Stop.HOLD)
new_speed = 0
else:
new_speed = speed
if button1 in brick.buttons():
#!/usr/bin/env pybricks-micropython
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)
left_motor.run_until_stalled(360, Stop.BRAKE) #모터의 최대 출력을 제한하지 않습니다.
wait(1000)
left_motor.run_until_stalled(360, Stop.BRAKE, 30) #모터의 최대 출력을 30%로 제한합니다.
wait(1000)
class robot:
#variaveis principais
position = [0,0]
Clawdir = -1
lista_lobby = []
posicao_lobby = 0
matrix = [[0,0,0,0,0],
[0,0,0,0,0],
[0,0,0,0,0],
[0,0,0,0,0],
[0,0,0,0,0]]
#sensors
resetY_Sensor = None
chromoSensor = None
#motors
claw_Motor = None
y_Motor = None
x_Motor1 = None
x_motor2 = None
xAxisMotors = None
#função de inicialização do robo
def __init__(self):
self.resetY_Sensor = TouchSensor(Port.S2)
self.chromoSensor = ColorSensor(Port.S1)
self.claw_Motor = Motor(Port.A)
self.y_Motor = Motor(Port.B)
self.x_Motor1 = Motor(Port.C)
self.x_Motor1.set_run_settings(200, 100)
self.x_Motor2 = Motor(Port.D)
self.x_Motor2.set_run_settings(200, 100)
self.xAxisMotors = DriveBase(self.x_Motor1,self.x_Motor2,50,100)
self.reset()
#metodo para resetar a garra
def reset_Claw(self):
self.claw_Motor.run_until_stalled(1000,Stop.BRAKE,38)
self.claw_Motor.run_time(5500*-1, 1700,Stop.BRAKE)
self.Clawdir = -1
#metodo para resetar o eixo do Y
def reset_YAxis(self):
self.y_Motor.run(300)
while (self.resetY_Sensor.pressed()== False):
wait(10)
self.y_Motor.stop()
#metodo para combinar ambos os resets anteriores
def reset(self):
self.reset_Claw()
self.reset_YAxis()
print("done reset")
#metodo para abrir e fechar a garra
def toggleClaw(self):
self.claw_Motor.run_time(5500*self.Clawdir,1300,Stop.BRAKE)
self.Clawdir = -self.Clawdir
#metodo para o movimento no eixo dos Y
def yAxis(self,pos):
self.y_Motor.run_angle(100,-217.7*pos,Stop.BRAKE,True)
#metodo para o movimento no eixo dos X
def xAxis(self,pos,dir):
self.xAxisMotors.drive_time(dir*18.75, 0, 1000*pos*4)
#combinação de ambos os metodos anteriores
def moveTo(self,xPos,yPos,resetY=True):
dir = 1
if(resetY):
self.reset_YAxis()
if(xPos - self.position[0]<0):
dir = -1
self.xAxis(abs(xPos - self.position[0]),dir)
if(not(resetY)):
self.yAxis(yPos - self.position[1])
else:
self.yAxis(yPos +0.25)
self.position[0]= xPos
self.position[1]= yPos
#metodo para o movimento no lobby onde lê as peças
def moveToLobby(self,pos,resetY = True):
if(pos%5==0):
resetY = True
self.moveTo(pos//5+5,pos%5,resetY)
#metodo para o movimento dentro do campo de jogo
def moveToGame(self,xPos,yPos):
if(xPos>4 or yPos>4):
quit()
self.moveTo(xPos,yPos,True)
#metodo para ler a cor repitindo 10 vezes
def readColor(self):
while(True):
tempList = []
for x in range(10):
wait(200)
tempList += [self.testecor(self.chromoSensor.rgb())]
tempList = list(dict.fromkeys(tempList))
print(tempList)
if(len(tempList)==1):
if(tempList[0]!=0):
return tempList[0]
#metodo para testar a cor individualmente
def testecor(self,a):
#print(a)
if(a[0]>a[1]+a[2] and a[0]+a[1]+a[2]>25):
return 1 #vermelho
elif(a[2]> a[0]+a[1] and a[0]+a[1]+a[2]>25):
return 2 #azul
elif(a[0]+a[1]+a[2]<70):
temp = self.chromoSensor.color()
if(temp==3):
return 3 #verde
elif(temp==1):
return 4 #preto
else:
return 0 #sem cor
elif(a[0]+a[1]+a[2]<180):
return 5 #amarelo
#metodo que combina o movimento e a leitura de cores para ler todo o lobby
def readLobby(self):
pos = 0
self.moveToLobby(pos,True)
while(len(self.lista_lobby)==0 or self.lista_lobby[-1]!=5): #le as peças ate ler amarelo
self.lista_lobby.append(self.readColor())
print(self.lista_lobby[:-1])
pos+=1
if(self.lista_lobby[-1]!=5):
self.moveToLobby(pos,False)
self.lista_lobby = self.lista_lobby[:-1]
self.reset_YAxis()
countOfChars = {i:self.lista_lobby.count(i) for i in self.lista_lobby}
print(countOfChars)
global pecasExistentes
global pecasExistentesOld
pecasExistentes += [countOfChars[1]]
pecasExistentes += [countOfChars[2]]
pecasExistentes += [countOfChars[3]]
pecasExistentes += [countOfChars[4]]
pecasExistentesOld = pecasExistentes.copy()
print(self.lista_lobby)
#metodo que verifica se uma figura foi feita
def checkFigure(self):
#itera sobre todos os padrões e verifica se existe algum
for pattern in figureSequence:
result = match_pattern(self.matrix,globals()[pattern])
print(self.matrix)
if(result != -1):
globals()["score"] += globals()[pattern + "p"]
for x in range(len(result[1])):
for y in range(len(result[1][x])):
if(result[1][x][y]==self.matrix[x+result[0][0]][y+result[0][1]]):
pecasExistentes[result[1][x][y]-1] -= 1
self.matrix[x+result[0][0]][y+result[0][1]] = 0
brick.sound.file(SoundFile.DETECTED,VOLUME)
brick.display.text(pattern)
block = True
while(block):
if Button.CENTER in brick.buttons():
block = False
wait(10)
#metodo que combina os metodos anteriores para mover uma peça para uma posição especifica
def movePieceTo(self, xPos, yPos):
self.moveToLobby(self.posicao_lobby)
self.matrix[xPos][yPos] = self.lista_lobby[self.posicao_lobby]
self.posicao_lobby += 1
self.toggleClaw()
self.moveToGame(xPos,yPos)
self.claw_Motor.run_time(1000,1000,Stop.BRAKE)
self.reset_YAxis()
self.reset_Claw()
self.checkFigure()
#!/usr/bin/env pybricks-micropython
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
Tarttuja = Motor(Port.A)
Tarttuja.run_until_stalled(200, Stop.COAST, 50)
Tarttuja.reset_angle(0)
Tarttuja.run_target(200, -90)
def robot_pick(position):
# Close the gripper to grab the wheel stack.
Tarttuja.run_until_stalled(200, Stop.HOLD, 50)
def robot_release(position):
# Open the gripper to release the wheel stack.
Tarttuja.run_target(200, -90)
LEFT = 160
MIDDLE = 100
RIGHT = 40
return inches*25.4
# Initialize our drive motors and a drive base
# For the arm motor, we want positive values to be up and negative to be down.
# The way that we're configured, we have to reverse the direction passed to
# the motor initialization to achieve this.
arm_motor = Motor(ARM_MOTOR_PORT, Direction.COUNTERCLOCKWISE)
left_motor = Motor(LEFT_MOTOR_PORT)
right_motor = Motor(RIGHT_MOTOR_PORT)
# Let's initialize our arm motor by putting it all the way down until it
# stalls. With our build, it will stall when it hits the supporting arms
# for the color sensor and touch sensor (so it doesn't hit the ground - it's
# still slightly in the air.)
arm_motor.stop()
arm_motor.run_until_stalled(-180)
arm_motor.stop()
# Initialize our drive base and drive 24.0 inches at 5 inches/sec.
robot = MyDriveBase(left_motor=left_motor, right_motor=right_motor,
wheel_diameter=WHEEL_DIAMETER_MM, axle_track=AXLE_TRACK_MM)
robot.drive_distance(inches=24.0, speed=5.0, steering=0)
# Raise the arm until we stall, which will hopefully raise the lever
# at the base of the crane.
arm_motor.stop()
arm_motor.run_until_stalled(90)
arm_motor.stop()
# Sleep for a little bit while the arm is raised so that the block
# can fall all the way down to rest on the other block.
class SpikeMonitor:
def __init__(self):
# Initialize devices.
self.ev3 = EV3Brick()
self.usb_motor = Motor(Port.D)
self.left_motor = Motor(Port.B)
self.right_motor = Motor(Port.A)
# Relax target tolerances so the motion is considered complete even
# if off by a few more degrees than usual. This way, it won't block.
# But set speed tolerance strict, so we move at least until fully
# stopped, which is when we are pressing the button.
self.left_motor.control.target_tolerances(speed=0, position=30)
self.right_motor.control.target_tolerances(speed=0, position=30)
# Run all motors to end points.
self.targets = {
'usb_in':
self.usb_motor.run_until_stalled(-SPEED, duty_limit=50) + 10,
'usb_out':
self.usb_motor.run_until_stalled(SPEED, duty_limit=50) - 10,
'center_pressed':
self.left_motor.run_until_stalled(-SPEED, duty_limit=50) + 10,
'left_pressed':
self.left_motor.run_until_stalled(SPEED, duty_limit=50),
'right_pressed':
self.right_motor.run_until_stalled(SPEED, duty_limit=50) + 10,
'bluetooth_pressed':
self.right_motor.run_until_stalled(-SPEED, duty_limit=50) - 10,
}
# Set other targets between end points.
self.targets['left_released'] = (self.targets['left_pressed'] +
self.targets['center_pressed']) / 2
self.targets['center_released'] = self.targets['left_released']
self.targets['right_released'] = (
self.targets['right_pressed'] +
self.targets['bluetooth_pressed']) / 2
self.targets['bluetooth_released'] = self.targets['right_released']
# Get in initial state.
self.press_center(False)
self.press_bluetooth(False)
self.insert_usb(False)
# Turn the hub off.
self.shutdown()
self.ev3.speaker.beep()
def insert_usb(self, insert):
key = 'usb_in' if insert else 'usb_out'
self.usb_motor.run_target(SPEED, self.targets[key])
def press_left(self, press):
if press:
self.left_motor.run_target(SPEED, self.targets['left_pressed'])
self.left_motor.dc(80)
else:
while abs(self.left_motor.speed()) > 100:
wait(10)
self.left_motor.run_target(SPEED, self.targets['left_released'],
Stop.COAST)
def press_center(self, press):
if press:
self.left_motor.run_target(SPEED, self.targets['center_pressed'])
else:
self.left_motor.run_target(SPEED, self.targets['center_released'],
Stop.COAST)
def press_right(self, press):
if press:
self.right_motor.run_target(SPEED, self.targets['right_pressed'])
else:
self.right_motor.run_target(SPEED, self.targets['right_released'],
Stop.COAST)
def press_bluetooth(self, press):
if press:
self.right_motor.run_target(SPEED,
self.targets['bluetooth_pressed'])
self.right_motor.dc(-100)
else:
while abs(self.right_motor.speed()) > 100:
wait(10)
self.right_motor.run_target(SPEED,
self.targets['bluetooth_released'],
Stop.COAST)
def click_center(self, duration=100):
self.press_center(False)
self.press_center(True)
wait(duration)
self.press_center(False)
def click_bluetooth(self, duration=200):
self.press_bluetooth(False)
self.press_bluetooth(True)
wait(duration)
self.press_bluetooth(False)
def click_left(self, duration=100):
self.press_left(False)
self.press_left(True)
wait(duration)
self.press_left(False)
def click_right(self, duration=100):
self.press_right(False)
self.press_right(True)
wait(duration)
self.press_right(False)
def activate_dfu(self):
self.press_bluetooth(True)
wait(600)
self.insert_usb(True)
wait(8000)
self.press_bluetooth(False)
def shutdown(self):
self.click_center(duration=4000)
def test_buttons(self):
while True:
while True:
pressed = self.ev3.buttons.pressed()
if any(pressed):
break
if Button.CENTER in pressed:
self.click_center()
elif Button.UP in pressed:
self.click_bluetooth()
elif Button.LEFT in pressed:
self.click_left()
elif Button.RIGHT in pressed:
self.click_right()
elif Button.DOWN in pressed:
break
while any(self.ev3.buttons.pressed()):
wait(10)
class EV3Game:
N_LEVELS = 9
N_OFFSET_DEGREES_FOR_HOLD_CUP = 60
N_SHUFFLE_SECONDS = 15
def __init__(self,
b_motor_port: Port = Port.B,
c_motor_port: Port = Port.C,
grip_motor_port: Port = Port.A,
touch_sensor_port: Port = Port.S1,
ir_sensor_port: Port = Port.S4,
ir_beacon_channel: int = 1):
self.ev3_brick = EV3Brick()
self.b_motor = Motor(port=b_motor_port,
positive_direction=Direction.CLOCKWISE)
self.c_motor = Motor(port=c_motor_port,
positive_direction=Direction.CLOCKWISE)
self.grip_motor = Motor(port=grip_motor_port,
positive_direction=Direction.CLOCKWISE)
self.touch_sensor = TouchSensor(port=touch_sensor_port)
self.ir_sensor = InfraredSensor(port=ir_sensor_port)
self.ir_beacon_channel = ir_beacon_channel
def calibrate_grip(self):
self.grip_motor.run_until_stalled(speed=-100,
then=Stop.HOLD,
duty_limit=None)
self.grip_motor.run_angle(speed=100,
rotation_angle=30,
then=Stop.HOLD,
wait=True)
def display_level(self):
self.ev3_brick.screen.clear()
self.ev3_brick.screen.print('Level {}'.format(self.level))
wait(300)
def start_up(self):
self.ev3_brick.light.on(color=Color.RED)
self.calibrate_grip()
self.level = 1
self.display_level()
self.choice = 2
self.current_b = self.current_c = 1
def select_level(self):
while not self.touch_sensor.pressed():
ir_buttons_pressed = \
set(self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_buttons_pressed.intersection(
{Button.LEFT_UP, Button.RIGHT_UP}) and \
(self.level < self.N_LEVELS):
self.level += 1
self.display_level()
elif ir_buttons_pressed.intersection(
{Button.LEFT_DOWN, Button.RIGHT_DOWN}) and \
(self.level > 1):
self.level -= 1
self.display_level()
self.ev3_brick.speaker.play_file(file=SoundFile.GO)
def move_1_rotate_b(self):
if self.current_b == 1:
self.rotate_b = self.N_OFFSET_DEGREES_FOR_HOLD_CUP + 180
elif self.current_b == 2:
self.rotate_b = 2 * self.N_OFFSET_DEGREES_FOR_HOLD_CUP + 180
elif self.current_b == 3:
self.rotate_b = 180
def move_1_rotate_c(self):
if self.current_c == 1:
self.rotate_c = 0
elif self.current_c == 2:
self.rotate_c = -self.N_OFFSET_DEGREES_FOR_HOLD_CUP
elif self.current_c == 3:
self.rotate_c = self.N_OFFSET_DEGREES_FOR_HOLD_CUP
def move_1(self):
self.move_1_rotate_b()
self.move_1_rotate_c()
self.current_b = 3
self.current_c = 1
def move_2_rotate_b(self):
if self.current_b == 1:
self.rotate_b = -self.N_OFFSET_DEGREES_FOR_HOLD_CUP - 180
elif self.current_b == 2:
self.rotate_b = -180
elif self.current_b == 3:
self.rotate_b = -2 * self.N_OFFSET_DEGREES_FOR_HOLD_CUP - 180
move_2_rotate_c = move_1_rotate_c
def move_2(self):
self.move_2_rotate_b()
self.move_2_rotate_c()
self.current_b = 2
self.current_c = 1
def move_3_rotate_b(self):
if self.current_b == 1:
self.rotate_b = 0
elif self.current_b == 2:
self.rotate_b = self.N_OFFSET_DEGREES_FOR_HOLD_CUP
elif self.current_b == 3:
self.rotate_b = -self.N_OFFSET_DEGREES_FOR_HOLD_CUP
def move_3_rotate_c(self):
if self.current_c == 1:
self.rotate_c = self.N_OFFSET_DEGREES_FOR_HOLD_CUP + 180
elif self.current_c == 2:
self.rotate_c = 180
elif self.current_c == 3:
self.rotate_c = 2 * self.N_OFFSET_DEGREES_FOR_HOLD_CUP + 180
def move_3(self):
self.move_3_rotate_b()
self.move_3_rotate_c()
self.current_b = 1
self.current_c = 2
move_4_rotate_b = move_3_rotate_b
def move_4_rotate_c(self):
if self.current_c == 1:
self.rotate_c = -self.N_OFFSET_DEGREES_FOR_HOLD_CUP - 180
elif self.current_c == 2:
self.rotate_c = -2 * self.N_OFFSET_DEGREES_FOR_HOLD_CUP - 180
elif self.current_c == 3:
self.rotate_c = -180
def move_4(self):
self.move_4_rotate_b()
self.move_4_rotate_c()
self.current_b = 1
self.current_c = 3
def execute_move(self):
speed = 100 * self.level
if self.current_b == 1:
self.b_motor.run_angle(speed=speed,
rotation_angle=self.rotate_b,
then=Stop.HOLD,
wait=True)
self.c_motor.run_angle(speed=speed,
rotation_angle=self.rotate_c,
then=Stop.HOLD,
wait=True)
else:
assert self.current_c == 1
self.c_motor.run_angle(speed=speed,
rotation_angle=self.rotate_c,
then=Stop.HOLD,
wait=True)
self.b_motor.run_angle(speed=speed,
rotation_angle=self.rotate_b,
then=Stop.HOLD,
wait=True)
def update_ball_cup(self):
if self.move in {1, 2}:
if self.cup_with_ball == 1:
self.cup_with_ball = 2
elif self.cup_with_ball == 2:
self.cup_with_ball = 1
else:
if self.cup_with_ball == 2:
self.cup_with_ball = 3
elif self.cup_with_ball == 3:
self.cup_with_ball = 2
def shuffle(self):
shuffle_start_time = time()
while time() - shuffle_start_time < self.N_SHUFFLE_SECONDS:
self.move = randint(1, 4)
if self.move == 1:
self.move_1()
elif self.move == 2:
self.move_2()
elif self.move == 3:
self.move_3()
elif self.move == 4:
self.move_4()
self.execute_move()
self.update_ball_cup()
def reset_motor_positions(self):
"""
Resetting motors' positions like it is done when the moves finish
"""
# Resetting Motor B to Position 1,
# which, for Motor B corresponds to Move 3
self.move_3_rotate_b()
# Reseting Motor C to Position 1,
# which, for Motor C corresponds to Move 1
self.move_1_rotate_c()
self.current_b = self.current_c = 1
# Executing the reset for both motors
self.execute_move()
def select_choice(self):
self.choice = None
while not self.choice:
ir_buttons_pressed = \
set(self.ir_sensor.buttons(channel=self.ir_beacon_channel))
if ir_buttons_pressed == {Button.LEFT_UP}:
self.choice = 1
elif ir_buttons_pressed == {Button.BEACON}:
self.choice = 2
# wait for BEACON button to turn off
while set(self.ir_sensor.buttons(
channel=self.ir_beacon_channel)) \
== {Button.BEACON}:
wait(10)
elif ir_buttons_pressed == {Button.RIGHT_UP}:
self.choice = 3
def cup_to_center(self):
# Saving a copy of the current Level
self.level_copy = self.level
# Using Level 1 to rotate the chosen cup to the center
self.level = 1
if self.choice == 1:
self.move = 1
self.move_1()
self.execute_move()
self.update_ball_cup()
elif self.choice == 3:
self.move = 3
self.move_3()
self.execute_move()
self.update_ball_cup()
self.reset_motor_positions()
# Restoring previous value of Level
self.level = self.level_copy
def lift_cup(self):
self.grip_motor.run_angle(speed=100,
rotation_angle=220,
then=Stop.HOLD,
wait=True)
def reset(motor: Motor):
motor.run_until_stalled(100, Stop.BRAKE, 50)
grab_ball()
def random_negate():
return [-1, 1][random.randrange(2)]
def check_container_full():
return color_sensor_ramp.color() != calibration_ramp
global last_direction_change
last_direction_change = 0
# reset axes
motor_arm.run_until_stalled(100, Stop.COAST, 30)
motor_arm.reset_angle(0)
motor_grabber.run_until_stalled(100, Stop.COAST, 30)
motor_grabber.reset_angle(0)
calibration_surface = color_sensor_floor.color()
calibration_ramp = color_sensor_ramp.color()
while ball_count < 4:
global last_direction_change
collision_avoidance()
check_ball()
if check_container_full():
break
# run every 1s
def scan(motor: Motor):
global scan_done
global scan_started
scan_started = True
motor.run_until_stalled(-50, Stop.BRAKE, 25)
scan_done = True
elbow_motor.hold()
# Initialize the base. First rotate it until the Touch Sensor
# in the base is pressed. Reset the motor angle to make this
# the zero point. Then hold the motor in place so it does not move.
base_motor.run(-60)
while not base_switch.pressed():
wait(10)
base_motor.reset_angle(0)
base_motor.hold()
# Initialize the gripper. First rotate the motor until it stalls.
# Stalling means that it cannot move any further. This position
# corresponds to the closed position. Then rotate the motor
# by 90 degrees such that the gripper is open.
gripper_motor.run_until_stalled(200, then=Stop.COAST, duty_limit=50)
gripper_motor.reset_angle(0)
gripper_motor.run_target(200, -90)
def robot_pick(position):
# This function makes the robot base rotate to the indicated
# position. There it lowers the elbow, closes the gripper, and
# raises the elbow to pick up the object.
# Rotate to the pick-up position.
base_motor.run_target(60, position)
# Lower the arm.
elbow_motor.run_target(60, -40)
# Close the gripper to grab the wheel stack.
gripper_motor.run_until_stalled(200, then=Stop.HOLD, duty_limit=50)
# Initialize the Touch Sensor. It is used to detect when the belt motor
# has moved the sorter module all the way to the left.
touch_sensor = TouchSensor(Port.S1)
# Initialize the Color Sensor. It is used to detect the color of the objects.
color_sensor = ColorSensor(Port.S3)
# This is the main loop. It waits for you to scan and insert 8 colored objects.
# Then it sorts them by color. Then the process starts over and you can scan
# and insert the next set of colored objects.
while True:
# Get the feed motor in the correct starting position.
# This is done by running the motor forward until it stalls. This
# means that it cannot move any further. From this end point, the motor
# rotates backward by 180 degrees. Then it is in the starting position.
feed_motor.run_until_stalled(120)
feed_motor.run_angle(450, -180)
# Get the conveyor belt motor in the correct starting position.
# This is done by first running the belt motor backward until the
# touch sensor becomes pressed. Then the motor stops, and the the angle is
# reset to zero. This means that when it rotates backward to zero later
# on, it returns to this starting position.
belt_motor.run(-500)
while not touch_sensor.pressed():
pass
belt_motor.stop()
wait(1000)
belt_motor.reset_angle(0)
# Clear all the contents from the display.
# Initialize the Touch Sensor. It is used to detect when the belt motor has
# moved the sorter module all the way to the left.
touch_sensor = TouchSensor(Port.S1)
# Initialize the Color Sensor. It is used to detect the color of the objects.
color_sensor = ColorSensor(Port.S3)
# This is the main loop. It waits for you to scan and insert 8 colored objects.
# Then it sorts them by color. Then the process starts over and you can scan
# and insert the next set of colored objects.
while True:
# Get the feed motor in the correct starting position.
# This is done by running the motor forward until it stalls. This
# means that it cannot move any further. From this end point, the motor
# rotates backward by 180 degrees. Then it is in the starting position.
feed_motor.run_until_stalled(120, duty_limit=30)
feed_motor.run_angle(450, -200)
# Get the conveyor belt motor in the correct starting position.
# This is done by first running the belt motor backward until the
# touch sensor becomes pressed. Then the motor stops, and the the angle is
# reset to zero. This means that when it rotates backward to zero later
# on, it returns to this starting position.
belt_motor.run(-500)
while not touch_sensor.pressed():
pass
belt_motor.stop()
wait(1000)
belt_motor.reset_angle(0)
# When we scan the objects, we store all the color numbers in a list.
elbow_motor.stop(Stop.HOLD)
# Initialize the base. First rotate it until the Touch Sensor
# in the base is pressed. Reset the motor angle to make this
# the zero point. Then hold the motor in place so it does not move.
base_motor.run(-60)
while not base_switch.pressed():
wait(10)
base_motor.reset_angle(0)
base_motor.stop(Stop.HOLD)
# Initialize the gripper and track. First rotate the motor until it stalls.
# Stalling means that it cannot move any further. This position
# corresponds to the closed position. Then rotate the motor
# by 90 degrees such that the gripper is open.
gripper_motor.run_until_stalled(200, Stop.COAST, 50)
gripper_motor.reset_angle(0)
gripper_motor.run_target(200, -90)
# Run track (we want track to run continuously)
trackMotor.run(140)
###########################################
# DEFINING FUNCTIONS AND DESTINATIONS #
###########################################
def robot_pick(position):
# This function makes the robot base rotate to the indicated
# position. There it lowers the elbow, closes the gripper, and
# raises the elbow to pick up the object.
