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 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 __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()
