def driveStraightForDistance(self,
distance,
speedLeft=None,
speedRight=None,
stopAtEnd=True):
''' Drive for given distance.
Args:
distance: distance to drive in mm
speedLeft: speed in mm/s (left motor)
speedRight: speed in mm/s (right motor)
stopAtEnd (bool): brake at end of movement
'''
if speedLeft == None or speedLeft == 0:
speedLeft = self.defaultSpeed
if speedRight == None or speedRight == 0:
speedRight = self.defaultSpeed
degPerSecLeft = 360 * speedLeft / (pi * self.wheel_diameter)
degPerSecRight = 360 * speedRight / (pi * self.wheel_diameter)
driveTime = StopWatch()
driveTime.start()
self.leftDriveMotor.on(SpeedNativeUnits(degPerSecLeft), block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSecRight), block=False)
while driveTime.value_ms <= abs(distance / speedLeft) * 1000:
sleep(0.02)
if stopAtEnd:
self.leftDriveMotor.stop()
self.rightDriveMotor.stop()
sleep(0.01)
def _wait(self, wait_for_button_press, wait_for_button_release, timeout_ms):
stopwatch = StopWatch()
stopwatch.start()
# wait_for_button_press/release can be a list of buttons or a string
# with the name of a single button. If it is a string of a single
# button convert that to a list.
if isinstance(wait_for_button_press, str):
wait_for_button_press = [wait_for_button_press, ]
if isinstance(wait_for_button_release, str):
wait_for_button_release = [wait_for_button_release, ]
for event in self.evdev_device.read_loop():
if event.type == evdev.ecodes.EV_KEY:
all_pressed = True
all_released = True
pressed = self.buttons_pressed
for button in wait_for_button_press:
if button not in pressed:
all_pressed = False
break
for button in wait_for_button_release:
if button in pressed:
all_released = False
break
if all_pressed and all_released:
return True
if timeout_ms is not None and stopwatch.value_ms >= timeout_ms:
return False
def __init__(self, motor_izquierdo, motor_derecho, radio_rueda,
separacion_ruedas, posicion, nav_sonar, nav_sensor_color):
localizacion.__init__(self, motor_izquierdo, motor_derecho,
radio_rueda, separacion_ruedas, posicion,
nav_sonar, nav_sensor_color)
#Fichero
self.f = None
self.escribir_fichero_activo = False
self.fin_escribir_fichero = True
self.t = StopWatch()
def _animate_rainbow():
# state 0: (LEFT,RIGHT) from (0,0) to (1,0)...RED
# state 1: (LEFT,RIGHT) from (1,0) to (1,1)...AMBER
# state 2: (LEFT,RIGHT) from (1,1) to (0,1)...GREEN
# state 3: (LEFT,RIGHT) from (0,1) to (0,0)...OFF
state = 0
left_value = 0
right_value = 0
MIN_VALUE = 0
MAX_VALUE = 1
self.all_off()
duration_ms = duration * 1000 if duration is not None else None
stopwatch = StopWatch()
stopwatch.start()
while True:
if state == 0:
left_value += increment_by
elif state == 1:
right_value += increment_by
elif state == 2:
left_value -= increment_by
elif state == 3:
right_value -= increment_by
else:
raise Exception("Invalid state {}".format(state))
# Keep left_value and right_value within the MIN/MAX values
left_value = min(left_value, MAX_VALUE)
right_value = min(right_value, MAX_VALUE)
left_value = max(left_value, MIN_VALUE)
right_value = max(right_value, MIN_VALUE)
self.set_color(group1, (left_value, right_value))
self.set_color(group2, (left_value, right_value))
if state == 0 and left_value == MAX_VALUE:
state = 1
elif state == 1 and right_value == MAX_VALUE:
state = 2
elif state == 2 and left_value == MIN_VALUE:
state = 3
elif state == 3 and right_value == MIN_VALUE:
state = 0
if self.animate_thread_stop or stopwatch.is_elapsed_ms(
duration_ms):
break
sleep(sleeptime)
self.animate_thread_stop = False
self.animate_thread_id = None
def wait_for_bump(self, buttons, timeout_ms=None):
"""
Wait for the button to be pressed down and then released.
Both actions must happen within timeout_ms.
"""
stopwatch = StopWatch()
stopwatch.start()
if self.wait_for_pressed(buttons, timeout_ms):
if timeout_ms is not None:
timeout_ms -= stopwatch.value_ms
return self.wait_for_released(buttons, timeout_ms)
return False
def _animate_flash():
even = True
duration_ms = duration * 1000 if duration is not None else None
stopwatch = StopWatch()
stopwatch.start()
while True:
if even:
for group in groups:
self.set_color(group, color)
else:
self.all_off()
if self.animate_thread_stop or stopwatch.is_elapsed_ms(duration_ms):
break
even = not even
sleep(sleeptime)
self.animate_thread_stop = False
self.animate_thread_id = None
def _animate_flash():
even = True
duration_ms = duration * 1000
stopwatch = StopWatch()
stopwatch.start()
while True:
if even:
for group in groups:
self.set_color(group, color)
else:
self.all_off()
if self.animate_thread_stop or stopwatch.value_ms >= duration_ms:
break
even = not even
sleep(sleeptime)
self.animate_thread_stop = False
self.animate_thread_id = None
def _animate_cycle():
index = 0
max_index = len(colors)
duration_ms = duration * 1000 if duration is not None else None
stopwatch = StopWatch()
stopwatch.start()
while True:
for group in groups:
self.set_color(group, colors[index])
index += 1
if index == max_index:
index = 0
if self.animate_thread_stop or stopwatch.is_elapsed_ms(duration_ms):
break
sleep(sleeptime)
self.animate_thread_stop = False
self.animate_thread_id = None
def _animate_police_lights():
self.all_off()
even = True
duration_ms = duration * 1000 if duration is not None else None
stopwatch = StopWatch()
stopwatch.start()
while True:
if even:
self.set_color(group1, color1)
self.set_color(group2, color2)
else:
self.set_color(group1, color2)
self.set_color(group2, color1)
if self.animate_thread_stop or stopwatch.is_elapsed_ms(duration_ms):
break
even = not even
sleep(sleeptime)
self.animate_thread_stop = False
self.animate_thread_id = None
from ev3dev2.console import Console
from ev3dev2.sound import Sound
from random import randint
from threading import Thread
from time import sleep
# Logging
logging.basicConfig(level=logging.DEBUG,
format='%(asctime)s %(levelname)5s: %(message)s')
log = logging.getLogger(__name__)
log.info("Starting Gyro Boy")
# Initialize
running = True
state = "reset"
timer = StopWatch()
timer.start()
# Brick variables
sound = Sound()
leds = Leds()
console = Console()
gyro_sensor = GyroSensor(INPUT_2)
gyro_sensor.mode = GyroSensor.MODE_GYRO_RATE
arms_motor = MediumMotor(OUTPUT_C)
left_motor = LargeMotor(OUTPUT_D)
right_motor = LargeMotor(OUTPUT_A)
color_sensor = ColorSensor(INPUT_1)
ultra_sonic_sensor = UltrasonicSensor(INPUT_4)
def test_stopwatch(self):
sw = StopWatch()
self.assertEqual(str(sw), "StopWatch: 00:00:00.000")
sw = StopWatch(desc="test sw")
self.assertEqual(str(sw), "test sw: 00:00:00.000")
self.assertEqual(sw.is_started, False)
sw.start()
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 0)
self.assertEqual(sw.value_secs, 0)
self.assertEqual(sw.value_hms, (0, 0, 0, 0))
self.assertEqual(sw.hms_str, "00:00:00.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
set_mock_ticks_ms(1500)
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 1500)
self.assertEqual(sw.value_secs, 1.5)
self.assertEqual(sw.value_hms, (0, 0, 1, 500))
self.assertEqual(sw.hms_str, "00:00:01.500")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
set_mock_ticks_ms(3000)
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 3000)
self.assertEqual(sw.value_secs, 3)
self.assertEqual(sw.value_hms, (0, 0, 3, 0))
self.assertEqual(sw.hms_str, "00:00:03.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), True)
self.assertEqual(sw.is_elapsed_secs(3), True)
set_mock_ticks_ms(1000 * 60 * 75.5) #75.5 minutes
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 1000 * 60 * 75.5)
self.assertEqual(sw.value_secs, 60 * 75.5)
self.assertEqual(sw.value_hms, (1, 15, 30, 0))
self.assertEqual(sw.hms_str, "01:15:30.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), True)
self.assertEqual(sw.is_elapsed_secs(3), True)
try:
# StopWatch can't be started if already running
sw.start()
self.fail()
except StopWatchAlreadyStartedException:
pass
# test reset behavior
sw.restart()
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 0)
self.assertEqual(sw.value_secs, 0)
self.assertEqual(sw.value_hms, (0, 0, 0, 0))
self.assertEqual(sw.hms_str, "00:00:00.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
set_mock_ticks_ms(1000 * 60 * 75.5 + 3000)
self.assertEqual(sw.is_started, True)
self.assertEqual(sw.value_ms, 3000)
self.assertEqual(sw.value_secs, 3)
self.assertEqual(sw.value_hms, (0, 0, 3, 0))
self.assertEqual(sw.hms_str, "00:00:03.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), True)
self.assertEqual(sw.is_elapsed_secs(3), True)
# test stop
sw.stop()
set_mock_ticks_ms(1000 * 60 * 75.5 + 10000)
self.assertEqual(sw.is_started, False)
self.assertEqual(sw.value_ms, 3000)
self.assertEqual(sw.value_secs, 3)
self.assertEqual(sw.value_hms, (0, 0, 3, 0))
self.assertEqual(sw.hms_str, "00:00:03.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), True)
self.assertEqual(sw.is_elapsed_secs(3), True)
# test reset
sw.reset()
self.assertEqual(sw.is_started, False)
self.assertEqual(sw.value_ms, 0)
self.assertEqual(sw.value_secs, 0)
self.assertEqual(sw.value_hms, (0, 0, 0, 0))
self.assertEqual(sw.hms_str, "00:00:00.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
set_mock_ticks_ms(1000 * 60 * 75.5 + 6000)
self.assertEqual(sw.is_started, False)
self.assertEqual(sw.value_ms, 0)
self.assertEqual(sw.value_secs, 0)
self.assertEqual(sw.value_hms, (0, 0, 0, 0))
self.assertEqual(sw.hms_str, "00:00:00.000")
self.assertEqual(sw.is_elapsed_ms(None), False)
self.assertEqual(sw.is_elapsed_secs(None), False)
self.assertEqual(sw.is_elapsed_ms(3000), False)
self.assertEqual(sw.is_elapsed_secs(3), False)
def followLineDist(self,
distance,
sensor,
sideToFollow,
stopAtEnd,
speed,
gain_multiplier=1):
''' Drive forward following line.
Uses direct control of drive motors and PD control.
Args:
distance: distance to follow line in mm
sensor: colorsensor to use for line following
sideToFollow: which side of line to follow: LineEdge.LEFT or LineEdge.RIGHT
stopAtEnd (bool): Stop motors at end of line following
speed: speed in mm/s
gain_multiplier: multiplier for P and D gains
'''
degPerSec = 360 * speed / (pi * self.wheel_diameter)
propGain = 6.0 * gain_multiplier
derivGain = 6 * gain_multiplier
target = (self.blackThreshold + self.whiteThreshold) / 2
print("******* starting line following ********")
# print("error,Pterm,Dterm")
# initialize term for derivative calc
previousError = target - avgReflection(sensor, 2)
i = 0
driveTime = StopWatch()
driveTime.start()
while driveTime.value_ms <= abs(distance / speed) * 1000:
# calc error and proportional term
error = target - avgReflection(sensor, 2)
Pterm = propGain * error
# calc d(error)/d(t) and derivative term
d_error_dt = error - previousError
Dterm = derivGain * d_error_dt
previousError = error
if sideToFollow == LineEdge.RIGHT:
self.leftDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm +
Dterm),
block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm -
Dterm),
block=False)
# eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging
if sideToFollow == LineEdge.LEFT:
self.leftDriveMotor.on(SpeedNativeUnits(degPerSec - Pterm -
Dterm),
block=False)
self.rightDriveMotor.on(SpeedNativeUnits(degPerSec + Pterm +
Dterm),
block=False)
# eprint("{:7.2f},{:7.2f},{:7.2f}".format(error, Pterm, Dterm)) # for debugging
# eprint("line following complete")
if stopAtEnd:
self.leftDriveMotor.stop()
self.rightDriveMotor.stop()
sleep(0.01)
# Play sound when we stop line following
sound.beep()
class navegacion(localizacion):
def __init__(self, motor_izquierdo, motor_derecho, radio_rueda,
separacion_ruedas, posicion, nav_sonar, nav_sensor_color):
localizacion.__init__(self, motor_izquierdo, motor_derecho,
radio_rueda, separacion_ruedas, posicion,
nav_sonar, nav_sensor_color)
#Fichero
self.f = None
self.escribir_fichero_activo = False
self.fin_escribir_fichero = True
self.t = StopWatch()
def coordenadas_global_a_robot(self, robot, punto_global):
R = np.array([[cos(robot[3][0]), -sin(robot[3][0]), 0.0],
[sin(robot[3][0]),
cos(robot[3][0]), 0.0], [0.0, 0.0, 1.0]])
Rt = R.transpose()
aux = -(Rt @ robot[:3])
T = np.array([[Rt[0][0], Rt[0][1], Rt[0][2], aux[0][0]],
[Rt[1][0], Rt[1][1], Rt[1][2], aux[1][0]],
[Rt[2][0], Rt[2][1], Rt[2][2], aux[2][0]], [0, 0, 0, 1]])
p = np.array([[punto_global[0][0]], [punto_global[1][0]],
[punto_global[2][0]], [1]])
resultado = T @ p
return resultado[:3]
def navegacion_planificada(self, puntos_objetivos, KW):
vector_hasta_destino = np.array([0.0, 0.0, 0.0])
for punto in puntos_objetivos:
while 1:
robot = self.odometria("RK4")
vector_hasta_destino[0] = punto[0] - robot[0][0]
vector_hasta_destino[1] = punto[1] - robot[1][0]
vector_hasta_destino[2] = punto[2] - robot[2][0]
modulo = sqrt(vector_hasta_destino @ vector_hasta_destino.T)
if (modulo <= 0.05):
sound.beep()
break
angulo_objetivo = atan2(vector_hasta_destino[1],
vector_hasta_destino[0])
if angulo_objetivo < 0:
angulo_objetivo = angulo_objetivo + 2 * pi
angulo_robot = robot[3][0]
while angulo_robot > 2 * pi:
angulo_robot = angulo_robot - 2 * pi
while angulo_robot < 0:
angulo_robot = angulo_robot + 2 * pi
angulo = angulo_objetivo - angulo_robot
if angulo < -pi:
angulo = angulo + 2 * pi
if angulo > pi:
angulo = -(2 * pi - angulo)
w = KW * angulo
if w > pi:
w = pi
if w < -pi:
w = -pi
self.correr(0.2, w)
self.parar()
def navegacion_reactiva_campos_virtuales(self, objetivo, rTs, KA, KR):
vector_resultante = np.array([0.0, 0.0, 0.0])
while 1:
vector_hasta_destino = self.coordenadas_global_a_robot(
self.odometria("RK4"), objetivo)
vector_de_repulsion = np.array([[0.0], [0.0], [0.0]])
modulo = sqrt(vector_hasta_destino[0].T @ vector_hasta_destino[0])
if (modulo <= 0.05):
break
obstaculo = rTs @ np.array([[self.s.distancia_sonar], [0.0], [0.0],
[1.0]])
modulo = sqrt(obstaculo[:3][0].T @ obstaculo[:3][0])
if modulo > 0.45:
vector_de_repulsion[0][0] = 0.0
vector_de_repulsion[1][0] = 0.0
vector_de_repulsion[2][0] = 0.0
else:
vector_de_repulsion = ((0.45 - modulo) / 0.45) * obstaculo
vector_resultante[0] = KA * vector_hasta_destino[0][
0] - KR * vector_de_repulsion[0][0]
vector_resultante[1] = KA * vector_hasta_destino[1][
0] - KR * vector_de_repulsion[1][0]
vector_resultante[2] = KA * vector_hasta_destino[2][
0] - KR * vector_de_repulsion[2][0]
v = 0.2 * vector_resultante[0]
w = 2 * vector_resultante[1]
if (v > 0.2):
v = 0.2
if (v < -0.2):
v = -0.2
if (w > pi):
w = pi
if (w < -pi):
w = -pi
self.correr(v, w)
self.parar()
#Guardar datos
def empezar_fichero(self, nombre_fichero, tiempo, *args, **kwargs):
def hilo_fichero():
self.t.start()
while self.escribir_fichero_activo:
for l in args:
if l == "tiempo":
self.f.write(str(self.t.value_ms / 1000) + " ")
elif l == "distancia_sonar":
self.f.write(str(self.s.distancia_sonar) + " ")
elif l == "otros_sensores_presentes":
self.f.write(
str(self.s.otros_sensores_presentes) + " ")
elif l == "color":
self.f.write(str(self.s.color) + " ")
elif l == "nombre_color":
self.f.write(str(self.s.nombre_color) + " ")
elif l == "ambiente":
self.f.write(str(self.s.ambiente) + " ")
elif l == "reflexion":
self.f.write(str(self.s.reflexion) + " ")
elif l == "rgb":
self.f.write(str(self.s.rgb) + " ")
elif l == "voltaje_bateria":
self.f.write(str(self.s.voltaje_bateria) + " ")
elif l == "corriente_bateria":
self.f.write(str(self.s.corriente_bateria) + " ")
elif l == "w_motor_derecho":
self.f.write(str(self.w_motor_derecho) + " ")
elif l == "w_motor_izquierdo":
self.f.write(str(self.w_motor_izquierdo) + " ")
elif l == "dc_motor_izquierdo":
self.f.write(str(self.dc_motor_izquierdo) + " ")
elif l == "dc_motor_derecho":
self.f.write(str(self.dc_motor_derecho) + " ")
elif l == "posicion_motor_derecho":
self.f.write(str(self.posicion_motor_derecho) + " ")
elif l == "velocidad_linear":
self.f.write(str(self.velocidad_linear) + " ")
elif l == "velocidad_angular":
self.f.write(str(self.velocidad_angular) + " ")
elif l == "odometria":
if "modo" in kwargs:
posicion = self.odometria(kwargs["modo"])
self.f.write(
str(posicion[0][0]) + " " +
str(posicion[1][0]) + " " +
str(posicion[2][0]) + " " +
str(posicion[3][0]) + " ")
elif l == "localizacion_probabilistica":
if ("mapa" in kwargs) and ("rox" in kwargs) and (
"roy" in kwargs) and (
"rotheta" in kwargs) and ("Rk" in kwargs):
mu, sigma = self.localizacion_probabilistica(
kwargs["mapa"], kwargs["rox"], kwargs["roy"],
kwargs["rotheta"], kwargs["Rk"])
self.f.write(
str(mu[0][0]) + " " + str(mu[1][0]) + " " +
str(mu[2][0]) + " " + str(sigma[0][0]) + " " +
str(sigma[0][1]) + " " + str(sigma[0][2]) +
" " + str(sigma[1][0]) + " " +
str(sigma[1][1]) + " " + str(sigma[1][2]) +
" " + str(sigma[2][0]) + " " +
str(sigma[2][1]) + " " + str(sigma[2][2]) +
" ")
self.f.write("\n")
sleep(tiempo)
self.t.stop()
self.fin_escribir_fichero = True
self.f = open(nombre_fichero, "w")
self.escribir_fichero_activo = True
self.fin_escribir_fichero = False
self.id_hilo_fichero = Thread(target=hilo_fichero)
self.id_hilo_fichero.start()
def parar_fichero(self):
self.escribir_fichero_activo = False
if not self.fin_escribir_fichero:
self.id_hilo_fichero.join(timeout=None)
self.f.close()
