# ------Input--------
print('Setting input values')
power = 40
target = 55
kp = 1 # Proportional gain. Start value 1, float(0.65)
kd = 0 # Derivative gain. Start value 0 ,float(0.02)
ki = 0 #float(0.01) # Integral gain. Start value 0
direction = -1
minRef = 3
maxRef = 26
# -------------------
# Connect two large motors on output ports B and C and check that
# the device is connected using the 'connected' property.
print('Connecting motors')
left_motor = LargeMotor(OUTPUT_B)
right_motor = LargeMotor(OUTPUT_A)
# One left and one right motor should be connected
# Connect color and touch sensors and check that they
# are connected.
print('Connecting sensors')
#ir = InfraredSensor(); assert ir.connected
col = ColorSensor(INPUT_2)
us = UltrasonicSensor(INPUT_1)
# Change color sensor mode
print('Setting color sensor mode')
col.mode = 'COL-REFLECT'
from ev3dev2.sensor.lego import TouchSensor
from ev3dev2.led import Leds
from ev3dev2.sensor.lego import ColorSensor, LightSensor
'''Kp = 520
Ki = 0
Kd = 0
offset = 20
Tp = 30
integral = 0
lastError = 0
dervitave = 0'''
lcolor1 = LightSensor(INPUT_2)
lcolor2 = LightSensor(INPUT_3)
#ccolor = ColorSensor(INPUT_1)
mr = LargeMotor(OUTPUT_A)
ml = LargeMotor(OUTPUT_B)
a = .175
def kalibrierung(a, l1, l2):
return (l2 - l1) * a
d = kalibrierung(a, lcolor1.reflected_light_intensity, lcolor2.reflected_light_intensity)
def firstwhile():
global switch1, switch2
lcolor1 = LightSensor(INPUT_2)
lcolor2 = LightSensor(INPUT_3)
Kp = 140
Ki = 0
Kd = 0
offset = d
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_A, SpeedPercent m = LargeMotor(OUTPUT_A) m.on_for_rotations(SpeedPercent(75), 5)
#!/usr/bin/env python3
# https://github.com/ev3dev/ev3dev-lang-python
# https://github.com/ev3dev/ev3dev-lang-python-demo#balanc3r
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import TouchSensor, ColorSensor, GyroSensor, UltrasonicSensor
from ev3dev2.led import Leds
from ev3dev2.sound import Sound
import math
import time
# Motors
l_motor = LargeMotor(OUTPUT_D)
r_motor = LargeMotor(OUTPUT_A)
# Sensors
color = ColorSensor(INPUT_1)
gyro = GyroSensor(INPUT_2)
ts = TouchSensor(INPUT_3)
us = UltrasonicSensor(INPUT_4)
# Sound
sound = Sound()
# sound.speak('Welcome to the E V 3 dev project!')
def forward(speed):
def moveGreenPlayer( inDistance ):
global GreenCurrent
LargeMotor( OUTPUT_B ).on_for_degrees( SpeedPercent( MotorSpeed ), inDistance, True, False )
GreenCurrent += inDistance
def moveYellowPlayer( inDistance ):
global YellowCurrent
LargeMotor( OUTPUT_D ).on_for_degrees( SpeedPercent( MotorSpeed ), -inDistance, True, False )
YellowCurrent += inDistance
def do_GET(self):
print(self.path)
if self.path.startswith("/ev3/"):
url = urllib.parse.urlparse(self.path)
path = url.path.split("/")
params = urllib.parse.parse_qs(url.query)
if path[2] == "sensor":
portN = params['portName'][0]
if portN == '1':
portN = INPUT_1
elif portN == '2':
portN = INPUT_2
elif portN == '3':
portN = INPUT_3
elif portN == '4':
portN = INPUT_4
type = params['type'][0]
if type == 'touch':
ts = TouchSensor(portN)
return self.send_result(ts.is_pressed)
if type == 'light':
ts = ColorSensor(portN)
return self.send_result(ts.reflected_light_intensity)
if type == 'color':
ts = ColorSensor(portN)
return self.send_result(ts.color)
if type == 'gyro':
ts = GyroSensor(portN)
return self.send_result(ts.angle)
if type == 'ultra':
ts = UltrasonicSensor(portN)
return self.send_result(ts.distance_centimeters)
if path[2] == "tank":
port1 = params['port1'][0]
if port1 == 'A':
port1 = OUTPUT_A
if port1 == 'B':
port1 = OUTPUT_B
if port1 == 'C':
port1 = OUTPUT_C
if port1 == 'D':
port1 = OUTPUT_D
port2 = params['port2'][0]
if port2 == 'A':
port2 = OUTPUT_A
if port2 == 'B':
port2 = OUTPUT_B
if port2 == 'C':
port2 = OUTPUT_C
if port2 == 'D':
port2 = OUTPUT_D
motor = MoveTank(port1, port2)
rotations = float(params['rotations'][0])
speed1 = float(params['speed1'][0])
speed2 = float(params['speed2'][0])
motor.on_for_rotations(speed1, speed2, rotations)
return self.send_result("OK")
if path[2] == "motor":
portN = params['portName'][0]
if portN == 'A':
portN = OUTPUT_A
if portN == 'B':
portN = OUTPUT_B
if portN == 'C':
portN = OUTPUT_C
if portN == 'D':
portN = OUTPUT_D
type = params['type'][0]
print(params)
if type == 'large':
motor = LargeMotor(portN)
if 'seconds' in params and 'speed' in params:
seconds = float(params['seconds'][0])
speed = float(params['speed'][0])
motor.on_for_seconds(SpeedPercent(speed), seconds)
return self.send_result("OK")
if 'rotations' in params and 'speed' in params:
rotations = float(params['rotations'][0])
speed = float(params['speed'][0])
motor.on_for_rotations(SpeedPercent(speed), rotations)
return self.send_result("OK")
if 'run' in params and 'speed' in params:
speed = float(params['speed'][0])
if params['run'][0] == 'true':
motor.on(SpeedPercent(speed))
else:
motor.off()
return self.send_result("OK")
if type == 'medium':
motor = MediumMotor(portN)
if 'seconds' in params and 'speed' in params:
seconds = float(params['seconds'][0])
speed = float(params['speed'][0])
motor.on_for_seconds(SpeedPercent(speed), seconds)
return self.send_result("OK")
if 'rotations' in params and 'speed' in params:
rotations = float(params['rotations'][0])
speed = float(params['speed'][0])
motor.on_for_rotations(SpeedPercent(speed), rotations)
return self.send_result("OK")
return self.send_result("ERROR")
if self.path == "/":
self.path = "snap.html"
SimpleHTTPRequestHandler.do_GET(self)
from ev3dev2.motor import LargeMotor,MediumMotor, OUTPUT_D, OUTPUT_A, OUTPUT_C, OUTPUT_B, follow_for_ms
from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS, SpeedDPM, MoveTank, MoveSteering, SpeedPercent
from ev3dev2.sound import Sound
from time import sleep
from ev3dev2.sensor.lego import ColorSensor
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sound import Sound
import math
import sys
import time
LeftAction = MediumMotor(OUTPUT_A)
RightAction = MediumMotor(OUTPUT_D)
TankPair = MoveTank(OUTPUT_B, OUTPUT_C, motor_class=LargeMotor)
LeftSensor = ColorSensor(INPUT_1)
RightSensor = ColorSensor(INPUT_4)
LeftWheel = LargeMotor(OUTPUT_B)
RightWheel = LargeMotor(OUTPUT_C)
sound = Sound()
def LineFollowing(FastSpeed,SlowSpeed,Degree):
DegreeSum = 0
AngleOld = 360 * RightWheel.position / RightWheel.count_per_rot
while DegreeSum < Degree:
if RightSensor.color == ColorSensor.COLOR_WHITE:
LeftWheel.on(SpeedDPS(FastSpeed))
RightWheel.on(SpeedDPS(SlowSpeed))
else:
RightWheel.on(SpeedDPS(FastSpeed))
LeftWheel.on(SpeedDPS(SlowSpeed))
AngleNew = 360 * RightWheel.position / RightWheel.count_per_rot
DegreeSum = DegreeSum + abs(AngleNew - AngleOld)
# some keyboard control setup and vars tty.setcbreak(sys.stdin) x = 0 # set here to make global pilot_mode = 1 # start in human mode, '1'; '-1' means auto-pilot # some Nav control vars initSearch = False # have we completed the initial search circle, looking for ir beacon? beaconLock = False # has the beacon been found and data read? cmpGenHeading = False # do we have a lock on compass heading for North? beaconSearch = False # are we searching for the beacon right now? cmpSearch = False # are we searching for North with the compass right now? navPrint = 10 # print the Nav msgs every nth cycle i = 0 # iterator for testing # setup motors mL = LargeMotor(OUTPUT_A) time.sleep(0.5) mL.reset() time.sleep(2) mLspd = 0 mL.stop_action = 'coast' mL.polarity = 'inversed' mL.position = 0 mR = LargeMotor(OUTPUT_B) time.sleep(0.5) mR.reset() time.sleep(2) mRspd = 0 mR.stop_action = 'coast'
from time import sleep import math ent_motor_medio = OUTPUT_A ent_motor_grande = OUTPUT_B ent_motor_esq = OUTPUT_C ent_motor_dir = OUTPUT_D ent_us_fr = INPUT_1 ent_us_lat = INPUT_2 ent_sc_esq = INPUT_3 ent_sc_dir = INPUT_4 steering_pair = MoveSteering(ent_motor_esq, ent_motor_dir) elevador_g = LargeMotor(ent_motor_grande) garra_m = MediumMotor(ent_motor_medio) # elevador_g.on_for_degrees(40, -180) # negativo sobe # garra_m.on_for_degrees(40, 96) # positivo abre cor_esq = ColorSensor(ent_sc_esq) cor_dir = ColorSensor(ent_sc_dir) us_lat = UltrasonicSensor(ent_us_lat) us_front = UltrasonicSensor(ent_us_fr) sound = Sound() btn = Button() tam_cano = 0
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D import time import threading armSpeed = 22 leftArm = -21.5 rightArm = 21.5 portA = LargeMotor(OUTPUT_A) portB = LargeMotor(OUTPUT_B) portC = LargeMotor(OUTPUT_C) portD = LargeMotor(OUTPUT_D) # C portA.on_for_degrees(armSpeed, rightArm) portA.on_for_degrees(armSpeed, leftArm) # D portA.on_for_degrees(armSpeed, leftArm) portA.on_for_degrees(armSpeed, rightArm) # E portB.on_for_degrees(armSpeed, rightArm) portB.on_for_degrees(armSpeed, leftArm) # F portB.on_for_degrees(armSpeed, leftArm) portB.on_for_degrees(armSpeed, rightArm)
#!/usr/bin/env python3
#Codigo que faz uma curva p/ direita, segue reto, e faz uma curva p/ esquerda
#importacao da biblioteca de motores
from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, MoveTank, SpeedPercent
#definicoes
tank = MoveTank(OUTPUT_A, OUTPUT_B)
motorA = LargeMotor(OUTPUT_A)
motorB = LargeMotor(OUTPUT_B)
motorC = MediumMotor(OUTPUT_C)
#testes p/ conseguir uma corva de 90* com 1motor + motor C
#90* p/ direita
def dir90():
motorC.on_for_degrees(SpeedPercent(50), 25, block=True)
motorA.on_for_rotations(SpeedPercent(100), 3.8)
motorA.stop()
motorC.on_for_degrees(SpeedPercent(-30), 25)
#90* p/ esquerda
def esq90():
motorC.on_for_degrees(SpeedPercent(-50), 25, block=True)
motorB.on_for_rotations(SpeedPercent(100), 3.8)
motorB.stop()
motorC.on_for_degrees(SpeedPercent(30), 25)
#Execucao:
dir90() #curva p/direita
#!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, MoveDifferential from ev3dev2.motor import SpeedDPS, SpeedRPM, SpeedRPS from ev3dev2.wheel import EV3Tire from time import sleep from ev3dev2.motor import MediumMotor, OUTPUT_A medium_motorA = MediumMotor(OUTPUT_A) large_motorB = LargeMotor(OUTPUT_B) large_motorC = LargeMotor(OUTPUT_C) STUD_MM = 8 mdiff = MoveDifferential(OUTPUT_B, OUTPUT_C, EV3Tire, 16 * STUD_MM) medium_motorA.on_for_degrees(speed=10, degrees=-110) medium_motorA.on_for_degrees(speed=10, degrees=110)
import os.path
# DEFINIÇÕES
ent_motor_esq = OUTPUT_C
ent_motor_dir = OUTPUT_D
ent_motor_grande = OUTPUT_B
ent_motor_medio = OUTPUT_A
ent_sc_esq = INPUT_3
ent_sc_dir = INPUT_4
ent_us_lat = INPUT_2
ent_us_fr = INPUT_1
steering_pair = MoveSteering(ent_motor_esq, ent_motor_dir)
garra_g = LargeMotor(ent_motor_grande)
garra_m = MediumMotor(ent_motor_medio)
cor_esq = ColorSensor(ent_sc_esq)
cor_dir = ColorSensor(ent_sc_dir)
usl = UltrasonicSensor(ent_us_lat)
usf = UltrasonicSensor(ent_us_fr)
sound = Sound()
btn = Button()
# FUNÇÕES
def distancia(sensor):
a1 = sensor.distance_centimeters
sleep(0.1)
def Crane():
Sound.speak("").wait()
MA = MediumMotor("outA")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("outD")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
GY.mode='GYRO-ANG'
GY.mode='GYRO-RATE'
GY.mode='GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 12
# change this to whatever speed what you want
left_wheel_speed = -300
right_wheel_speed = -300
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #wheel alignment
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# change the value to how far you want the robot to go. V
while MB.position > -900:
if GY.value() == 0:
left_wheel_speed = -300
right_wheel_speed = -300
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 9
else:
if GY.value() < 0:
correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement
left_wheel_speed = left_wheel_speed + gyro_adjust
right_wheel_speed = right_wheel_speed - gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
else:
correct_rate = abs (GY.value()) # Same idea as the other if statement
right_wheel_speed = right_wheel_speed + gyro_adjust
left_wheel_speed = left_wheel_speed - gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) > correct_rate:
gyro_adjust = gyro_adjust + 1
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
# wait for the block to drop. V
sleep(1.5)
# pulling away from the crane. V
tank_drive.on_for_rotations(SpeedPercent(20), SpeedPercent(10), 0.50)
# Unlocking attachment. V
MD.on_for_degrees(SpeedPercent(50), 360)
# pulling away from unlocked attachment. V
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 1)
# gyro 90 degree spin turn
while GY.value() < 91:
MB.run_forever(speed_sp=300)
MC.run_forever(speed_sp=-300)
# drive into home
tank_drive.on_for_rotations(SpeedPercent(50), SpeedPercent(50), 4)
MB.stop(stop_action="coast")
MC.stop(stop_action="coast")
Launchrun()
def run(self):
# sensors
cs = ColorSensor()
cs.mode = 'COL-REFLECT' # measure light intensity
# motors
lm = LargeMotor('outC')
rm = LargeMotor('outB')
speed = 360 / 2 # deg/sec, [-1000, 1000]
dt = 500 # milliseconds
stop_action = "coast"
# PID tuning
Kp = 1 # proportional gain
Ki = 0.002 # integral gain
Kd = 0.01 # derivative gain
integral = 0
previous_error = 0
# initial measurment
target_value = 35
# Start the main loop
while not self.shut_down:
# Calculate steering using PID algorithm
error = target_value - cs.value()
if previous_error > 0 and error < 0:
integral = 0
integral += (error * dt)
derivative = (error - previous_error) / dt
# u zero: on target, drive forward
# u positive: too bright, turn right
# u negative: too dark, turn left
u = (Kp * error) + (Ki * integral) + (Kd * derivative)
# limit u to safe values: [-1000, 1000] deg/sec
if speed + abs(u) > 1000:
if u >= 0:
u = 1000 - speed
else:
u = speed - 1000
# run motors
if u >= 0:
lm.run_timed(time_sp=dt,
speed_sp=speed + abs(u),
stop_action=stop_action)
rm.run_timed(time_sp=dt, speed_sp=0, stop_action=stop_action)
sleep(dt / 2000)
else:
lm.run_timed(time_sp=dt, speed_sp=0, stop_action=stop_action)
rm.run_timed(time_sp=dt,
speed_sp=speed + abs(u),
stop_action=stop_action)
sleep(dt / 2000)
previous_error = error
def Traffic():
Sound.speak("").wait()
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
T1 = TouchSensor("")
GY.mode='GYRO-ANG'
GY.mode='GYRO-RATE'
GY.mode='GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 1
starting_value = GY.value()
gyro_correct_loops = 0
straight_correct_loops = 0
gyro_correct_straight = 0
# change this to whatever speed what you want
left_wheel_speed = 100
right_wheel_speed = 100
tank_drive.on_for_rotations(SpeedPercent(100), SpeedPercent(100), 0.01) #wheel alignment
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# change the value to how far you want the robot to go
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 0.675) # Originally 0.95 - the robot starts out from base
while GY.value() < 85: #gyro turns 85 degrees and faces towards the swing
left_wheel_speed = 100
right_wheel_speed = -100
#MB is left wheel & MC is right wheel
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
while MB.position > -1600: # this is the gyro program, the first line tells the bot to continue loop until it reaches a defined position
if GY.value() == 90: #this runs if the gyro is OK and already straight, sets a lot of variables as well
left_wheel_speed = -300
right_wheel_speed = -300
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 8
gyro_correct_loops = 0
gyro_correct_straight = 0
straight_correct_loops = 0
else: #if the gyro is off it runs this section of code
if GY.value() < 90:
correct_rate = abs (GY.value()) # This captures the gyro value at the beginning of the statement
right_wheel_speed = right_wheel_speed - gyro_adjust #changes the wheel speeds accordingly
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
if abs (GY.value()) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
gyro_correct_loops = gyro_correct_loops + 1
if GY.value() == 0 and gyro_correct_straight == 0: #runs only if bot isn't already on the original line it started from
while straight_correct_loops < gyro_correct_loops: #Basically this messes up the gyro again so the bot can correct back to the line it was orignally on
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1 #sets this to 1 so that it doesn't go off the line again
gyro_correct_loops = 0
straight_correct_loops = 0
else:
correct_rate = abs (GY.value()) # Same idea as the other if statement, just reversed
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -300
right_wheel_speed = -300
gyro_correct_loops = gyro_correct_loops + 1
if abs (GY.value()) >= correct_rate:
gyro_adjust = gyro_adjust + 1
if GY.value() == 0 and gyro_correct_straight == 0:
while straight_correct_loops < gyro_correct_loops:
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1
gyro_correct_loops = 0
straight_correct_loops = 0
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
while GY.value() > 19: #this turns the bot towards the circle, need to be less than straight 90 degrees
left_wheel_speed = -100
right_wheel_speed = 100
#MB is left wheel & MC is right wheel
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1.35) #goes forward (2.5) slightly to move the block into tan
tank_drive.on_for_rotations(SpeedPercent(30), SpeedPercent(30), 0.7) #drives back a bit (1)
while GY.value() < 145: #turns to face the bridge backwards
left_wheel_speed = 100 #originaly 200
right_wheel_speed = -100 #originaly 200
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
# Drives up bridge
tank_drive.on_for_rotations(SpeedPercent(70), SpeedPercent(70), 3) #Onto the bridge!!
while True:
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
def moveRedPlayer( inDistance ):
global RedCurrent
LargeMotor( OUTPUT_A ).on_for_degrees( SpeedPercent( MotorSpeed ), -inDistance, True, False )
RedCurrent += inDistance
#!/usr/bin/env micropython
from ev3dev2.motor import LargeMotor, OUTPUT_B
LARGE_MOTOR = LargeMotor(address=OUTPUT_B)
LARGE_MOTOR.on_for_seconds(speed=50.0, seconds=4.0, block=True, brake=True)
LARGE_MOTOR.on_for_seconds(speed=100.0, seconds=4.0, block=True, brake=True)
LARGE_MOTOR.on_for_seconds(speed=-100.0, seconds=4.0, block=True, brake=True)
LARGE_MOTOR.on_for_rotations(speed=100.0,
rotations=5.0,
block=True,
brake=True)
def moveBluePlayer( inDistance ):
global BlueCurrent
LargeMotor( OUTPUT_C ).on_for_degrees( SpeedPercent( MotorSpeed ), inDistance, True, False )
BlueCurrent += inDistance
# Make sure the same address is set in Pixy2 PIXY2_I2C_ADDRESS = 0x54 # Signature of the ball configured in PixyMon BALL_SIGNATURE = 1 # Data for requesting block GET_OBJECTS_COMMAND = [174, 193, 32, 2, BALL_SIGNATURE, 1] # The y position of the ball when it reaches the goal - I found this value by opening PixyMon and hovering # my mouse over the goalie line on the board. PixyMon shows the coordinates of the mouse, so I used # the y-position of the mouse as Ye. Ye = 190 # Sets the lego port of the large motor to port C lm = LargeMotor(OUTPUT_C) #Where the robot starts BALL_START = 170 # Set LEGO port for Pixy2 on input port 1 in1 = LegoPort(INPUT_1) in1.mode = 'other-i2c' # Short wait for the port to get ready time.sleep(0.5) # Settings for I2C (SMBus(3) for INPUT_1) bus = SMBus(3)
class MindCuber(object):
scan_order = [
5, 9, 6, 3, 2, 1, 4, 7, 8, 23, 27, 24, 21, 20, 19, 22, 25, 26, 50, 54,
51, 48, 47, 46, 49, 52, 53, 14, 10, 13, 16, 17, 18, 15, 12, 11, 41, 43,
44, 45, 42, 39, 38, 37, 40, 32, 34, 35, 36, 33, 30, 29, 28, 31
]
hold_cube_pos = 85
rotate_speed = 400
flip_speed = 300
flip_speed_push = 400
def __init__(self):
self.shutdown = False
self.flipper = LargeMotor(address=OUTPUT_A)
self.turntable = LargeMotor(address=OUTPUT_B)
self.colorarm = MediumMotor(address=OUTPUT_C)
self.color_sensor = ColorSensor()
self.color_sensor.mode = self.color_sensor.MODE_RGB_RAW
self.infrared_sensor = InfraredSensor()
self.init_motors()
self.state = ['U', 'D', 'F', 'L', 'B', 'R']
self.rgb_solver = None
signal.signal(signal.SIGTERM, self.signal_term_handler)
signal.signal(signal.SIGINT, self.signal_int_handler)
filename_max_rgb = 'max_rgb.txt'
if os.path.exists(filename_max_rgb):
with open(filename_max_rgb, 'r') as fh:
for line in fh:
(color, value) = line.strip().split()
if color == 'red':
self.color_sensor.red_max = int(value)
log.info("red max is %d" % self.color_sensor.red_max)
elif color == 'green':
self.color_sensor.green_max = int(value)
log.info("green max is %d" %
self.color_sensor.green_max)
elif color == 'blue':
self.color_sensor.blue_max = int(value)
log.info("blue max is %d" % self.color_sensor.blue_max)
def init_motors(self):
for x in (self.flipper, self.turntable, self.colorarm):
x.reset()
log.info("Initialize flipper %s" % self.flipper)
self.flipper.on(SpeedDPS(-50), block=True)
self.flipper.off()
self.flipper.reset()
log.info("Initialize colorarm %s" % self.colorarm)
self.colorarm.on(SpeedDPS(500), block=True)
self.colorarm.off()
self.colorarm.reset()
log.info("Initialize turntable %s" % self.turntable)
self.turntable.off()
self.turntable.reset()
def shutdown_robot(self):
log.info('Shutting down')
self.shutdown = True
if self.rgb_solver:
self.rgb_solver.shutdown = True
for x in (self.flipper, self.turntable, self.colorarm):
# We are shutting down so do not 'hold' the motors
x.stop_action = 'brake'
x.off(False)
def signal_term_handler(self, signal, frame):
log.error('Caught SIGTERM')
self.shutdown_robot()
def signal_int_handler(self, signal, frame):
log.error('Caught SIGINT')
self.shutdown_robot()
def apply_transformation(self, transformation):
self.state = [self.state[t] for t in transformation]
def rotate_cube(self, direction, nb):
current_pos = self.turntable.position
final_pos = 135 * round(
(self.turntable.position + (270 * direction * nb)) / 135.0)
log.info(
"rotate_cube() direction %s, nb %s, current_pos %d, final_pos %d" %
(direction, nb, current_pos, final_pos))
if self.flipper.position > 35:
self.flipper_away()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
final_pos)
if nb >= 1:
for i in range(nb):
if direction > 0:
transformation = [0, 1, 5, 2, 3, 4]
else:
transformation = [0, 1, 3, 4, 5, 2]
self.apply_transformation(transformation)
def rotate_cube_1(self):
self.rotate_cube(1, 1)
def rotate_cube_2(self):
self.rotate_cube(1, 2)
def rotate_cube_3(self):
self.rotate_cube(-1, 1)
def rotate_cube_blocked(self, direction, nb):
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# OVERROTATE depends on lot on MindCuber.rotate_speed
current_pos = self.turntable.position
OVERROTATE = 18
final_pos = int(135 * round(
(current_pos + (270 * direction * nb)) / 135.0))
temp_pos = int(final_pos + (OVERROTATE * direction))
log.info(
"rotate_cube_blocked() direction %s nb %s, current pos %s, temp pos %s, final pos %s"
% (direction, nb, current_pos, temp_pos, final_pos))
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
temp_pos)
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed / 4),
final_pos)
def rotate_cube_blocked_1(self):
self.rotate_cube_blocked(1, 1)
def rotate_cube_blocked_2(self):
self.rotate_cube_blocked(1, 2)
def rotate_cube_blocked_3(self):
self.rotate_cube_blocked(-1, 1)
def flipper_hold_cube(self, speed=300):
current_position = self.flipper.position
# Push it forward so the cube is always in the same position
# when we start the flip
if (current_position <= MindCuber.hold_cube_pos - 10
or current_position >= MindCuber.hold_cube_pos + 10):
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed),
MindCuber.hold_cube_pos)
sleep(0.05)
def flipper_away(self, speed=300):
"""
Move the flipper arm out of the way
"""
log.info("flipper_away()")
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(speed), 0)
def flip(self):
"""
Motors will sometimes stall if you call on_to_position() multiple
times back to back on the same motor. To avoid this we call a 50ms
sleep in flipper_hold_cube() and after each on_to_position() below.
We have to sleep after the 2nd on_to_position() because sometimes
flip() is called back to back.
"""
log.info("flip()")
if self.shutdown:
return
# Move the arm down to hold the cube in place
self.flipper_hold_cube()
# Grab the cube and pull back
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 0
self.flipper.on_to_position(SpeedDPS(self.flip_speed), 190)
sleep(0.05)
# At this point the cube is at an angle, push it forward to
# drop it back down in the turntable
self.flipper.ramp_up_sp = 200
self.flipper.ramp_down_sp = 400
self.flipper.on_to_position(SpeedDPS(self.flip_speed_push),
MindCuber.hold_cube_pos)
sleep(0.05)
transformation = [2, 4, 1, 3, 0, 5]
self.apply_transformation(transformation)
def colorarm_middle(self):
log.info("colorarm_middle()")
self.colorarm.on_to_position(SpeedDPS(600), -750)
def colorarm_corner(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_corner(%d)" % square_index)
position_target = -580
if square_index == 1:
position_target -= 10
elif square_index == 3:
position_target -= 30
elif square_index == 5:
position_target -= 20
elif square_index == 7:
pass
else:
raise ScanError(
"colorarm_corner was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_edge(self, square_index):
"""
The lower the number the closer to the center
"""
log.info("colorarm_edge(%d)" % square_index)
position_target = -640
if square_index == 2:
position_target -= 20
elif square_index == 4:
position_target -= 40
elif square_index == 6:
position_target -= 20
elif square_index == 8:
pass
else:
raise ScanError(
"colorarm_edge was given unsupported square_index %d" %
square_index)
self.colorarm.on_to_position(SpeedDPS(600), position_target)
def colorarm_remove(self):
log.info("colorarm_remove()")
self.colorarm.on_to_position(SpeedDPS(600), 0)
def colorarm_remove_halfway(self):
log.info("colorarm_remove_halfway()")
self.colorarm.on_to_position(SpeedDPS(600), -400)
def scan_face(self, face_number):
log.info("scan_face() %d/6" % face_number)
if self.shutdown:
return
if self.flipper.position > 35:
self.flipper_away(100)
self.colorarm_middle()
self.colors[int(MindCuber.scan_order[self.k])] = self.color_sensor.rgb
self.k += 1
i = 1
target_pos = 115
self.colorarm_corner(i)
# The gear ratio is 3:1 so 1080 is one full rotation
self.turntable.reset()
self.turntable.on_to_position(SpeedDPS(MindCuber.rotate_speed),
1080,
block=False)
self.turntable.wait_until('running')
while True:
# 135 is 1/8 of full rotation
if self.turntable.position >= target_pos:
current_color = self.color_sensor.rgb
self.colors[int(MindCuber.scan_order[self.k])] = current_color
i += 1
self.k += 1
if i == 9:
# Last face, move the color arm all the way out of the way
if face_number == 6:
self.colorarm_remove()
# Move the color arm far enough away so that the flipper
# arm doesn't hit it
else:
self.colorarm_remove_halfway()
break
elif i % 2:
self.colorarm_corner(i)
if i == 1:
target_pos = 115
elif i == 3:
target_pos = 380
else:
target_pos = i * 135
else:
self.colorarm_edge(i)
if i == 2:
target_pos = 220
elif i == 8:
target_pos = 1060
else:
target_pos = i * 135
if self.shutdown:
return
if i < 9:
raise ScanError('i is %d..should be 9' % i)
self.turntable.wait_until_not_moving()
self.turntable.off()
self.turntable.reset()
log.info("\n")
def scan(self):
log.info("scan()")
self.colors = {}
self.k = 0
self.scan_face(1)
self.flip()
self.scan_face(2)
self.flip()
self.scan_face(3)
self.rotate_cube(-1, 1)
self.flip()
self.scan_face(4)
self.rotate_cube(1, 1)
self.flip()
self.scan_face(5)
self.flip()
self.scan_face(6)
if self.shutdown:
return
log.info("RGB json:\n%s\n" % json.dumps(self.colors))
self.rgb_solver = RubiksColorSolverGeneric(3)
self.rgb_solver.enter_scan_data(self.colors)
self.rgb_solver.crunch_colors()
self.cube_kociemba = self.rgb_solver.cube_for_kociemba_strict()
log.info("Final Colors (kociemba): %s" % ''.join(self.cube_kociemba))
# This is only used if you want to rotate the cube so U is on top, F is
# in the front, etc. You would do this if you were troubleshooting color
# detection and you want to pause to compare the color pattern on the
# cube vs. what we think the color pattern is.
'''
log.info("Position the cube so that U is on top, F is in the front, etc...to make debugging easier")
self.rotate_cube(-1, 1)
self.flip()
self.flipper_away()
self.rotate_cube(1, 1)
input('Paused')
'''
def move(self, face_down):
log.info("move() face_down %s" % face_down)
position = self.state.index(face_down)
actions = {
0: ["flip", "flip"],
1: [],
2: ["rotate_cube_2", "flip"],
3: ["rotate_cube_1", "flip"],
4: ["flip"],
5: ["rotate_cube_3", "flip"]
}.get(position, None)
for a in actions:
if self.shutdown:
break
getattr(self, a)()
def run_kociemba_actions(self, actions):
log.info('Action (kociemba): %s' % ' '.join(actions))
total_actions = len(actions)
for (i, a) in enumerate(actions):
if self.shutdown:
break
if a.endswith("'"):
face_down = list(a)[0]
rotation_dir = 1
elif a.endswith("2"):
face_down = list(a)[0]
rotation_dir = 2
else:
face_down = a
rotation_dir = 3
log.info("Move %d/%d: %s%s (a %s)" %
(i, total_actions, face_down, rotation_dir, pformat(a)))
self.move(face_down)
if rotation_dir == 1:
self.rotate_cube_blocked_1()
elif rotation_dir == 2:
self.rotate_cube_blocked_2()
elif rotation_dir == 3:
self.rotate_cube_blocked_3()
log.info("\n")
def resolve(self):
if self.shutdown:
return
cmd = ['kociemba', ''.join(map(str, self.cube_kociemba))]
output = check_output(cmd).decode('ascii')
if 'ERROR' in output:
msg = "'%s' returned the following error\n%s\n" % (' '.join(cmd),
output)
log.error(msg)
print(msg)
sys.exit(1)
actions = output.strip().split()
self.run_kociemba_actions(actions)
self.cube_done()
def cube_done(self):
self.flipper_away()
def wait_for_cube_insert(self):
rubiks_present = 0
rubiks_present_target = 10
log.info('wait for cube...to be inserted')
while True:
if self.shutdown:
break
dist = self.infrared_sensor.proximity
# It is odd but sometimes when the cube is inserted
# the IR sensor returns a value of 100...most of the
# time it is just a value less than 50
if dist < 50 or dist == 100:
rubiks_present += 1
log.info("wait for cube...distance %d, present for %d/%d" %
(dist, rubiks_present, rubiks_present_target))
else:
if rubiks_present:
log.info('wait for cube...cube removed (%d)' % dist)
rubiks_present = 0
if rubiks_present >= rubiks_present_target:
log.info('wait for cube...cube found and stable')
break
time.sleep(0.1)
#!/usr/bin/env python3
import sys
import math
from ev3dev2.motor import LargeMotor, OUTPUT_C, OUTPUT_D, OUTPUT_B
from ev3dev2.sensor.lego import UltrasonicSensor
from ev3dev2.sound import Sound
from ev3dev2.sensor.lego import GyroSensor
import time
from ev3dev2.sound import Sound
m1 = LargeMotor(OUTPUT_C)
m2 = LargeMotor(OUTPUT_D)
m3 = LargeMotor(OUTPUT_B)
us = UltrasonicSensor()
sound = Sound()
print(us.distance_centimeters, file=sys.stderr)
a_min = m3.position + 10
m3.on(-10, block=False)
while m3.position < a_min:
a_min = m3.position
time.sleep(0.1)
m3.off()
print("a_min", a_min, file=sys.stderr)
a_max = m3.position - 10
m3.on(10, block=False)
while m3.position > a_max:
#!/usr/bin/env python3
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3
from ev3dev2.sensor.lego import TouchSensor, ColorSensor
from ev3dev2.led import Leds
from time import sleep
# TODO: Add code here
colour_s1 = ColorSensor(INPUT_1) # sensor 1 for light intensity
colour_s2 = ColorSensor(INPUT_2) # sensor 2 for light intensity
lm = LargeMotor(OUTPUT_B); # left motor on PORT B
rm = LargeMotor(OUTPUT_C); # right motor on PORT C
a = 0.6;
d = a * (colour_s2.reflected_light_intensity() - colour_s1.reflected_light_intensity())
while True:
if (colour_s2.reflected_light_intensity() - colour_s1.reflected_light_intensity()) > d:
lm.on_for_rotations(SpeedPercent(100), 2) # maybe change LEFT AND RIGHT
rm.on_for_rotations(SpeedPercent(0), 2)
elif (colour_s1.reflected_light_intensity() - colour_s2.reflected_light_intensity()) > d:
lm.on_for_rotations(SpeedPercent(0), 2) # maybe change LEFT AND RIGHT
rm.on_for_rotations(SpeedPercent(100), 2)
#!/usr/bin/env python3
from ev3dev2.motor import MoveSteering, MoveTank, MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D
from ev3dev2.sensor.lego import TouchSensor, ColorSensor, GyroSensor
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
import xml.etree.ElementTree as ET
import threading
import time
from sys import stderr
gyro = GyroSensor(INPUT_1)
steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C)
tank_block = MoveTank(OUTPUT_B, OUTPUT_C)
largeMotor_Left = LargeMotor(OUTPUT_B)
largeMotor_Right = LargeMotor(OUTPUT_C)
#_________________________________________________________________________________________________________________________________
def StraightGyro_current(stop, speed, rotations):
print("In StraightGyro_current", file=stderr)
current_degrees = largeMotor_Left.position
rotations = rotations * 360
target_rotations = current_degrees + rotations
target = gyro.angle
current_gyro_reading = target
# print("Current Gyro Reading: {}".format(current_gyro_reading))
while float(
current_degrees
) < target_rotations: # if the currentm rotations is smaller than the target rotations
if stop():
break
#!/usr/bin/env micropython
from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_D
from ev3dev2.sensor import INPUT_4
from ev3dev2.sensor.lego import InfraredSensor
from ev3dev2.sound import Sound
MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A)
TAIL_MOTOR = LargeMotor(address=OUTPUT_B)
CHEST_MOTOR = LargeMotor(address=OUTPUT_D)
IR_SENSOR = InfraredSensor(address=INPUT_4)
SPEAKER = Sound()
def drive_once_by_ir_beacon(channel: int = 1, speed: float = 100):
if IR_SENSOR.top_left(channel=channel) and IR_SENSOR.top_right(
channel=channel):
TAIL_MOTOR.on(speed=speed, brake=False, block=False)
elif IR_SENSOR.bottom_left(channel=channel) and IR_SENSOR.bottom_right(
channel=channel):
TAIL_MOTOR.on(speed=-speed, brake=False, block=False)
elif IR_SENSOR.top_left(channel=channel):
MEDIUM_MOTOR.on(speed=-50, brake=False, block=False)
TAIL_MOTOR.on(speed=speed, brake=False, block=False)
elif IR_SENSOR.top_right(channel=channel):
from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, OUTPUT_D, SpeedPercent, MoveTank
from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
from ev3dev2.sensor.lego import TouchSensor, ColorSensor, GyroSensor, UltrasonicSensor
from ev3dev2.led import Leds
import time
leds = Leds()
try:
A = LargeMotor(OUTPUT_A)
except:
pass
try:
B = LargeMotor(OUTPUT_B)
except:
pass
try:
C = LargeMotor(OUTPUT_C)
except:
pass
try:
D = LargeMotor(OUTPUT_D)
except:
pass
try:
ultra = UltrasonicSensor()
except:
pass
try:
touch = TouchSensor()
except:
def main():
# start sensor and motor definitions
Sound.speak("").wait()
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
# end sensor and motor definitions
# start calibration
GY.mode = 'GYRO-ANG'
GY.mode = 'GYRO-RATE'
GY.mode = 'GYRO-ANG'
# end calibration
# The following line would be if we used tank_drive
# tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
# start definition of driving parameters
loop_gyro = 0
starting_value = GY.value()
# end definition of driving parameters
# set initial speed parameters
speed = 20
adjust = 1
# change 999999999999 to however you want to go
while loop_gyro < 999999999999:
# while Gyro value is the same as the starting value, then go straigt.
while GY.value() == starting_value:
left_wheel_speed = speed
right_wheel_speed = speed
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
return
# while greater than starting value, then go left.
while GY.value() > starting_value:
left_wheel_speed = speed - adjust
right_wheel_speed = speed
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
return
# while less than starting value, then go right.
while GY.value() < starting_value:
left_wheel_speed = speed + adjust
right_wheel_speed = speed
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
return
return
def main():
MA = MediumMotor("")
MB = LargeMotor("outB")
MC = LargeMotor("outC")
MD = MediumMotor("")
GY = GyroSensor("")
C3 = ColorSensor("")
C4 = ColorSensor("")
#Setting the Gyro. V
GY.mode = 'GYRO-ANG'
GY.mode = 'GYRO-RATE'
GY.mode = 'GYRO-ANG'
tank_drive = MoveTank(OUTPUT_B, OUTPUT_C)
loop_gyro = 0
gyro_adjust = 1
starting_value = GY.value()
gyro_correct_loops = 0
straight_correct_loops = 0
gyro_correct_straight = 0
# change this to whatever speed what you want. V
left_wheel_speed = 100
right_wheel_speed = 100
# change the loop_gyro verses the defined value argument to however far you want to go
# if Gyro value is the same as the starting value, go straight, if more turn right, if less turn left
# change the value to how far you want the robot to go. V
#Pulling out of Launch area. V
tank_drive.on_for_rotations(SpeedPercent(-30), SpeedPercent(-30), 1.625)
#Gyro Turn toowards the red circle. V
while GY.value() < 80:
left_wheel_speed = 100
right_wheel_speed = -100
#MB is left wheel & MC is right wheel
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
#Driving forward towards the red circle. V
while MB.position > -2550: #was -2180, Joshua is changing it to -2500 to stay in circle better
if GY.value() == 90:
left_wheel_speed = -200
right_wheel_speed = -200
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
gyro_adjust = 8
gyro_correct_loops = 0
gyro_correct_straight = 0
straight_correct_loops = 0
else:
if GY.value() < 90:
correct_rate = abs(
GY.value()
) # This captures the gyro value at the beginning of the statement
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -200
right_wheel_speed = -200
if abs(
GY.value()
) >= correct_rate: # If gyro value has worsened despite the correction then change the adjust variable for next time
gyro_adjust = gyro_adjust + 1
gyro_correct_loops = gyro_correct_loops + 1
if GY.value() == 0 and gyro_correct_straight == 0:
while straight_correct_loops < gyro_correct_loops + 1:
right_wheel_speed = right_wheel_speed - gyro_adjust
left_wheel_speed = left_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1
gyro_correct_loops = 0
straight_correct_loops = 0
else:
correct_rate = abs(
GY.value()) # Same idea as the other if statement
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
MB.run_forever(speed_sp=left_wheel_speed)
MC.run_forever(speed_sp=right_wheel_speed)
left_wheel_speed = -200
right_wheel_speed = -200
gyro_correct_loops = gyro_correct_loops + 1
if abs(GY.value()) >= correct_rate:
gyro_adjust = gyro_adjust + 1
if GY.value(
) == 0 and gyro_correct_straight == 0: #this code corrects the gyro back to the right line
while straight_correct_loops < gyro_correct_loops + 1: #runs this loop until it makes the gyro the opposite of what it was when it was wrong in the first place
left_wheel_speed = left_wheel_speed - gyro_adjust
right_wheel_speed = right_wheel_speed + gyro_adjust
straight_correct_loops = straight_correct_loops + 1
gyro_correct_straight = 1 #makes sure that when the gyro is corrected to both straight and the line it was on that gyro is not messed up again
gyro_correct_loops = 0
straight_correct_loops = 0
# stop all motors
MB.stop(stop_action="hold")
MC.stop(stop_action="hold")
tank_drive.on_for_seconds(SpeedPercent(-100), SpeedPercent(-100), 1)
#Pulling away from the Red circle and driving into home. V
tank_drive.on_for_rotations(SpeedPercent(65), SpeedPercent(65), 9)
from ev3dev2.motor import LargeMotor, MediumMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank
from enum import Enum
from time import sleep
import threading
import math
class ArmPositions(Enum):
Up = 1
Settle = 2
Down = 3
Flip = 4
armMotor = LargeMotor(OUTPUT_A)
armMotorSpeed = 25
tableMotor = LargeMotor(OUTPUT_B)
tableMotorSpeed = 50
tableMotorOvershoot = 50
tableDegreesPerTurn = 270
scannerMotor = MediumMotor(OUTPUT_C)
scannerMotorSpeed = 50
scannerPositionHold = -200
scannerPositionCenter = -750
scannerPositionEdge = -625
scannerPositionCorner = -575
def turnTable(turns, doOvershoot=True):
from time import sleep
import logging
#logging
logging.basicConfig(level=logging.DEBUG, format='%(asctime)s %(levelname)5s: %(message)s')
log=logging.getLogger(__name__)
log.info('Stair climber starting......')
# define the touch sensor
ts = TouchSensor(INPUT_3)
# define the gyro sensor
gy = GyroSensor(INPUT_2)
# define the large motor on port B
lm_move = LargeMotor(OUTPUT_B)
# define the large motor on port D
lm_lifter = LargeMotor(OUTPUT_D)
# define the midium motor on port A
mm_move = MediumMotor(OUTPUT_A)
# define the button
btn = Button()
# define lcd display
lcd = Display()
# define sound
snd = Sound()
# define the steps to go, initial value is 0
