def timePassed(self, seconds):
if self._timer <= 0:
self._timer = Timer.getFPGATimestamp()
if Timer.getFPGATimestamp() - self._timer >= seconds:
self._timer = -1
return True
else:
return False
def get(self, input):
if input == self.last_input:
time = Timer.getFPGATimestamp()
if time - self.last_timestamp <= self.debounce_period:
return -1
else:
self.last_timestamp = time
return input
else:
self.last_input = input
self.last_timestamp = Timer.getFPGATimestamp()
return input
def get(self, input):
if input == self.last_input:
time = Timer.getFPGATimestamp()
if time - self.last_timestamp <= self.debounce_period:
return -1
else:
self.last_timestamp = time
return input
else:
self.last_input = input
self.last_timestamp = Timer.getFPGATimestamp()
return input
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **"
)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **"
)
if self.source == 'dashboard':
self.setpoint = SmartDashboard.getNumber('z_angle', 1)
# may want to break if no valid setpoint is passed
self.start_angle = self.robot.drivetrain.navx.getAngle()
if self.absolute: # trust the navx to be correctly oriented to the field
self.setpoint = self.setpoint - self.start_angle
if self.setpoint > 180:
self.setpoint = -(360 - self.setpoint)
if self.setpoint < -180:
self.setpoint = (360 + self.setpoint)
self.error = 0
self.prev_error = 0
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp(), 1)
print("\n" + f"** Started {self.getName()} at {self.start_time} s **",
flush=True)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} at {self.start_time - self.robot.enabled_time:2.2f} s **"
)
# note I made this a bit more complicated to demonstrate the flexibility of the button approach
# two button approach
if self.command == 'stop':
self.robot.shooter.stop_flywheel()
self.shooter_enabled = False
elif self.command == 'spin':
self.robot.shooter.set_flywheel(velocity=self.velocity)
self.shooter_enabled = True
else:
# toggle button approach
if self.shooter_enabled:
self.robot.shooter.stop_flywheel()
self.shooter_enabled = False
else:
self.robot.shooter.set_flywheel(velocity=self.velocity)
self.shooter_enabled = True
def update_characterization(self):
now = Timer.getFPGATimestamp()
left_position = self.drivetrain.get_position(self.drivetrain.l_encoder)
left_rate = self.drivetrain.get_rate(self.drivetrain.l_encoder)
right_position = self.drivetrain.get_position(
self.drivetrain.r_encoder)
right_rate = self.drivetrain.get_rate(self.drivetrain.r_encoder)
if self.isReal():
battery = 2 * wpilib.RobotController.getVoltage6V(
) # no battery voltage in python controller?
else:
battery = 11 #ToDo - get a real vs simulated value for this
motor_volts = battery * math.fabs(self.prior_autospeed)
left_motor_volts = motor_volts
right_motor_volts = motor_volts
autospeed = self.autoSpeedEntry.getDouble(0)
self.prior_autospeed = autospeed
factor = -1.0 if self.rotateEntry.getBoolean(False) else 1.0
self.drivetrain.drive.tankDrive(factor * autospeed, autospeed, False)
self.drivetrain.drive.feed()
vals = [
now, battery, autospeed, left_motor_volts, right_motor_volts,
left_position, right_position, left_rate, right_rate,
self.drivetrain.navx.getRotation2d().radians()
]
for i in vals:
self.entries.append(i)
self.counter += 1
def searching(self) -> None:
"""
The vision system does not have a target, we try to find one using odometry
"""
if self.is_manual_aiming:
self.next_state_now("manual_aiming")
if self.target_estimator.is_ready():
# print(f"searching -> tracking {self.vision.get_vision_data()}")
self.time_target_lost = None
self.next_state("tracking")
else:
# Scan starting straight downrange.
time_now = Timer.getFPGATimestamp()
# Start a scan only if it's been a minimum time since we lost the target
if (self.time_target_lost is None or
(time_now - self.time_target_lost) > self.TARGET_LOST_TO_SCAN):
# Start a scan only if it's been a minimum time since we started
# a scan. This allows the given scan to grow enough to find the
# target so that we don't just start the scan over every cycle.
if (self.time_of_last_scan is None
or (time_now - self.time_of_last_scan) >
self.MIN_SCAN_PERIOD):
self.turret.scan(-self.chassis.get_heading())
self.time_of_last_scan = time_now
def get_output(self, current_input: float, setpoint: float) -> float:
'''Get PID output for process
current_input:
The current PID input
setpoint:
Desired output of process/input to PID
'''
# Current time in seconds
current_time = Timer.getFPGATimestamp()
# Time elapsed since last update
time_change = current_time - self._previous_time
# The current error
current_error = self._get_continuous_error(setpoint - current_input)
self._integral_term += self._coefs.i * (current_error * time_change)
self._integral_term = clamp(self._integral_term, self._output_max,
self._output_min)
# Protect againsts ZeroDivisionError caused
# by time resolution in simulator
if time_change <= 0.0:
time_change = 0.005
derivative = (current_input - self._previous_input) / time_change
self._previous_input = current_input
self._previous_time = current_time
output = ((self._coefs.p * current_error) + self._integral_term +
(self._coefs.d * derivative))
return clamp(output, self._output_max, self._output_min)
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp()-self.robot.enabled_time, 1)
print("\n" + f"** Started {self.getName()} at {self.start_time} s **", flush=True)
SmartDashboard.putString("alert", f"** Started {self.getName()} at {self.start_time - self.robot.enabled_time:2.2f} s **")
self.robot.shooter.set_flywheel(self.velocity)
self.robot.shooter.set_hood_setpoint(self.angle)
def end(self):
"""Called once after isFinished returns true"""
end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Ended {self.name} at {end_time} s with a duration of {round(end_time-self.start_time,1)} s **")
SmartDashboard.putString("alert", f"** Ended {self.name} at {end_time} s with a duration of {round(end_time-self.start_time,1)} s **")
for key in self.telemetry:
SmartDashboard.putNumberArray("telemetry_" + str(key), self.telemetry[key])
self.robot.drivetrain.stop()
def begin(self):
self.start_time = Timer.getFPGATimestamp()
if self.mode == MacroMode.PLAYING:
self._load_current_macro()
self.next_state('_play_macro')
elif self.mode == MacroMode.RECORDING:
self._load_macro_recorder()
self.next_state('_record_macro')
def execute(self):
if not self.current_action:
if self.actions:
self.current_action = self.actions.pop(0)
self.has_reset = False
if self.current_action.timeout:
self.timeout_start = Timer.getFPGATimestamp()
else:
self.position_controller.stop()
self.angle_controller.stop()
self.path_controller.stop()
if self.current_action:
if self.current_action.rotate:
if not self.has_reset:
self.angle_controller.reset_angle()
self.has_reset = True
else:
self.angle_controller.align_to(self.current_action.rotate)
if self.angle_controller.is_aligned():
self.current_action = None
elif self.current_action.position:
if not self.has_reset:
self.position_controller.reset_position_and_heading()
self.has_reset = True
else:
self.position_controller.move_to(
self.current_action.position)
if self.position_controller.is_at_location():
self.current_action = None
elif self.current_action.path:
if not self.has_reset:
self.path_controller.set(self.current_action.path,
self.current_action.reverse)
self.has_reset = True
else:
self.path_controller.run()
if self.path_controller.is_finished():
self.current_action = None
if self.current_action and self.current_action.timeout:
if Timer.getFPGATimestamp() - self.timeout_start > \
self.current_action.timeout:
self.current_action = None
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Started {self.getName()} at {self.start_time} s **", flush=True)
self.prior_autospeed = 0
self.data = ''
networktables.NetworkTablesInstance.getDefault().setUpdateRate(0.010)
self.counter = 0
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp(), 1)
print("\n" + f"** Started {self.getName()} at {self.start_time} s **",
flush=True)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} at {self.start_time - self.robot.enabled_time:2.2f} s **"
)
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.name} with input {self.direction} at {self.start_time} s **",
flush=True)
self.robot.pneumatics.actuate_solenoid(self.direction)
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
if self.direction == "open":
self.robot.peripherals.open_gate()
elif self.direction == "close":
self.robot.peripherals.close_gate()
else:
print("Something happened that I didn't understand in GateServo")
def __init__(self, table: NetworkTable) -> None:
self.last_pong = Timer.getFPGATimestamp()
self.table = table
self.ping_time_entry = self.table.getEntry("ping")
self.rio_pong_time_entry = self.table.getEntry("rio_pong")
self.raspi_pong_time_entry = self.table.getEntry("raspi_pong")
self.latency_entry = self.table.getEntry("clock_offset")
self.ping()
def teleopInit(self):
"""This function is called at the beginning of operator control."""
# This makes sure that the autonomous stops running when
# teleop starts running. If you want the autonomous to
# continue until interrupted by another command, remove
# this line or comment it out.
self.reset()
self.enabled_time = Timer.getFPGATimestamp()
if self.autonomousCommand is not None:
self.autonomousCommand.cancel()
def clear_characterization(self):
elapsed_time = Timer.getFPGATimestamp() - self.enabled_time
self.drivetrain.tank_drive_volts(0, 0)
self.data = str(self.entries)
self.data = self.data[1:-2] + ', '
self.telemetryEntry.setString(self.data)
self.entries.clear()
print(
f'** collected {self.counter} data points in {elapsed_time} s **')
self.data = ''
def get_data(self) -> VisionData:
# Clients should check is_ready before calling, so this is just
# defensive in case they don't
if not self.is_ready():
return VisionData(0.0, 0.0, 0.0)
if abs(self.angle_to_target) < math.radians(5.0):
distance = self.range_finder.get_distance() - self.CAMERA_TO_LIDAR
else:
distance = self.vision_range
return VisionData(distance, self.angle_to_target,
Timer.getFPGATimestamp())
def end(self, message='Ended'):
"""Called once after isFinished returns true"""
end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
f"** {message} {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **"
)
SmartDashboard.putString(
"alert",
f"** Ended {self.getName()} at {end_time} s after {round(end_time-self.start_time,1)} s **"
)
self.robot.drivetrain.stop()
def _play_macro(self):
if self.current_macro_steps:
time, step = self.current_macro_steps[0]
if Timer.getFPGATimestamp() - self.start_time >= time:
self.macros.put_component_states(step)
self.current_macro_steps.pop(0)
self.prev_step = step
elif self.prev_step:
self.macros.put_component_states(self.prev_step)
else:
self.done()
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Started {self.name} at {self.start_time} s **")
self.telemetry = {
'time': [],
'position': [],
'velocity': [],
'current': []
}
self.counter = 0
def initialize(self):
"""Called just before this Command runs the first time."""
# perhaps here we need to set the contol mode for the controllers ... not sure about this
self.is_twist_active = False
self.corrected_twist = 0
self.execution_count = 0
self.previous_twist_error = 0
self.gyro_error = 0
self.heading = self.robot.navigation.get_angle()
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Started {self.name} at {self.start_time} s **",
flush=True)
def end(self, message='Ended'):
"""Called once after isFinished returns true"""
self.robot.drivetrain.drive.arcadeDrive(0,0)
#print(f"** {message} {self.getName()} at {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)} s **", flush=True)
elapsed_time = Timer.getFPGATimestamp() - self.start_time - self.robot.enabled_time
self.robot.drivetrain.tank_drive_volts(0,0)
self.data = str(self.entries)
self.data = self.data[1:-2] + ', '
self.telemetryEntry.setString(self.data)
self.entries.clear()
print(f'** {self.getName()} collected {self.counter} data points in {elapsed_time} s **')
self.data = ''
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.name} with input {self.state} at {self.start_time} s **",
flush=True)
SmartDashboard.putString(
"alert",
f"** Started {self.name} with input {self.state} at {self.start_time} s **"
)
self.heading = self.robot.navigation.get_angle()
def autonomousInit(self):
"""Called when autonomous mode is enabled"""
self.enabled_time = Timer.getFPGATimestamp()
if self.characterize:
self.init_characterization()
else:
# self.autonomousCommand = FRCCharacterization(self, button=self.oi.buttonA, timeout=60)
if self.isReal():
self.autonomousCommand = AutonomousBounce(
self) # bounce paths - four of them
else:
self.autonomousCommand = AutonomousRamsete(
self) # single paths from the drop down
self.autonomousCommand.start()
def initialize(self):
"""Called just before this Command runs the first time."""
if self.from_dashboard:
self.setpoint = SmartDashboard.getNumber('Auto Rotation', 0)
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.name} with setpoint {self.setpoint} at {self.start_time} s **"
)
SmartDashboard.putString(
"alert",
f"** Started {self.name} with setpoint {self.setpoint} at {self.start_time} s **"
)
self.start_angle = self.robot.navigation.get_angle()
self.error = 0
self.prev_error = 0
def initialize(self):
"""Called just before this Command runs the first time."""
if self.from_dashboard:
self.setpoint = SmartDashboard.getNumber('Auto Distance',0)
self.start_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Started {self.name} with setpoint {self.setpoint} and control_type {self.control_type} at {self.start_time} s **")
SmartDashboard.putString("alert", f"** Started {self.name} with setpoint {self.setpoint} and control_type {self.control_type} at {self.start_time} s **")
if self.control_type == 'position':
self.robot.drivetrain.goToSetPoint(self.setpoint)
elif self.control_type == 'velocity':
self.robot.drivetrain.set_velocity(self.setpoint)
else:
pass
self.has_arrived = False
self.telemetry = {'time':[], 'position':[], 'velocity':[], 'current':[], 'output':[]}
self.counter = 0
self.extra_time = 0.5 # amount of time to give it to slow down after we reach setpoint
self.setTimeout(self.initial_timeout) # needs to be in initialize because we modify it in execute - dashboard instance keeps getting reused
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print("\n" + f"** Started {self.getName()} at {self.start_time} s **")
SmartDashboard.putString("alert", f"** Started {self.getName()} at {self.start_time} s **")
if self.use_dash:
self.kp = SmartDashboard.getNumber('kp_vel', self.kp)
self.kd = SmartDashboard.getNumber('kd_vel', self.kd)
self.left_speed_setpoint = SmartDashboard.getNumber('l_vel', self.left_speed_setpoint)
self.right_speed_setpoint = SmartDashboard.getNumber('r_vel', self.right_speed_setpoint)
self.left_controller = wpilib.controller.PIDController(self.kp, 0 , self.kd, period=0.02)
self.right_controller = wpilib.controller.PIDController(self.kp, 0 , self.kd, period=0.02)
self.previous_time = -1
self.left_controller.reset()
self.right_controller.reset()
def initialize(self):
"""Called just before this Command runs the first time."""
self.start_time = round(Timer.getFPGATimestamp(), 1)
print(
"\n" +
f"** Started {self.getName()} with enabling as {not self.shooter_enabled} at {self.start_time} s **",
flush=True)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} at {self.start_time - self.robot.enabled_time:2.2f} s **"
)
# toggle button approach
if self.shooter_enabled:
self.robot.shooter.stop_flywheel()
self.shooter_enabled = False
else:
self.robot.shooter.set_flywheel(velocity=self.omega)
self.shooter_enabled = True
def __init__(self):
self.debounce_period = 0.5
self.last_input = -1
self.last_timestamp = Timer.getFPGATimestamp()
def time(self):
return Timer.getFPGATimestamp()
