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