# Write your program here
brick.sound.beep()
# Global Variables for Jarvis
pi = 3.141592653589793238462643383279502884197169399375105820974944592307816406286
radius = 2.8 / 100
diameter = 2 * pi * radius
max_speed = 300
max_acceleration = 500
motor = Motor(Port.A)
# set max speed and max acceleration for motor for all motor calls
motor.set_run_settings(max_speed, max_acceleration)
# My first project yay!!!!!!!!!!!!!
def move(distance, velocity):
angle = distance * 360 /diameter
angular_velocity = velocity / radius
motor.run_target(angular_velocity, angle) # time is references in milliseconds
#
# Movement Code
move(0.3 0,10)
# Configure the track motor on Port D. gripper_motor = Motor(Port.A) trackMotor = Motor(Port.D) # Configure the elbow motor. It has an 8-teeth and a 40-teeth gear # connected to it. We would like positive speed values to make the # arm go upward. This corresponds to counterclockwise rotation # of the motor. elbow_motor = Motor(Port.B, Direction.COUNTERCLOCKWISE, [8, 40]) #Same es elbow just different values for gears. base_motor = Motor(Port.C, Direction.COUNTERCLOCKWISE, [12, 36]) # Limit the elbow and base accelerations. This results in # very smooth motion. elbow_motor.set_run_settings(50, 120) base_motor.set_run_settings(50, 120) # Set up the Touch Sensor. It defines the starting point of the base. base_switch = TouchSensor(Port.S1) # Set up the Color Sensor. This sensor detects when the elbow # is in the starting position. This is when the sensor sees the # white beam up close. elbow_sensor = ColorSensor(Port.S3) ################################### # INITIALIZATION # ################################### # Initialize the elbow. First make it go down for 500 ms.
logfile = open ("log4.csv", "w") # open data log file
logfile.write("seconds,motor_angle,gyro_1,gyro_2,diff\n")
done = False # flag to halt program
DTFlag = False
watch = StopWatch()
amplitude = 90
# =================================
mB = Motor(Port.B) # initialize two large motors
mC = Motor(Port.C)
robot = DriveBase(mB, mC, 94, 115) # wheel OD=94mm, wheelbase=115mm
mB.set_run_settings(200, 250) # max speed, max accel
mC.set_run_settings(200, 250) # max speed, max accel
gy = GyroSensor(Port.S3)
gy2 = GyroSensor(Port.S2)
gy.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle)
gy2.mode='GYRO-RATE' # deliver turn-rate (must integrate for angle)
sleep(0.5)
gy.mode='GYRO-ANG' # changing modes causes recalibration
gy2.mode='GYRO-ANG' # changing modes causes recalibration
sleep(3)
gy.reset_angle(0)
gy2.reset_angle(0)
mB.reset_angle(0)
mC.reset_angle(0)
# smoothly between -90 and +90 degrees.
angle_now = sin(seconds)*amplitude
# Make the motor track the given angle
left_motor.track_target(angle_now)
'''
'''
left_motor.dc(40)
right_motor.dc(40)
wait(1000)
left_motor.set_dc_settings(100, 10) #두번째 파라메터 모름
left_motor.dc(3)
right_motor.dc(20)
wait(2000)
'''
#left_motor.dc(100)
#while not left_motor.stalled():
# wait(1)
#left_motor.stop(Stop.BRAKE)
#right_motor.stop(Stop.BRAKE)
#
#left_motor.run_until_stalled(100, Stop.COAST)
# Set the maximum speed to 200 deg/s and acceleration to 400 deg/s/s.
left_motor.set_run_settings(600, 100)
# Make the motor run for 5 seconds. Even though the speed argument is 300
# deg/s in this example, the motor will move at only 200 deg/s because of
# the settings above.
left_motor.run_angle(600, 3600, Stop.BRAKE, False)
right_motor.run_angle(600, 3600, Stop.BRAKE, True)
from pybricks.tools import wait
from pybricks.robotics import DriveBase
# Configure the grabber motor on Port A with default settings.
# Initialize a motor where positive speed values should go clockwise
grabber = Motor(Port.A, Direction.CLOCKWISE, [4, 4])
# Configure the arm motor. It has an 8-teeth and a 40-teeth gear
# connected to it. We would like positive speed values to make the
# arm go upward. This corresponds to clockwise rotation
# of the motor.
arm = Motor(Port.D, Direction.CLOCKWISE,
[8, 40]) # Configure the motor that rotates the base.
base_motor_B = Motor(Port.B)
base_motor_C = Motor(Port.C)
# Limit the arm and base accelerations. This results in
# very smooth motion. Like an industrial robot.
arm.set_run_settings(60, 120)
base_motor_B.set_run_settings(60, 120)
base_motor_C.set_run_settings(60, 120)
wheels = DriveBase(base_motor_B, base_motor_C, 56, 114)
# The colored objects are either red, green, blue, or yellow.
POSSIBLE_COLORS = (Color.RED, Color.GREEN, Color.BLUE, Color.YELLOW)
# Initialize the color sensor
color_sensor = ColorSensor(Port.S4)
# Initialize the arm. First make it go down for one second.
# Then make it go upwards slowly (15 degrees per second) until
# a brick button is pressed. Then reset the motor
# angle to make this the zero point. Finally, hold the motor
# in place so it does not move.
arm.run_time(-30, 1000)
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()
#!/usr/bin/env pybricks-micropython
from pybricks import ev3brick as brick
from pybricks.ev3devices import Motor, TouchSensor, ColorSensor
from pybricks.parameters import Port, Stop, Direction, Button
from pybricks.tools import wait
motor_grip = Motor(Port.A)
motor_arm = Motor(Port.B, Direction.COUNTERCLOCKWISE, [8, 40])
motor_base = Motor(Port.C, Direction.COUNTERCLOCKWISE, [12, 36])
motor_arm.set_run_settings(60, 120)
motor_base.set_run_settings(60, 120)
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)
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()
