class DriveTime(Command):
'''
classdocs
'''
_stopwatch = None
_start_time = None
_duration = None
_speed = None
_ramp_threshold = None
def __init__(self, robot, duration, speed, ramp_threshold, name=None, timeout=None):
'''
Constructor
'''
super().__init__(name, timeout)
self.robot = robot;
self.requires(robot.drivetrain)
self._stopwatch = Stopwatch()
self._duration = duration
self._speed = speed
self._ramp_threshold = ramp_threshold
def initialize(self):
"""Called before the Command is run for the first time."""
# Start stopwatch
self._stopwatch.start()
return Command.initialize(self)
def execute(self):
"""Called repeatedly when this Command is scheduled to run"""
speed = self._speed
time_left = self._duration - self._stopwatch.elapsed_time_in_secs()
if (time_left < self._ramp_threshold):
speed = speed * time_left / self._ramp_threshold
self.robot.drivetrain.arcade_drive(speed, 0.0)
return Command.execute(self)
def isFinished(self):
"""Returns true when the Command no longer needs to be run"""
# If elapsed time is more than duration
return self._stopwatch.elapsed_time_in_secs() >= self._duration
def end(self):
"""Called once after isFinished returns true"""
self._stopwatch.stop()
# Stop driving
self.robot.drivetrain.arcade_drive(0.0, 0.0)
def interrupted(self):
"""Called when another command which requires one or more of the same subsystems is scheduled to run"""
self.end()
class RaisArmTime(Command):
def __init__(self, robot, raise_speed, stop_time, name=None, timeout=None):
'''
Constructor
'''
super().__init__(name, timeout)
self._robot = robot
self._raise_speed = raise_speed
self._stop_time = stop_time
self.requires(robot.arm)
self._stopwatch = Stopwatch()
def initialize(self):
"""Called before the Command is run for the first time."""
self._stopwatch.start()
def execute(self):
"""Called repeatedly when this Command is scheduled to run"""
self._robot.arm.move_arm(self._raise_speed)
def isFinished(self):
"""Returns true when the Command no longer needs to be run"""
return self._stopwatch.elapsed_time_in_secs() >= self._stop_time
def end(self):
"""Called once after isFinished returns true"""
self._robot.arm.move_arm(0)
def interrupted(self):
"""Called when another command which requires one or more of the same subsystems is scheduled to run"""
self.end()
def test_command_full(robot, drivetrain_default, hal_data, duration, timeout, speed, left_ex_speed, right_ex_speed):
robot.drivetrain = drivetrain_default
dt = TurnTime(robot, duration, speed, "CustomTurnTime", timeout)
sw = Stopwatch()
assert dt is not None
dt.initialize()
sw.start()
while not dt.isFinished():
dt.execute()
assert hal_data['pwm'][1]['value'] == left_ex_speed
assert hal_data['pwm'][2]['value'] == right_ex_speed
dt.end()
sw.stop()
if duration < timeout:
assert isclose(sw.elapsed_time_in_secs(), duration)
else:
# TODO: Timeouts don't seem to work in testing?
assert isclose(sw.elapsed_time_in_secs(), timeout)
class TurnTime(Command):
_stopwatch = None
_start_time = None
_duration = None
_speed = None
def __init__(self, robot, duration, speed, name=None, timeout=15):
"""Constructor"""
super().__init__(name, timeout)
self.robot = robot
self.requires(robot.drivetrain)
self._stopwatch = Stopwatch()
self._duration = duration
self._speed = speed
def initialize(self):
"""Called before the Command is run for the first time."""
# Start stopwatch
self._stopwatch.start()
return Command.initialize(self)
def execute(self):
"""Called repeatedly when this Command is scheduled to run"""
speed = self._speed
self.robot.drivetrain.arcade_drive(0.0, speed, False)
return Command.execute(self)
def isFinished(self):
"""Returns true when the Command no longer needs to be run"""
# If elapsed time is more than duration
return self._stopwatch.elapsed_time_in_secs(
) >= self._duration or self.isTimedOut()
def end(self):
"""Called once after isFinished returns true"""
self._stopwatch.stop()
# Stop driving
self.robot.drivetrain.arcade_drive(0.0, 0.0)
def interrupted(self):
"""Called when another command which requires one or more of the same subsystems is scheduled to run"""
self.end()
