class MovementController:
"""Class to move the robot"""
_ROTATION_TO_DEGREE = 1
_DISTANCE_TO_DEGREE = 1
def __init__(self):
self.moveSteering = MoveSteering(constants.LEFT_MOTOR_PORT,
constants.RIGHT_MOTOR_PORT)
def rotate(self, degrees, speed=100, block=True):
"""Rotate the robot a certain number of degrees. Positive is counter-clockwise"""
self.moveSteering.on_for_degrees(
100,
SpeedPercent(speed),
degrees * MovementController._ROTATION_TO_DEGREE,
block=block)
def travel(self, distance, speed=100, block=True):
"""Make the robot move forward or backward a certain number of cm"""
self.moveSteering.on_for_degrees(
0,
speed,
distance * MovementController._DISTANCE_TO_DEGREE,
block=block)
def steer(self, direction, speed=100):
"""Make the robot move in a direction"""
self.moveSteering.on(direction, speed)
def stop(self):
"""Make robot stop"""
self.moveSteering.off()
def run(power, target, kp, kd, ki, direction, minRef, maxRef):
lastError = error = integral = 0
left_motor.run_direct()
right_motor.run_direct()
free_count = 0
isFree = False
while not btn.any() and not isFree:
# if ts.value():
# print ('Breaking loop') # User pressed touch sensor
# break
refRead = col.value()
error = target - (100 * (refRead - minRef) / (maxRef - minRef))
derivative = error - lastError
lastError = error
integral = float(0.5) * integral + error
course = (kp * error + kd * derivative + ki * integral) * direction
for (motor, pow) in zip((left_motor, right_motor),
steering2(course, power)):
motor.duty_cycle_sp = pow
free_count, isFree = freeSpot(free_count, isFree)
sleep(0.01) # Aprox 100Hz
left_motor.stop()
right_motor.stop()
sleep(2)
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
steer_pair.on_for_degrees(steering=0,
speed=15,
degrees=200,
brake=False,
block=True)
parking.back_parl_park(steer_pair)
class Driver:
def __init__(self):
self.driver = MoveSteering(OUTPUT_B, OUTPUT_C)
self.speed = 40
def set_speed(self, speed):
self.speed = max(-100, min(100, speed))
def get_speed(self):
return self.speed
def move(self):
self.driver.on(0, SpeedPercent(self.speed))
def move_cm(self, cm):
TRANSFORM_CONST = 37.0
self.driver.on_for_degrees(0, SpeedPercent(self.speed), cm * TRANSFORM_CONST)
def move_neg_cm(self, cm):
TRANSFORM_CONST = 37.0
self.driver.on_for_degrees(0, SpeedPercent(self.speed), -cm * TRANSFORM_CONST)
def reverse(self):
self.driver.on(0, SpeedPercent(-self.speed))
def reverse_cm(self, cm):
TRANSFORM_CONST = 37.0
self.driver.on_for_degrees(0, SpeedPercent(-self.speed), cm*TRANSFORM_CONST)
def stop(self):
self.driver.off()
def turn(self, steering):
steering = max(-100, min(100, steering))
self.driver.on(steering, self.speed)
def turn_rotations(self, steering, rotations):
steering = max(-100, min(100, steering))
self.driver.on_for_rotations(steering, SpeedPercent(self.speed), rotations)
def turn_degrees(self, degrees):
TRANSFORM_CONST = 3.9
self.driver.on_for_degrees(100, SpeedPercent(self.speed), degrees * TRANSFORM_CONST)
def turn_neg_degrees(self, degrees):
TRANSFORM_CONST = 3.9
steering = 100 if degrees > 0 else -100
self.driver.on_for_degrees(steering, SpeedPercent(self.speed), -degrees * TRANSFORM_CONST)
def move_seconds(self, seconds):
self.driver.on_for_seconds(0, self.speed, seconds)
def back_seconds(self, seconds):
self.driver.on_for_seconds(0, -self.speed, seconds)
class Drive(object):
def __init__(self):
self.pid = PIDController(kP=1.0, kI=0.0, kD=0.1)
# motors
try:
self.steerPair = MoveSteering(OUTPUT_B, OUTPUT_C)
except ev3dev2.DeviceNotFound as e:
Debug.print("Error:", e)
self.speed = Config.data['pid']['fast']['speed_max']
def updateConfig(self):
self.speed = Config.data['pid']['fast']['speed_max']
self.pid.updateConfig()
def followLine(self, sensValues):
colorLeft = sensValues["ColorLeft"][1] # TODO: HSL? Lichtwert anpassen
colorRight = sensValues["ColorRight"][1]
feedback = colorLeft - colorRight
self.pid.update(feedback)
turn = self.pid.output
self.steerPair.on(turn, self.speed)
def followLineSlow(self, speed, sensValues):
colorLeft = sensValues["ColorLeft"][1] # TODO: HSL? Lichtwert anpassen
colorRight = sensValues["ColorRight"][1]
feedback = colorLeft - colorRight
self.pid.update(feedback)
turn = self.pid.output
self.steerPair.on(turn, self.speed)
def turn(self, action):
def right():
self.steerPair.on_for_degrees(-100, 20, 200)
def left():
self.steerPair.on_for_degrees(100, 20, 200)
if action == "right":
right()
elif action == "left":
left()
elif action == "skip":
self.steerPair.on_for_degrees(
0, -20, 50) # back off until centered on cross
self.steerPair.wait_until_not_moving(2000)
left()
else:
raise AttributeError("no valid action string given for turn()")
def brake(self):
self.steerPair.off()
class RobotControl(RobotHandle):
def __init__(self, *args_module_name):
super().__init__(*args_module_name)
self.m_left = OUTPUT_B # Motor left
self.m_right = OUTPUT_A # Motor right
self.motor_left = LargeMotor(OUTPUT_B)
self.motor_right = LargeMotor(OUTPUT_A)
self.s_us = INPUT_1 # Sensor Ultrasonic
self.sl_left = INPUT_3 # Sensor Light left
self.sl_right = INPUT_4 # Sensor Right left
self.mMT = MoveTank(self.m_left, self.m_right) # move with 2 motors
self.mMS = MoveSteering(self.m_left, self.m_right) # move on position
self.sUS = UltrasonicSensor(self.s_us)
self.sLS_left = LightSensor(INPUT_3)
self.sLS_right = LightSensor(INPUT_4)
self.thread_detect_danger = ControlThread()
self.thread_detect_light_intesity = ControlThread()
self.stop_detect_light_intesity = False
self.max_speed = self.mMT.max_speed
#self.distance_cm = Value('d', 0.0)
def run(self):
#pool_detect_danger = Process(target=self.detect_danger, args=self.distance_cm)
#print('RUN: Distance: ', self.distance_cm.value)
#movement = ControlThread(self.mMT.on, 15, 15)
#thread_detect_light_intesity = ControlThread(self.detect_light_intensitiy)
self.thread_detect_light_intesity.function = self.detect_light_intensitiy
self.thread_detect_danger.function = self.detect_danger
thread_detect_danger = ControlThread(self.detect_danger)
try:
#self.mMT.on(15, 15)
# 11.3, 0.05, 3.2
self.follow_line(8.4, 0.05, 0.8, 'left', SpeedPercent(20))
#self.follow_line(args.kp, args.ki, args.kd, 'left', SpeedPercent(10))
except Exception:
self.mMT.off()
raise
#self.mMT.off()
#movement.start()
self.thread_detect_light_intesity.start()
self.thread_detect_danger.start()
return True
def detect_light_intensitiy(self):
while True:
if self.stop_detect_light_intesity:
break
if not args.light_mode:
args.light_mode = 'ambient'
if args.light_mode == 'ambient':
light_intesitiy_left = self.sLS_left.ambient_light_intensity
light_intesitiy_right = self.sLS_right.ambient_light_intensity
sleep(.500)
elif args.light_mode == 'reflected':
light_intesitiy_left = self.sLS_left.reflected_light_intensity
light_intesitiy_right = self.sLS_right.reflected_light_intensity
sleep(.500)
print('LEFT[{}]: {}'.format(args.light_mode, light_intesitiy_left))
print('RIGHT[{}]: {}'.format(args.light_mode,
light_intesitiy_right))
def get_light_intensitiy(self, side: str): # side = left, or right
if args.light_mode == 'ambient':
left = self.sLS_left.ambient_light_intensity
right = self.sLS_right.ambient_light_intensity
else:
left = self.sLS_left.reflected_light_intensity
right = self.sLS_right.reflected_light_intensity
if side == 'left':
return left
elif side == 'right':
return right
def detect_danger(self):
while True:
d = self.sUS.distance_centimeters
#print('dd: distance:', d)
if d < 10:
self.turn_on_wand()
self.mMT.stop()
#args.light_mode = 'reflected'
pass
sleep(.500)
def turn_on_wand(self):
self.mMS.on_for_degrees(-100, 20, parser.d)
def now_stay_on_that_line(self):
# kp=3, ki=0, kd=0
#self.mMT.cs = self.sl_left
#self.mMT.follow_line(3, 0, 0, 'left', SpeedPercent(20))
pass
def follow_line(self,
kp,
ki,
kd,
side,
speed=SpeedPercent(50),
target_light_intensity=None,
follow_left_edge=True,
white=60,
off_line_count_max=20,
sleep_time=0.01,
follow_for=follow_for_forever,
**kwargs):
self.cs = self.sLS_left
if target_light_intensity is None:
target_light_intensity = self.get_light_intensitiy(side)
integral = 0.0
last_error = 0.0
derivative = 0.0
off_line_count = 0
speed_native_units = -speed.to_native_units(self.motor_left)
MAX_SPEED = SpeedNativeUnits(self.max_speed / 2)
while follow_for(self, **kwargs):
if args.light_mode == 'ambient':
reflected_light_intensity = self.cs.ambient_light_intensity
else:
reflected_light_intensity = self.cs.reflected_light_intensity
error = target_light_intensity - reflected_light_intensity
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = -((kp * error) + (ki * integral) +
(kd * derivative))
if not follow_left_edge:
turn_native_units *= -1
#if error < last_error:
# speed_native_units -= 2
#elif error > last_error:
# speed_native_units += 5
right_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
left_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
print(turn_native_units, speed_native_units)
# DEBUG
#print('Error: %s' % error)
#print('reflected_light_intesity: : %s' % reflected_light_intensity)
#print('integral: %s' % integral)
#print('derivative: %s' % derivative)
#print('last_error: %s' % last_error)
#print('turn_native_units: %s' % turn_native_units)
#print('left_speed: %s' % left_speed)
#print('right_speed: %s' % right_speed)
if left_speed > MAX_SPEED:
print("%s: left_speed %s is greater than MAX_SPEED %s" %
(self, left_speed, MAX_SPEED))
self.mMT.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
if right_speed > MAX_SPEED:
print("%s: right_speed %s is greater than MAX_SPEED %s" %
(self, right_speed, MAX_SPEED))
self.mMT.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
# Have we lost the line?
if reflected_light_intensity >= white:
off_line_count += 1
if off_line_count >= off_line_count_max:
self.mMT.stop()
raise LineFollowErrorLostLine("we lost the line")
else:
off_line_count = 0
if sleep_time:
time.sleep(sleep_time)
self.mMT.on(left_speed, right_speed)
self.mMT.stop()
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from time import sleep steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) # 스티어링 모드 설정 steer_pair.on(0,50) # 직진 스팅어링으로 속도 50% 무한 주행 sleep(1) steer_pair.off(brake=True) # 주행 멈춤 sleep(1) steer_pair.on_for_seconds(30,50, 2) # 좌회전 스팅어링으로 속도 50% 2초 주행 sleep(1) # 직진 스팅어링으로 속도 50% 반대방향 3회전 주행 steer_pair.on_for_rotations(0,50, -3) sleep(1) steer_pair.on_for_degrees(0,50, 180) # 직진 스팅어링으로 속도 50% 180도 주행 sleep(1)
MEDIUM_MOTOR = MediumMotor(address=OUTPUT_A)
STEER_DRIVER = MoveSteering(left_motor_port=OUTPUT_B,
right_motor_port=OUTPUT_C,
motor_class=LargeMotor)
SCREEN = Display()
SPEAKER = Sound()
SCREEN.image_filename(filename='/home/robot/image/Pinch right.bmp',
clear_screen=True)
SCREEN.update()
STEER_DRIVER.on_for_degrees(steering=-100,
speed=25,
degrees=50,
brake=True,
block=True)
MEDIUM_MOTOR.on_for_rotations(speed=100, rotations=3, block=True, brake=True)
SCREEN.image_filename(filename='/home/robot/image/Pinch left.bmp',
clear_screen=True)
SCREEN.update()
STEER_DRIVER.on_for_degrees(steering=100,
speed=25,
degrees=100,
brake=True,
block=True)
steering_pair.off()
else:
lado=-1
while cor_dir.color==cor_aux_d:
steering_pair.on(-60,10)
else:
steering_pair.off()
sleep(0.5)
return
def acharborda():
while cor_dir.color==cor_esq.color:
steering_pair.on(0,10)
else:
steering_pair.off()
return
def girando_na_borda(angulo, lado): #1 direita, -1 esquerda
rotacoes=1.1*angulo/90
steering_pair.on_for_rotations(lado*55,-10,rotacoes)
return
acharborda()
steering_pair.on_for_degrees(0,-10,180)
alinhamento()
if cor_dir.color==3 or cor_esq.color==3: #verde
girando_na_borda(90,-1)
elif cor_dir.color==1 or cor_esq.color==1: #preto
girando_na_borda(90,1)
elif cor_dir.color==0 or cor_esq.color==0: #não cor
steering_pair.on_for_degrees(0,-10,50)
rgbmax_e = definir_rgbmax('esq')
rgbmax_d = definir_rgbmax('dir')
pos = 7
while True:
while meeting_area: #começa aleatoriamente na meeting area
#termina com os sensores de cor no vazio certo
sound.beep()
alinhamento()
if cor('esq')!='vazio' and cor('dir')!='vazio' and not(antes_preto):
a = testar_preto()
global antes_preto
antes_preto = True
if a: #está no preto
steering_pair.on_for_degrees(0,-20,250) #volta e vira pra direita
girar_pro_lado('dir',90)
sound.speak('black')
else: #está na rampa
steering_pair.on_for_degrees(0,-20,250) #volta e vira pra esquerda
sound.speak('green')
girar_pro_lado('esq',90)
elif cor('esq')=='vazio' or cor('dir')=='vazio' and not(antes_preto): #virado pro vazio
global no_vazio
no_vazio = True
if not(no_vazio and antes_preto):
steering_pair.on_for_degrees(0,-25,350) #volta e vira pra esquera
girar_pro_lado('esq',90)
sound.speak('empty')
global no_vazio
no_vazio = True
rgbmax_e = definir_rgbmax('esq')
rgbmax_d = definir_rgbmax('dir')
garra_g.on_for_seconds(10, 1.5)
garra_m.on_for_seconds(10, 1)
mapadecores = ['vermelho', 'amarelo', 'azul']
while True:
while meeting_area: #começa aleatoriamente na meeting area
#termina com os sensores de cor no vazio certo
sound.beep()
while cor('esq') == 'branco' and cor('dir') == 'branco':
steering_pair.on(0, 15)
else:
steering_pair.off()
steering_pair.on_for_degrees(0, -15, 70)
alinhamento()
steering_pair.on_for_degrees(0, 10, 70)
if cor('esq') == 'vazio' or cor('dir') == 'vazio':
global no_vazio
no_vazio = True
if cor('esq') != cor('dir'):
steering_pair.on_for_degrees(0, -15, 150)
alinhamento()
steering_pair.on_for_degrees(0, -25,
350) #volta e vira pra esquera
girar_pro_lado('esq', 90)
sound.speak('empty')
elif (cor('esq') != 'vazio' and cor('dir') != 'vazio'):
a = testar_preto()
if a: #está no preto
leds = Leds()
print("running main")
running = True
steer_pair.on(steering=0, speed=15)
count = 0
while (running):
if us.distance_centimeters > 15: # to detect objects closer than 40cm
# In the above line you can also use inches: us.distance_inches < 16
leds.set_color('LEFT', 'RED')
leds.set_color('RIGHT', 'RED')
count += 1
if count > 20:
steer_pair.off()
running = False
else:
count = 0
leds.set_color('LEFT', 'GREEN')
leds.set_color('RIGHT', 'GREEN')
sleep(0.1) # Give the CPU a rest
sleep(3)
if not running:
steer_pair.on_for_degrees(steering=0,
speed=15,
degrees=160,
brake=False,
block=True)
parking.back_parl_park(steer_pair)
def autocompletar(cor1, cor2):
A = {cor1, cor2}
B = {'azul', 'vermelho', 'amarelo'}
for item in (B - A):
cor = item
return cor
if h:
aprender_cores = False
else: #verificar se já existe o arquivo, se não existir:
cores = open("cores.txt", "w+") #cria o arquivo
cores.close() #fecha o arquivo
steering_pair.on_for_degrees(0, -10, 300)
girar_pro_lado('esq', 90) #90 graus pra esquerda
alinhamento() #achou a faixa preta
sleep(0.1)
alinhamento() #achou a primeira cor
sleep(0.1)
steering_pair.on_for_degrees(0, 10, 200)
cor1 = cor('esq') #salvar primeira cor
sleep(0.1)
steering_pair.on_for_degrees(0, 10, 100)
girar_pro_lado('esq', 90) #90 graus pra esquerda
alinhamento()
steering_pair.on_for_degrees(0, 10, 200)
cor2 = cor('esq') #salvar segunda cor
cor3 = autocompletar(cor1, cor2) #autocompletar 3a cor
cores = open("cores.txt", "a")
def Robotrun1():
robot = MoveSteering(OUTPUT_A, OUTPUT_B)
colorLeft = ColorSensor(INPUT_1)
colorRight = ColorSensor(INPUT_3)
motorA = LargeMotor(OUTPUT_A)
motorD = LargeMotor(OUTPUT_D)
motorC = LargeMotor(OUTPUT_C)
motorC.off(brake=True)
motorD.off(brake=True)
Constants.STOP = False
GyroDrift() #check gyro drift at the start of every robot run
show_text("Robot Run 1")
#Wall square and move forward till first line intersection
acceleration(degrees=DistanceToDegree(70), finalSpeed=50, steering=2)
while colorLeft.reflected_light_intensity > 10 and False == Constants.STOP:
robot.on(steering=1, speed=20)
robot.off()
#Move forward towards step counter
acceleration(degrees=DistanceToDegree(13), finalSpeed=20, steering=2)
#Move back and forth until the left sensor encounters white
while colorLeft.reflected_light_intensity < Constants.WHITE and False == Constants.STOP:
#robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
#print("RobotRun2 stop=" + str(Constants.STOP), file=stderr)
MoveForwardWhite(distanceInCm=3)
if colorLeft.reflected_light_intensity >= Constants.WHITE:
break
robot.on_for_degrees(degrees=DistanceToDegree(1.5),
steering=-1,
speed=-8)
robot.off()
#Move back and forth until the left sensor encounters black
while colorLeft.reflected_light_intensity > Constants.BLACK and False == Constants.STOP:
#robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
#print("RobotRun2 stop=" + str(Constants.STOP), file=stderr)
MoveForwardBlack(distanceInCm=3)
if colorLeft.reflected_light_intensity <= Constants.BLACK:
break
robot.on_for_degrees(degrees=DistanceToDegree(1.5),
steering=-1,
speed=-8)
robot.off()
#counter = 0
#while counter < 5 and False == Constants.STOP:
# robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
# robot.on_for_degrees(degrees=DistanceToDegree(0.75), steering=-1, speed=-15)
# counter += 1
#robot.off()
#Series of movements to turn left after step counter mission and then wall square to align with pullup bar
accelerationMoveBackward(degrees=DistanceToDegree(12),
steering=-15,
finalSpeed=-20)
GyroTurn(steering=-100, angle=40)
acceleration(degrees=DistanceToDegree(10.5), finalSpeed=20)
while colorRight.reflected_light_intensity < Constants.WHITE and False == Constants.STOP:
robot.on(speed=10, steering=-1)
robot.off()
acceleration(degrees=DistanceToDegree(2), finalSpeed=20)
GyroTurn(steering=-100, angle=60)
# wall square
robot.on_for_seconds(steering=0, speed=-5, seconds=2, brake=False)
#acceleration(degrees=DistanceToDegree(5), finalSpeed=20, steering=0)
#lineSquare()
#Go under pullup bar and then line square
acceleration(degrees=DistanceToDegree(56), finalSpeed=40, steering=-1)
#lineFollowPID(degrees=DistanceToDegree(40), kp=1.25, ki=0.01, kd=5, color=ColorSensor(INPUT_3))
lineSquare()
#doing bociaa mission
acceleration(degrees=DistanceToDegree(22), finalSpeed=30, steering=0.5)
motorD.on_for_seconds(speed=-25, seconds=0.5, brake=False)
motorD.on_for_seconds(speed=15, seconds=0.25, brake=False)
motorD.on_for_seconds(speed=-25, seconds=0.25, brake=False)
GyroTurn(steering=50, angle=5)
motorC.on_for_seconds(speed=-25, seconds=0.5, brake=False)
motorC.on_for_seconds(speed=15, seconds=0.25, brake=True)
motorC.on_for_seconds(speed=-35, seconds=0.20, brake=False)
motorC.on_for_seconds(speed=15, seconds=0.5, brake=True)
GyroTurn(steering=-50, angle=5)
#motorD.on_for_degrees(speed=30, degrees=15, brake=True)
#Go backward after Boccia and then line square again
accelerationMoveBackward(degrees=DistanceToDegree(22), finalSpeed=30)
lineSquare()
#Turn towards slide and line follow until next intersection. Slide person will become dead by Bobby attachment
GyroTurn(steering=-45, angle=85)
lineFollowPID(degrees=DistanceToDegree(15),
kp=1.25,
ki=0.01,
kd=5,
color=ColorSensor(INPUT_1))
acceleration(degrees=DistanceToDegree(5), finalSpeed=20)
#Turn towards next line and follow the line, then square on the line near intersection
GyroTurn(steering=50, angle=20)
#motorD.on_for_degrees(speed=30, degrees=15, brake=True)
lineFollowPID(degrees=DistanceToDegree(11),
kp=1.25,
ki=0.01,
kd=5,
color=ColorSensor(INPUT_1))
lineSquare()
acceleration(degrees=DistanceToDegree(5), finalSpeed=20, steering=5)
lineFollowPID(degrees=DistanceToDegree(25),
kp=1.25,
ki=0.01,
kd=5,
color=ColorSensor(INPUT_1))
#Turn towards basketball
GyroTurn(steering=100, angle=40)
acceleration(degrees=DistanceToDegree(3), finalSpeed=20, steering=5)
#Lift the basket using right side arm attachment
motorD.on_for_seconds(speed=26, seconds=0.4, brake=True)
motorD.on_for_seconds(speed=-25, seconds=0.5, brake=False)
#Turn towards bench and flatten the bench using left side arm attachement
GyroTurn(steering=-100, angle=90)
#acceleration(degrees=DistanceToDegree(5), finalSpeed=50, steering=0)
motorC.on_for_degrees(speed=-10, degrees=50, brake=True)
GyroTurn(steering=100, angle=5)
motorC.on_for_degrees(speed=10, degrees=50, brake=True)
#Turn towards home and move at 100 speed
GyroTurn(steering=100, angle=35)
acceleration(degrees=DistanceToDegree(70), finalSpeed=100, steering=0)
motorC.off(brake=False)
motorD.off(brake=False)
else:
ctrl = 1
return
#aqui começa o código
controle = 0
print('ok')
while controle == 0:
acharborda() #procura uma borda
cor_vista = 0
qe = qual_cor('d')
qd = qual_cor('e')
if qe == 3 or qd == 3: #verifica se ela é verde
steering_pair.on_for_degrees(0, -10, 200)
steering_pair.on_for_degrees(-50, -10, 180)
acharborda()
steering_pair.on_for_degrees(0, -10, 45) #volta pra se alinhar
alinhamento() #se alinha
sleep(0.5)
if cor_dir.color == 1 or cor_esq.color == 1: #faixa preta: virar pra direita
girando_na_borda(100, -1)
# elif qual_cor('e')==3 or qual_cor('d')==3: #verde: rampa: virar pra esquerda
# steering_pair.on_for_degrees(0,-10,45)
# girando_na_borda(100,1)
elif cor_dir.color == 0 or cor_esq.color == 0: #não-cor: tem que descobrir se é o lado certo
controle = 1
steering_pair.on_for_degrees(0, -10, 100)
girando_na_borda(90, -1) #vira pra esquerda e repete o processo inicial
pegar_cano = True
rgbmax_e = definir_rgbmax('esq')
rgbmax_d = definir_rgbmax('dir')
else:
sleep(0.01) # Wait 0.01 second
sleep(10)
sound.beep()
sound.beep()
while pegar_cano: #começa virado pro vazio certo
#terminar com os sensores no vazio certo
sound.speak('pipe')
girar_pro_lado('esq',90)
alinhar_ate_achar('preto')
steering_pair.on_for_degrees(0,20,120)
mov_cores()
while cor('esq')!='branco':
steering_pair.on(10,20)
else:
steering_pair.off()
pegar_um_cano() #dentro dessa função tem que ter a mudança de pegar_cano (e a rotina pra caso ele não consiga pegar o cano)
cor_atual = cor('esq')
girar_pro_lado('dir',90)
alinhamento()
m = cor('esq') != 'preto'
n = cor('dir') != 'preto'
while m or n:
girar_pro_lado('dir',90)
alinhamento()
class RobotControl(RobotHandle):
def __init__(self, *args_module_name):
super().__init__(*args_module_name)
self.m_left = OUTPUT_B # Motor left
self.m_right = OUTPUT_A # Motor right
self.s_us = INPUT_1 # Sensor Ultrasonic
self.sl_left = INPUT_3 # Sensor Light left
self.sl_right = INPUT_4 # Sensor Right left
self.mMT = MoveTank(self.m_left, self.m_right) # move with 2 motors
self.mMS = MoveSteering(self.m_left, self.m_right) # move on position
self.sUS = UltrasonicSensor(self.s_us)
self.sLS_left = LightSensor(INPUT_3)
self.sLS_right = LightSensor(INPUT_4)
self.thread_detect_danger = ControlThread()
self.thread_detect_light_intesity = ControlThread()
self.stop_detect_light_intesity = False
#self.distance_cm = Value('d', 0.0)
def run(self):
#pool_detect_danger = Process(target=self.detect_danger, args=self.distance_cm)
#print('RUN: Distance: ', self.distance_cm.value)
#movement = ControlThread(self.mMT.on, 15, 15)
#thread_detect_light_intesity = ControlThread(self.detect_light_intensitiy)
self.thread_detect_light_intesity.function = self.detect_light_intensitiy
self.thread_detect_danger.function = self.detect_danger
thread_detect_danger = ControlThread(self.detect_danger)
try:
#self.mMT.on(15, 15)
self.now_stay_on_that_line()
except Exception:
self.mMT.off()
raise
#self.mMT.off()
#movement.start()
self.thread_detect_light_intesity.start()
self.thread_detect_danger.start()
return True
def detect_light_intensitiy(self):
while True:
if self.stop_detect_light_intesity:
break
if not args.light_mode:
args.light_mode = 'ambient'
if args.light_mode == 'ambient':
light_intesitiy_left = self.sLS_left.ambient_light_intensity
light_intesitiy_right = self.sLS_right.ambient_light_intensity
sleep(.500)
elif args.light_mode == 'reflected':
light_intesitiy_left = self.sLS_left.reflected_light_intensity
light_intesitiy_right = self.sLS_right.reflected_light_intensity
sleep(.500)
print('LEFT[{}]: {}'.format(args.light_mode, light_intesitiy_left))
print('RIGHT[{}]: {}'.format(args.light_mode,
light_intesitiy_right))
def get_light_intensitiy(self, side: str): # side = left, or right
if args.light.mode == 'ambient':
left = self.sLS_left.ambient_light_intensity
right = self.sLS_right.ambient_light_intensity
if args.light.mode == 'reflected':
left = self.sLS_left.reflected_light_intensity
right = self.sLS_right.reflected_light_intensity
if side == 'left':
return left
elif side == 'right':
return right
def detect_danger(self):
while True:
d = self.sUS.distance_centimeters
#print('dd: distance:', d)
if d < 10:
self.turn_on_wand()
self.mMT.stop()
#args.light_mode = 'reflected'
pass
sleep(.500)
def turn_on_wand(self):
self.mMS.on_for_degrees(-100, 20, parser.d)
def now_stay_on_that_line(self):
# kp=3, ki=0, kd=0
self.mMT.cs = self.sl_left
self.mMT.follow_line(3, 0, 0, 'left', SpeedPercent(20))
def follow_line(self,
kp,
ki,
kd,
side,
speed,
target_light_intensity=None,
follow_left_edge=True,
white=60,
off_line_count_max=20,
sleep_time=0.01,
follow_for=follow_for_forever,
**kwargs):
"""
PID line follower
``kp``, ``ki``, and ``kd`` are the PID constants.
``speed`` is the desired speed of the midpoint of the robot
``target_light_intensity`` is the reflected light intensity when the color sensor
is on the edge of the line. If this is None we assume that the color sensor
is on the edge of the line and will take a reading to set this variable.
``follow_left_edge`` determines if we follow the left or right edge of the line
``white`` is the reflected_light_intensity that is used to determine if we have
lost the line
``off_line_count_max`` is how many consecutive times through the loop the
reflected_light_intensity must be greater than ``white`` before we
declare the line lost and raise an exception
``sleep_time`` is how many seconds we sleep on each pass through
the loop. This is to give the robot a chance to react
to the new motor settings. This should be something small such
as 0.01 (10ms).
``follow_for`` is called to determine if we should keep following the
line or stop. This function will be passed ``self`` (the current
``MoveTank`` object). Current supported options are:
- ``follow_for_forever``
- ``follow_for_ms``
``**kwargs`` will be passed to the ``follow_for`` function
Example:
.. code:: python
from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent, follow_for_ms
from ev3dev2.sensor.lego import ColorSensor
tank = MoveTank(OUTPUT_A, OUTPUT_B)
tank.cs = ColorSensor()
try:
# Follow the line for 4500ms
tank.follow_line(
kp=11.3, ki=0.05, kd=3.2,
speed=SpeedPercent(30),
follow_for=follow_for_ms,
ms=4500
)
except Exception:
tank.stop()
raise
"""
#assert self.cs, "ColorSensor must be defined"
if target_light_intensity is None:
target_light_intensity = self.get_light_intensitiy(side)
integral = 0.0
last_error = 0.0
derivative = 0.0
off_line_count = 0
speed_native_units = speed.to_native_units(self.left_motor)
MAX_SPEED = SpeedNativeUnits(self.max_speed)
while follow_for(self, **kwargs):
reflected_light_intensity = self.cs.reflected_light_intensity
error = target_light_intensity - reflected_light_intensity
integral = integral + error
derivative = error - last_error
last_error = error
turn_native_units = (kp * error) + (ki * integral) + (kd *
derivative)
if not follow_left_edge:
turn_native_units *= -1
left_speed = SpeedNativeUnits(speed_native_units -
turn_native_units)
right_speed = SpeedNativeUnits(speed_native_units +
turn_native_units)
if left_speed > MAX_SPEED:
print("%s: left_speed %s is greater than MAX_SPEED %s" %
(self, left_speed, MAX_SPEED))
self.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
if right_speed > MAX_SPEED:
print("%s: right_speed %s is greater than MAX_SPEED %s" %
(self, right_speed, MAX_SPEED))
self.stop()
raise LineFollowErrorTooFast(
"The robot is moving too fast to follow the line")
# Have we lost the line?
if reflected_light_intensity >= white:
off_line_count += 1
if off_line_count >= off_line_count_max:
self.stop()
raise LineFollowErrorLostLine("we lost the line")
else:
off_line_count = 0
if sleep_time:
time.sleep(sleep_time)
self.on(left_speed, right_speed)
self.stop()
# create an object for the color sensor on input 2
colorLeft = ColorSensor(INPUT_2)
# create an object for the motors on output b and c
steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C)
# set the direction for straight
direction = 0
# turn the motors on at speed 20
steering_drive.on(direction, 20)
# continuously check the color until the sensor detects pure green
while True:
if colorLeft.rgb[0] < 60 and colorLeft.rgb[1] > 75 and colorLeft.rgb[
2] < 40:
print('Left RGB: ' + str(colorLeft.rgb))
sleep(0.01)
# Explicitly stop motors at end of program
steering_drive.off()
break
sleep(3)
sound.speak('Commence drawing a regular pentagon')
for x in range(5):
steering_drive.on_for_seconds(steering=0, speed=20, seconds=3)
steering_drive.on_for_degrees(steering=-100, speed=20, degrees=160)
steering_drive.off()
GyroDrift()
show_text("Robot Run 2")
acceleration(degrees=DistanceToDegree(70), finalSpeed=50, steering=2)
while colorLeft.reflected_light_intensity > 10 and False == Constants.STOP:
robot.on(steering=2, speed=20)
robot.off()
acceleration(degrees=DistanceToDegree(13), finalSpeed=20, steering=2)
while colorLeft.reflected_light_intensity < Constants.WHITE and False == Constants.STOP:
#robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
#print("RobotRun2 stop=" + str(Constants.STOP), file=stderr)
MoveForwardWhite(distanceInCm=2)
robot.on_for_degrees(degrees=DistanceToDegree(0.75), steering=2, speed=-10)
robot.off()
counter = 0
while counter < 5 and False == Constants.STOP:
robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
robot.on_for_degrees(degrees=DistanceToDegree(0.75), steering=2, speed=-10)
counter += 1
robot.off()
accelerationMoveBackward(degrees=DistanceToDegree(10), steering=-15, finalSpeed=-20)
GyroTurn(steering=-100, angle=40)
acceleration(degrees=DistanceToDegree(10.5), finalSpeed=20)
while colorRight.reflected_light_intensity < Constants.WHITE and False == Constants.STOP:
robot.on(speed=10, steering=0)
robot.off()
#!/usr/bin/env from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) steer_pair.on_for_degrees(steering=100, speed=20, degrees=930)
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from time import sleep steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C) # gentle turn left steer_pair.on_for_rotations(steering=-25, speed=75, rotations=2) sleep(1) # same as above but using degrees instead of rotations steer_pair.on_for_degrees(steering=-25, speed=75, degrees=720) sleep(1) # turn right on the spot for 3 seconds steer_pair.on_for_seconds(steering=100, speed=40, seconds=3) sleep(1) # equivalent to above steer_pair.on(steering=100, speed=40) sleep(3) steer_pair.off()
from BasicFunctions import *
deg = DistanceToDegree(20)
lineFollow(degrees=deg, GAIN=0.75, color=ColorSensor(INPUT_3))
lineFollowTillIntersection(GAIN=0.75, color=ColorSensor(INPUT_3), color2=ColorSensor(INPUT_1))
robot= MoveSteering(OUTPUT_B, OUTPUT_C)
gyro=GyroSensor(INPUT_2)
colorleft = ColorSensor(INPUT_1)
colorright = ColorSensor(INPUT_4)
lmB = LargeMotor(OUTPUT_B)
lmR = LargeMotor(OUTPUT_C)
acceleration(65, 50, 1, 5.4, MoveSteering(OUTPUT_B, OUTPUT_C), LM=lmB)
while colorleft.reflected_light_intensity < 70:
robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
robot.on_for_degrees(speed=-20, steering = 0, degrees = DistanceToDegree(0.5))
while colorleft.reflected_light_intensity > 10:
robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
robot.on_for_degrees(speed=-20, steering = 0, degrees = DistanceToDegree(0.5))
while colorleft.reflected_light_intensity < 70:
robot.on_for_degrees(speed=20, steering = 0, degrees = DistanceToDegree(2))
robot.on_for_degrees(speed=-20, steering = 0, degrees = DistanceToDegree(0.5))
robot.on_for_degrees(speed=-30, steering=-5, degrees=DistanceToDegree(20))
GyroTurn(50, 90, gyro, robot)
robot.on_for_degrees(speed=-30, steering=0, degrees=DistanceToDegree(90))
GyroTurn(angle=90, steering=100)
global rgbmax_d
global rgbmax_e
waiting = False
ir_pro_gasoduto = True
rgbmax_e = definir_rgbmax('esq')
rgbmax_d = definir_rgbmax('dir')
else:
sleep(0.01) # Wait 0.01 second
sleep(10)
sound.beep()
sound.beep()
while ir_pro_gasoduto: #começa com os sensores no vazio certo
#termina paralelo ao gasoduto
steering_pair.on_for_degrees(0, -20, 350)
girar_pro_lado('esq', 90)
while cor('esq') != 'azul' and cor('dir') != 'azul':
steering_pair.on(0, -40)
else:
steering_pair.off()
steering_pair.on_for_degrees(0, -55, 300)
girar_pro_lado('esq', 180)
while usf.distance_centimeters > 15:
steering_pair.on(0, 10)
else:
steering_pair.off()
if usf.distance_centimeters < 16 and cor('esq') == 'azul':
girar_pro_lado('dir', 90)
sound.speak('go go go')
garra_g.on_for_degrees(20, -1200)
class run2019:
def __init__(self):
self.btn = Button()
self.LLight = LightSensor(INPUT_1)
self.RLight = LightSensor(INPUT_4)
self.cs = ColorSensor()
self.drive = MoveTank(OUTPUT_A, OUTPUT_D)
self.steer = MoveSteering(OUTPUT_A, OUTPUT_D)
self.heightmotor = LargeMotor(OUTPUT_B)
self.clawactuator = MediumMotor(OUTPUT_C)
os.system('setfont Lat15-TerminusBold14')
self.speed = 40
self.sectionCache = 0
self.orient = {'N': "1", 'E': "1", 'S': "1", 'W': "1"}
def sensordata(self):
print("Left Light Sensor: ", end="", file=sys.stderr)
print(self.LLight.reflected_light_intensity, end=" ", file=sys.stderr)
print("Right Light Sensor: ", end="", file=sys.stderr)
print(self.RLight.reflected_light_intensity, file=sys.stderr)
def turn(self, direc): # Half - works
self.drive.on_for_degrees(SpeedDPS(225), SpeedDPS(225), 223)
if direc == "L" or direc == "l":
self.steer.on_for_degrees(steering=-100,
speed=SpeedDPS(720),
degrees=400)
elif direc == "R" or direc == "r":
self.steer.on_for_degrees(steering=100,
speed=SpeedDPS(720),
degrees=720)
self.steer.off()
def dti(self,
speed,
n,
startCounting=False,
sectionCache=0): # Drive to nth intersection
kp = 1.1
ki = 0
kd = 0
integral = 0
perror = error = 0
inters = 0
piderror = 0
while not self.btn.any(
): # Remember to try stuff twice, this is a keyboard interrupt
lv = self.LLight.reflected_light_intensity
rv = self.RLight.reflected_light_intensity
error = rv - lv
integral += integral + error
derivative = lv - perror
piderror = (error * kp) + (integral * ki) + (derivative * kd)
if speed + abs(piderror) > 100:
if piderror >= 0:
piderror = 100 - speed
else:
piderror = speed - 100
self.drive.on(left_speed=speed - piderror,
right_speed=speed + piderror)
sleep(0.01)
perror = error
# Drive up to nth intersection
# These values are subject to change depending on outside factors, CHANGE ON COMPETITION DAY
if (lv <= 50 and rv <= 55) or (lv <= 50 and rv >= 55) or (
lv >= 50 and rv <= 55): # Currently at an intersection
inters += 1
if (startCounting == True):
sectionCache += 1
if inters == n: # Currently at nth intersection
self.drive.off()
return
self.drive.off()
self.drive.on_for_seconds(SpeedDPS(115), SpeedDPS(115), 1)
print("Left Value: {}, Right Value: {}, P error: {}, Inters: {}".
format(lv, rv, piderror, inters),
file=sys.stderr)
def main(self):
self.heightmotor.on(speed=self.speed)
self.heightmotor.wait_until_not_moving()
# # while not btn.any():
# # sensordata()
# # ## STORING COLOURS
self.drive.on_for_degrees(
left_speed=self.speed, right_speed=self.speed,
degrees=50) # To drive past little initial intersection
print(self.orient, file=sys.stderr) #DEBUG
self.turn("L")
# # # GO TO FIRST BNPs
self.dti(self.speed, 5, startCounting=True)
self.turn("L")
self.dti(self.speed, 1)
sleep(10)
sound.beep()
sound.beep()
while aprender_cores: #começa com os sensores de cor no vazio certo
#termina virado pro vazio certo
h = os.path.exists("cores.txt")
if h:
global aprender_cores
global mapadecores
mapadecores = [line.rstrip('\n') for line in open("cores.txt")] # pra ler o arquivo pra lista de novo
aprender_cores = False
else: #verificar se já existe o arquivo, se não existir:
cores = open("cores.txt", "w+") #cria o arquivo
cores.close() #fecha o arquivo
steering_pair.on_for_degrees(0,-20,350)
girar_pro_lado('esq',90) #90 graus pra esquerda
alinhamento() #achou a faixa preta
# sleep(0.1)
# k = (cor('dir') != 'amarelo') and (cor('dir') != 'azul') and (cor('dir') != 'vermelho')
# l = (cor('esq') != 'amarelo') and (cor('esq') != 'azul') and (cor('esq') != 'vermelho')
# while k and l:
# alinhamento() #achou a primeira cor
# sleep(0.1)
# k = (cor('dir') != 'amarelo') and (cor('dir') != 'azul') and (cor('dir') != 'vermelho')
# l = (cor('esq') != 'amarelo') and (cor('esq') != 'azul') and (cor('esq') != 'vermelho')
# steering_pair.on_for_degrees(0,20,100)
steering_pair.on_for_degrees(0,20,100)
girar_pro_lado('esq',90)
cor1 = cor('dir') #salvar primeira cor
while cor('dir')==cor1 or cor('dir')=='preto':
log = open('log.txt', 'a')
log.write('Sensores no branco, indo pra frente\n')
log.close()
steering_pair.on(0, 20)
else:
log = open('log.txt', 'a')
log.write('Algum sensor saiu do branco, para.\n')
log.close()
steering_pair.off()
#trocar isso acima por alinhamento()
#steering_pair.on_for_degrees(0,10,30)
if cor('esq') == 'vazio' or cor('dir') == 'vazio':
log = open('log.txt', 'a')
log.write('Um dos sensores está no vazio\n')
log.close()
steering_pair.on_for_degrees(0, -10, 120)
sleep(1)
alinhamento()
sleep(1)
steering_pair.on_for_degrees(0, -10, 350)
sleep(1)
girar_pro_lado('esq', 90)
sleep(1)
else:
log = open('log.txt', 'a')
log.write('Nenhum sensor está no vazio\n')
log.close()
steering_pair.on_for_degrees(0, -10, 130)
alinhamento()
sleep(1)
a = testar_preto()
#!/usr/bin/env python3
from ev3dev2.motor import OUTPUT_B, OUTPUT_C, MoveSteering
from ev3dev2.sensor.lego import TouchSensor
from time import sleep
ts = TouchSensor()
sound = Sound()
steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
steer_pair.on_for_rotations(steering=0, speed=50, rotations=4.5, brake=True, block=True)
sleep(1)
steer_pair.on_for_degrees(steering=100, speed=10, degrees=180, brake=True, block=True)
sleep(1)
steer_pair.on_for_rotations(steering=0, speed=50, rotations=3)
sleep(1)
steer_pair.on_for_degrees(steering=100, speed=10, degrees=180, brake=True, block=True)
sleep(1)
steer_pair.on_for_rotations(steering=0, speed=50, rotations=5)
sleep(1)
steer_pair.on_for_degrees(steering=100, speed=10, degrees=180, brake=True, block=True)
sleep(1)
steer_pair.on(steering=0, speed=50)
while not ts.is_pressed: # while touch sensor is not pressed
sleep(0.01)
tank_pair.off()
sleep(5)
#!/usr/bin/env python3 from ev3dev2.motor import MoveSteering, OUTPUT_B, OUTPUT_C from time import sleep # Demonstrate how to use MoveSteering class. # Have the robot rotate 360 degrees on one wheel then the other # Using the same four methods we are now familiar with: # on(), on_for_degrees(), on_for_rotations(), on_for_seconds() steering_drive = MoveSteering(OUTPUT_B, OUTPUT_C) steering_drive.on(-100, 35) sleep(2) steering_drive.on(100, 35) sleep(2) steering_drive.stop() # Store the value of 2 360 rotations in a variable degrees = 360 * 2 # -100 = turn all the way to the left; steering_drive.on_for_degrees(-100, 30, degrees) # 100 = turn all the way to the right steering_drive.on_for_degrees(100, 30, degrees) steering_drive.on_for_rotations(-100, 30, 2) steering_drive.on_for_rotations(100, 30, 2)
tank.on_for_rotations(50, 10, 6) #Drawing letter " q " #!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, MediumMotor, MoveTank, MoveSteering, OUTPUT_A, OUTPUT_B, OUTPUT_C Medium_motor = MediumMotor(OUTPUT_A) Large_motor1 = LargeMotor(OUTPUT_B) Large_motor2 = LargeMotor(OUTPUT_C) steer_Pair = MoveSteering(OUTPUT_B, OUTPUT_C) tank = MoveTank(OUTPUT_B, OUTPUT_C) Medium_motor.on_for_degrees(50, 500) steer_Pair.on_for_degrees(steering = 90, speed = 75, degrees = 1080) tank.on_for_degrees(50, 50, 360) #Drawing letter " F " #!/usr/bin/env python3 from ev3dev2.motor import LargeMotor, MediumMotor, MoveTank, MoveSteering, OUTPUT_A, OUTPUT_B, OUTPUT_C Medium_motor = MediumMotor(OUTPUT_A) Large_motor1 = LargeMotor(OUTPUT_B) Large_motor2 = LargeMotor(OUTPUT_C) steer_Pair = MoveSteering(OUTPUT_B, OUTPUT_C) tank = MoveTank(OUTPUT_B, OUTPUT_C) Medium_motor.on_for_degrees(50, 500) tank.on_for_degrees(70, 70, 360)
#ver o comprimento dessa faixa estável
#voltar pra onde essa faixa acabou
achou_fim_cano = False
achou_cano = False
dist = []
while distancia_do_corre(usl) > 40:
steering_pair.on(0, 50)
else:
sleep(2)
sound.beep()
steering_pair.off()
while not (achou_fim_cano):
sleep(0.02)
steering_pair.on_for_degrees(0, 10, 10)
sleep(0.02)
d = distancia_do_corre(usl)
dist.append(d)
if len(dist) > 20: #ter certeza que já tem 7
a = 0
for i in range(20): #pega os 7 últimos e faz a média
a = a + dist[len - 1 - i]
media_dos_7 = a / 20
b = 0
for i in range(20):
if abs(dist[len - 1 - i] - media_dos_7
) > 5: #marcar quantos resultados tão instáveis
b += 1
if b == 0 and not (achou_cano):
sleep(1)
class CarControl:
def __init__(self):
self.tank_pair = MoveTank(OUTPUT_A, OUTPUT_B)
self.steer_pair = MoveSteering(OUTPUT_A, OUTPUT_B)
self.line_sensor = ColorSensor(INPUT_2)
self.line_counter = ColorSensor(INPUT_3)
self.sound = Sound()
self.gyro = GyroSensor(INPUT_1)
self.gyro.reset()
self.dir_state = "S"
def drive(self, l_speed, r_speed):
self.tank_pair.on(left_speed=l_speed, right_speed=r_speed)
def reverse(self):
self.tank_pair.on_for_degrees(left_speed=-90,
right_speed=-90,
degrees=360,
brake=True)
def brake(self):
self.tank_pair.off(brake=True)
def turn_left(self):
self.steer_pair.on_for_degrees(steering=-100,
speed=90,
degrees=180,
brake=True)
def turn_right(self):
self.steer_pair.on_for_degrees(steering=100,
speed=90,
degrees=170,
brake=True)
def turn_arround(self):
self.steer_pair.on_for_degrees(steering=-100,
speed=90,
degrees=370,
brake=True)
def read_line_sensor(self):
return (self.line_sensor.reflected_light_intensity)
def read_line_counter(self):
return (self.line_counter.reflected_light_intensity)
def play_sound(self):
self.sound.beep()
def read_gyro(self):
return self.gyro.angle
def turn_left_new(self):
self.tank_pair.on(left_speed=-90, right_speed=90)
if self.dir_state == "S":
while True:
if self.read_gyro(
) <= -90: #Ikke sikker på at den altid kan læse ved præcist -90
self.tank_pair.off(brake=True)
self.dir_state = "L"
return 0
elif self.dir_state == "L":
while True:
if self.read_gyro() == 180 or self.read_gyro() == -180:
self.steer_pair.stop()
self.dir_state = "B"
return 0
elif self.dir_state == "B":
while True:
if self.read_gyro() == 90 or self.read_gyro() == -270:
self.steer_pair.stop()
self.dir_state = "R"
return 0
elif self.dir_state == "R":
while True:
if self.read_gyro(
) == 0 or self.read_gyro == -360 or self.read_gyro == 360:
self.gyro.reset()
self.steer_pair.stop()
self.dir_state = "S"
return 0
def turn_right_new(self):
self.steer_pair.on(90, 90)
print("JEG ER HER")
if self.dir_state == "S":
while True:
if self.read_gyro == 90:
self.steer_pair.stop()
self.dir_state = "R"
return 0
elif self.dir_state == "L":
while True:
if self.read_gyro() == 360 or self.read_gyro() == 0:
self.gyro.reset()
self.steer_pair.stop()
self.dir_state = "S"
return 0
elif self.dir_state == "B":
while True:
if self.read_gyro() == 270 or self.read_gyro == -90:
self.steer_pair.stop()
self.dir_state == "R"
return 0
elif self.dir_state == "R":
while True:
if self.read_gyro() == 180 or self.read_gyro() == -180:
self.steer_pair.stop()
self.dir_state == "B"
return 0
