def iterate(self, robot_mode, pilot_stick, copilot_stick):
if robot_mode == RobotMode.TEST:
self.test_mode()
return
if pilot_stick.LeftBumper().get():
self.claw_close.set(False)
self.claw_open.set(True)
elif pilot_stick.RightBumper().get():
self.claw_close.set(True)
self.claw_open.set(False)
if pilot_stick.A().get():
self.shoot_ball()
#Do we need a boolean to look for pressed edge?
if pilot_stick.DpadUp().get():
self.stow_claw()
#Do we need a boolean to look for pressed edge?
if pilot_stick.DpadDown().get():
self.deploy_claw()
self.iterate_state_machine()
SmartDashboard.putString("Claw State", self.current_state.name)
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 iterate(self, robot_mode, pilot_stick, copilot_stick):
pilot_x = pilot_stick.LeftStickX()
pilot_y = pilot_stick.LeftStickY()
if abs(pilot_x) > 0 or abs(pilot_y) > 0:
self.current_state = DriveState.OPERATOR_CONTROL
self.drive.arcadeDrive(pilot_x, pilot_y, False)
else:
if self.current_state == DriveState.FOLLOW_PATH:
self.process_auto_path()
if pilot_stick.X().get():
self.drive_front_extend.set(True)
self.drive_front_retract.set(False)
self.drive_back_extend.set(False)
self.drive_back_retract.set(True)
elif pilot_stick.Y().get():
self.drive_front_extend.set(False)
self.drive_front_retract.set(True)
self.drive_back_extend.set(True)
self.drive_back_retract.set(False)
elif pilot_stick.B().get():
self.drive_front_extend.set(False)
self.drive_front_retract.set(True)
self.drive_back_extend.set(False)
self.drive_back_retract.set(True)
SmartDashboard.putString("Drive State", self.current_state.name)
def change_PIDs(self, factor=1, dict_0=None, dict_1=None):
"""Pass a value to the and update the PIDs, probably use 1.5 and 0.67 to see how they change
can also pass it a dictionary {'kP': 0.06, 'kI': 0.0, 'kD': 0, 'kIz': 0, 'kFF': 0} to set
slot 0 (position) or slot 1 (velocity) """
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
for key in keys:
if dict_0 == None:
self.PID_dict_pos[key] *= factor
else:
self.PID_dict_pos[key] = dict_0[key] / self.PID_multiplier
SmartDashboard.putString(
"alert",
f"Pos: kP: {self.PID_dict_pos['kP']} kI: {self.PID_dict_pos['kI']} kD: {self.PID_dict_pos['kD']} kIz: {self.PID_dict_pos['kIz']} kFF: {self.PID_dict_pos['kFF']}"
)
if dict_1 == None:
self.PID_dict_vel[key] *= factor
else:
self.PID_dict_vel[key] = dict_1[key] / self.PID_multiplier
SmartDashboard.putString(
"alert",
f"Vel: kP: {self.PID_dict_vel['kP']} kI: {self.PID_dict_vel['kI']} kD: {self.PID_dict_vel['kD']} kIz: {self.PID_dict_vel['kIz']} kFF: {self.PID_dict_vel['kFF']}"
)
self.configure_controllers(pid_only=True)
self.display_PIDs()
def periodic(self) -> None:
"""Perform odometry and update dash with telemetry"""
self.counter += 1
self.odometry.update(geo.Rotation2d.fromDegrees(-self.navx.getAngle()),
self.l_encoder.getPosition(),
-self.r_encoder.getPosition())
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position (only good for WCD)'
pose = self.get_pose()
SmartDashboard.putString(
'drive_pose',
f'[{pose.X():2.2f}, {pose.Y():2.2f}, {pose.rotation().degrees():2.2f}]'
)
SmartDashboard.putString(
'drive_encoders LR',
f'[{self.get_position(self.l_encoder):2.3f}, {self.get_position(self.r_encoder):2.2f}]'
)
SmartDashboard.putString('drive_heading',
f'[{-self.navx.getAngle():2.3f}]')
if self.counter % 100 == 0:
pass
# self.display_PIDs()
msg = f"Positions: ({self.l_encoder.getPosition():2.2f}, {self.r_encoder.getPosition():2.2}, {self.navx.getAngle():2.2})"
msg = msg + f" Rates: ({self.l_encoder.getVelocity():2.2f}, {self.r_encoder.getVelocity():2.2f}) Time: {Timer.getFPGATimestamp() - self.robot.enabled_time:2.1f}"
SmartDashboard.putString("alert", msg)
SmartDashboard.putString("sparks", str(self.error_dict))
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, message='Ended') -> None:
"""Called once after isFinished returns true"""
self.robot.drivetrain.stop()
print(
f"** {message} {self.getName()} at {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)} s **"
)
SmartDashboard.putString('/Vision/TestKey', 'ended')
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 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 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 autonomousInit(self):
#self.m_autoSelected = self.m_autoChooser.GetSelected()
self.m_autoSelected = KAUTONAME_CUSTOM
SmartDashboard.putString("Auto Selected", self.m_autoSelected)
print("Auto selected: " + self.m_autoSelected)
if (self.m_autoSelected == KAUTONAME_CUSTOM):
# Custom Auto goes here
print("Auto selected: " + "Custom Autonomous")
else:
# Default Auto goes here
print("Auto selected: " + "Default Autonomous")
def periodic(self) -> None:
"""Perform odometry and update dash with telemetry"""
self.counter += 1
self.odometry.update(geo.Rotation2d.fromDegrees(-self.navx.getAngle()), self.l_encoder.getDistance(), -self.r_encoder.getDistance())
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position (only good for WCD)'
pose = self.get_pose()
SmartDashboard.putString('drive_pose', f'[{pose.X():2.2f}, {pose.Y():2.2f}, {pose.rotation().degrees():2.2f}]' )
if self.counter % 100 == 0:
pass
def autonomousPeriodic(self):
self.m_autoSelected = KAUTONAME_CUSTOM
if (self.m_autoSelected == KAUTONAME_CUSTOM):
# Custom Auto goes here
print("Auto selected: " + "Custom Autonomous")
else:
# Default Auto goes here
print("Auto selected: " + "Default Autonomous")
SmartDashboard.putString("Auto Selected", self.m_autoSelected)
wpilib.SmartDashboard.putNumber("YawAngle", self.m_imu.getAngle())
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 iterate(self, robot_mode, pilot_stick, copilot_stick):
if robot_mode == RobotMode.TEST:
self.test_mode()
return
if pilot_stick.Back().get():
self.stow_harpoon()
if pilot_stick.Start().get():
self.deploy_harpoon()
self.iterate_state_machine()
SmartDashboard.putString("Harpoon State", self.current_state.name)
SmartDashboard.putNumber("IRVolts", self.ir_harpoon.getVoltage())
def iterate(self, robot_mode, simulation, pilot_stick, copilot_stick):
self.robot_mode = robot_mode
lift_position = self.lift_talon.getAnalogInRaw()
if lift_position > self.lift_stow_position + 5:
SmartDashboard.putBoolean("Creep", True)
else:
SmartDashboard.putBoolean("Creep", False)
if robot_mode == RobotMode.TEST:
if self.test_lift_pneumatic.getSelected() == 1:
self.lift_pneumatic_extend.set(False)
self.lift_pneumatic_retract.set(True)
else:
self.lift_pneumatic_extend.set(True)
self.lift_pneumatic_retract.set(False)
#need to check these separately so we don't disable the mechanism completely if we end up one tick outside our allowable range
if lift_position > self.min_lift_position or lift_position < self.max_lift_position:
self.lift_talon.set(
ControlMode.PercentOutput,
-1.0 * pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS))
elif lift_position < self.min_lift_position:
#allow upward motion
self.lift_talon.set(
ControlMode.PercentOutput,
max(
-1.0 *
pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0))
elif lift_position > self.max_lift_position:
#allow downward motion
self.lift_talon.set(
ControlMode.PercentOutput,
min(
-1.0 *
pilot_stick.getRawAxis(robotmap.XBOX_RIGHT_Y_AXIS), 0))
else:
self.lift_talon.set(ControlMode.PercentOutput, 0.0)
else:
self.iterate_state_machine(pilot_stick, copilot_stick)
SmartDashboard.putString("Lift State", self.current_state.name)
SmartDashboard.putString("Lift Preset", self.current_lift_preset.name)
SmartDashboard.putNumber("Lift Setpoint", self.lift_setpoint)
SmartDashboard.putNumber("Lift Position", lift_position)
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(), 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 dashboardPeriodic(self):
#self.wristUp = self.getNumber("WristUpSpeed" , 0.5)
#self.wristDown = self.getNumber("WristDownSpeed" , 0.2)
#self.wristUpVolts = self.getNumber("WristUpVoltage" , 5)
#self.wristDownVolts = self.getNumber("WristDownVoltage" , 2)
#self.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain)
#self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity)
SmartDashboard.putNumber("Wrist Position",
self.wrist.getQuadraturePosition())
SmartDashboard.putData("PID Controller", self.cargoController)
SmartDashboard.putNumber("Wrist Angle", self.getAngle())
SmartDashboard.putNumber("Wrist Power", self.input)
SmartDashboard.putNumber("Wrist Power2", self.input2)
SmartDashboard.putString("Last Cargo Command", self.lastCargoCommand)
SmartDashboard.putBoolean("Wrist PinState Quad A",
self.wrist.getPinStateQuadA())
SmartDashboard.putBoolean("Wrist PinState Quad B",
self.wrist.getPinStateQuadB())
self.F = SmartDashboard.getNumber("F Gain", 0)
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 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()
self.write_telemetry = SmartDashboard.getBoolean("ramsete_write", self.write_telemetry)
if self.write_telemetry:
location = Path.cwd() if self.robot.isSimulation() else Path('/home/lvuser/py/') # it's not called robot on the robot
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
file_name = timestamp + '_'+ self.course + f'_kpv_{self.kp_vel:2.1f}'.replace('.','p') + f'_vel_{str(self.velocity).replace(".","p")}' +'.pkl'
pickle_file = location / 'sim' / 'data' / file_name
with open(pickle_file.absolute(), 'wb') as fp:
out_dict = {'TIMESTAMP':timestamp,'DATA':self.telemetry, 'COURSE':self.course, 'VELOCITY':self.velocity,
'KP_VEL':self.kp_vel, 'KD_VEL':self.kd_vel, 'BETA':self.beta, 'ZETA':self.zeta}
pickle.dump(out_dict, fp)
print(f'*** Wrote ramsete command data to {file_name} ***')
else:
print(f'*** Skipping saving of telemetry to disk ***')
def log(self):
self.counter += 1
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position (only good for WCD)
try:
distance = 0.5 * (self.sparkneo_encoder_1.getPosition() -
self.sparkneo_encoder_3.getPosition())
except:
print(
f"Problem: encoder position returns {self.sparkneo_encoder_1.getPosition()}"
)
distance = 0
self.x = self.x + (distance - self.previous_distance) * math.sin(
math.radians(self.robot.navigation.get_angle()))
self.y = self.y + (distance - self.previous_distance) * math.cos(
math.radians(self.robot.navigation.get_angle()))
self.previous_distance = distance
# send values to the dash to make sure encoders are working well
#SmartDashboard.putNumber("Robot X", round(self.x, 2))
#SmartDashboard.putNumber("Robot Y", round(self.y, 2))
for ix, encoder in enumerate(self.encoders):
SmartDashboard.putNumber(
f"Position Enc{str(int(1+ix))}",
round(encoder.getPosition() - self.encoder_offsets[ix], 2))
SmartDashboard.putNumber(f"Velocity Enc{str(int(1+ix))}",
round(encoder.getVelocity(), 2))
SmartDashboard.putNumber(
"Current M1",
round(self.spark_neo_left_front.getOutputCurrent(), 2))
SmartDashboard.putNumber(
"Current M3",
round(self.spark_neo_right_front.getOutputCurrent(), 2))
SmartDashboard.putBoolean('AccLimit', self.is_limited)
if self.counter % 1000 == 0:
# self.display_PIDs()
SmartDashboard.putString(
"alert",
f"Position: ({round(self.x, 1)},{round(self.y, 1)}) Time: {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)}"
)
def initialize(self):
"""Called just before this Command runs the first time."""
# allow setpoint to be controlled by the dashboard
if self.source == 'dashboard':
setpoint = SmartDashboard.getNumber('z_distance', 1)
self.setpoint_sign = math.copysign(1, setpoint)
self.setpoint = (math.fabs(setpoint)) # correction for overshoot
# self.robot.drivetrain.reset_encoders() # does not work in sim mode
self.start_pos = self.robot.drivetrain.get_average_encoder_distance()
if self.k_dash:
self.kp, self.kd = SmartDashboard.getNumber(
'zdrive_kp',
self.kp), SmartDashboard.getNumber('zdrive_kd', self.kd)
self.ki, self.period = SmartDashboard.getNumber(
'zdrive_ki',
self.ki), SmartDashboard.getNumber('zdrive_per', self.kperiod)
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.getName()} with setpoint {self.setpoint} (kp={self.kp}, ki={self.ki}, kd={self.kd}) at {self.start_time} s **"
)
SmartDashboard.putString(
"alert",
f"** Started {self.getName()} with setpoint {self.setpoint} at {self.start_time} s **"
)
self.has_finished = False
self.controller = wpilib.controller.PIDController(self.kp,
self.ki,
self.kd,
period=self.kperiod)
self.controller.setSetpoint(self.setpoint)
self.controller.setTolerance(self.tolerance)
self.controller.reset()
def end(self, message='Ended'):
"""Called once after isFinished returns true"""
end_time = round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
elapsed_time = end_time - self.start_time + self.time_offset
print(
f"** {message} {self.getName()} at {end_time} s after {round(elapsed_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()
AutonomousRamseteSimple.time_offset += end_time - self.start_time # maintain running time in string of commands BUT don't reassign or it makes an instance
# ToDo: update this so it only happens at the end of a list of commands. May need a new variable.
self.write_telemetry = SmartDashboard.getBoolean(
"ramsete_write", self.write_telemetry)
if self.write_telemetry:
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
file_name = timestamp + '_' + self.course + f'_kpv_{self.kp_vel:2.1f}'.replace(
'.',
'p') + f'_vel_{str(self.velocity).replace(".","p")}' + '.pkl'
pickle_file = Path.cwd() / 'sim' / 'data' / file_name
with open(pickle_file.absolute(), 'wb') as fp:
out_dict = {
'TIMESTAMP': timestamp,
'DATA': self.telemetry,
'COURSE': self.course,
'VELOCITY': self.velocity,
'KP_VEL': self.kp_vel,
'KD_VEL': self.kd_vel,
'BETA': self.beta,
'ZETA': self.zeta
}
pickle.dump(out_dict, fp)
print(f'*** Wrote ramsete command data to {file_name} ***')
else:
print(
f'*** Skipping saving of {len(self.telemetry)} telemetry points to disk ***'
)
def robotInit(self):
self.timer = wpilib.Timer()
self.timer.start()
self.stick = XboxController(0)
SmartDashboard.putString("LEFT_Y_VALUES", "LEFT_Y_VALUES")
SmartDashboard.putNumber("left_y_rate", 0.6)
SmartDashboard.putNumber("left_y_expo", 1.0)
SmartDashboard.putNumber("left_y_deadband", 0.1)
SmartDashboard.putNumber("left_y_power", 1.5)
SmartDashboard.putNumber("left_y_min", -0.5)
SmartDashboard.putNumber("left_y_max", 0.5)
SmartDashboard.putString("LEFT_X_VALUES", "LEFT_X_VALUES")
SmartDashboard.putNumber("left_x_rate", 0.5)
SmartDashboard.putNumber("left_x_expo", 1.0)
SmartDashboard.putNumber("left_x_deadband", 0.1)
SmartDashboard.putNumber("left_x_power", 1.5)
SmartDashboard.putNumber("left_x_min", -0.5)
SmartDashboard.putNumber("left_x_max", 0.5)
SmartDashboard.putString("RIGHT_Y_VALUES", "RIGHT_Y_VALUES")
SmartDashboard.putNumber("right_y_rate", 1.0)
SmartDashboard.putNumber("right_y_expo", 0.0)
SmartDashboard.putNumber("right_y_deadband", 0.1)
SmartDashboard.putNumber("right_y_power", 1.0)
SmartDashboard.putNumber("right_y_min", -1.0)
SmartDashboard.putNumber("right_y_max", 1.0)
SmartDashboard.putString("RIGHT_X_VALUES", "RIGHT_X_VALUES")
SmartDashboard.putNumber("right_x_rate", 0.5)
SmartDashboard.putNumber("right_x_expo", 1.0)
SmartDashboard.putNumber("right_x_deadband", 0.1)
SmartDashboard.putNumber("right_x_power", 1.5)
SmartDashboard.putNumber("right_x_min", -0.5)
SmartDashboard.putNumber("right_x_max", 0.5)
def initialize(self):
self.robot.info("SpinIntakeWheelsCommand %s init" % self.direction)
self.setTimeout(10) # TODO: finialize timing
SmartDashboard.putString("Intakewheels spinning", self.direction)
def teleopPeriodic(self):
# push all tunable values
if self.driver.get_update_telemetry():
for t in self.tunables:
t.push()
self.drivetrain_component.push_pid_dash()
# update telemetry values
dash.putNumber("NavX Angle", self.navx_component.get_angle())
dash.putNumber("Drivetrain Position", self.drivetrain_component.get_position())
dash.putNumber("Current Left Velocity", self.encoders.get_velocity(EncoderSide.LEFT))
dash.putNumber("Current Right Velocity", self.encoders.get_velocity(EncoderSide.RIGHT))
dash.putNumber("Battery Voltage", self.robot_controller.getBatteryVoltage())
dash.putString("Current PID Mode", {
DriveMode.PID_DRIVE: "PID Drive",
DriveMode.PID_TURN: "PID Turn",
DriveMode.VELOCITY_CONTROL: "Velocity Control"
}[self.pid_mode])
# pull all pid values off the dashboard
if self.driver.get_update_pid_dash():
self.drivetrain_component.update_pid_dash()
# get the current arc for arc driving
current_arc = ArcDrive(
self.dash_arc_radius.get(),
self.dash_arc_distance.get(),
self.dash_v_max.get(),
RobotMap.Drivetrain.wheelbase
)
# handle whatever the current pid controller is if starting pid
if self.driver.get_enable_pid():
if self.pid_mode == DriveMode.PID_TURN:
self.drivetrain_component.turn_to_angle(self.dash_target_angle.get())
if self.pid_mode == DriveMode.PID_DRIVE:
self.drivetrain_component.drive_to_position(self.dash_target_position.get())
if self.pid_mode == DriveMode.VELOCITY_CONTROL:
self.drivetrain_component.velocity_control(self.dash_target_velocity_left.get(), self.dash_target_velocity_right.get())
# drive the robot via commands
if self.drivetrain_component.drive_mode == DriveMode.MANUAL_DRIVE:
driver_x, driver_y = self.driver.get_driver_input_curvature()
self.drivetrain_component.curvature_drive(driver_y, driver_x)
elif self.drivetrain_component.drive_mode == DriveMode.PID_TURN:
self.drivetrain_component.turn_to_angle(self.dash_target_angle.get())
elif self.drivetrain_component.drive_mode == DriveMode.PID_DRIVE:
self.drivetrain_component.drive_to_position(self.dash_target_position.get())
elif self.drivetrain_component == DriveMode.VELOCITY_CONTROL:
self.drivetrain_component.velocity_control(self.dash_target_velocity_left.get(), self.dash_target_velocity_right.get())
# switch to manual control if enabled
if self.driver.get_manual_control_override():
self.drivetrain_component.drive_mode = DriveMode.MANUAL_DRIVE
# rotate through pid control modes
if self.driver.get_toggle_pid_type_pressed():
self.pid_mode = {
DriveMode.PID_TURN: DriveMode.PID_DRIVE,
DriveMode.PID_DRIVE: DriveMode.VELOCITY_CONTROL,
DriveMode.VELOCITY_CONTROL: DriveMode.PID_TURN
}[self.pid_mode]
def initialize(self):
"""Called just before this Command runs the first time."""
self.previous_time = -1
self.init_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
self.robot.drivetrain.drive.feed(
) # this initialization is taking some time now
# update gains from dash if desired
if self.dash is True:
self.kp_vel = SmartDashboard.getNumber("ramsete_kpvel",
self.kp_vel)
self.beta = SmartDashboard.getNumber("ramsete_B", self.beta)
self.zeta = SmartDashboard.getNumber("ramsete_Z", self.zeta)
self.write_telemetry = SmartDashboard.getBoolean(
"ramsete_write", self.write_telemetry)
# create controllers
self.follower = wpilib.controller.RamseteController(
self.beta, self.zeta)
self.left_controller = wpilib.controller.PIDController(
self.kp_vel, 0, self.kd_vel)
self.right_controller = wpilib.controller.PIDController(
self.kp_vel, 0, self.kd_vel)
self.left_controller.reset()
self.right_controller.reset()
#ToDo - make this selectable, probably from the dash, add the other trajectories (done)
trajectory_choice = self.path # path is passed in on constructor this time
self.velocity = float(self.robot.oi.velocity_chooser.getSelected()
) # get the velocity from the GUI
self.trajectory = drive_constants.generate_trajectory(
trajectory_choice,
self.velocity,
simulation=self.robot.isSimulation(),
save=False)
self.course = trajectory_choice
# Note - we are setting to pose to have the robot physically in the start position - usually absolute matters
if self.relative: # figure out if we want to start where we are or where the trajectory says we should be
self.start_pose = geo.Pose2d(
self.trajectory.sample(0).pose.X(),
self.trajectory.sample(0).pose.Y(),
self.robot.drivetrain.get_rotation2d())
else:
#self.start_pose = self.robot.drivetrain.get_pose()
field_x = SmartDashboard.getNumber(
'field_x',
self.trajectory.sample(0).pose.X())
field_y = SmartDashboard.getNumber(
'field_y',
self.trajectory.sample(0).pose.Y())
self.start_pose = geo.Pose2d(
field_x, field_y, self.robot.drivetrain.get_rotation2d())
self.robot.drivetrain.reset_odometry(self.start_pose)
self.robot.drivetrain.drive.feed(
) # this initialization is taking some time now
initial_state = self.trajectory.sample(0)
# these are all meters in 2021
self.previous_speeds = self.kinematics.toWheelSpeeds(
wpimath.kinematics.ChassisSpeeds(
initial_state.velocity, 0,
initial_state.curvature * initial_state.velocity))
self.start_time = round(
Timer.getFPGATimestamp() - self.robot.enabled_time, 1)
print(
"\n" +
f"** Started {self.getName()} on {self.course} with load time {self.start_time-self.init_time:2.2f}s"
f" (b={self.beta}, z={self.zeta}, kp_vel={self.kp_vel}) at {self.start_time} s **"
)
SmartDashboard.putString(
"alert", f"** Started {self.getName()} at {self.start_time} s **")
if self.reset_telemetry: # don't reset telemetry if we want to chain trajectories together
print(
'Time\tTrVel\tTrRot\tlspd\trspd\tram an\tram vx\tram vy\tlffw\trffw\tlpid\trpid'
)
self.telemetry.clear(
) # Aha. important not to set it to a new value (=[]) , because then it makes an instance
AutonomousRamseteSimple.time_offset = 0 # seems like an easier way to do it, but man. Globals are always ugly.
else:
pass
def teleopPeriodic(self):
#print(self.limelight.get)
#print(self.color_wheel_motor.__dir__());
#print("execute method")
try:
# drive the drivetrain as needed
driver_x, driver_y = self.driver.get_curvature_output()
# manually handled driving
if self.drivetrain.state == DrivetrainState.MANUAL_DRIVE:
self.drivetrain.curvature_drive(driver_y, driver_x)
# set turbo mode
self.drivetrain.set_power_scaling(self.driver.process_turbo_mode())
# # check and see if we need to activate driver assists
# if self.drivetrain.ready_straight_assist() and self.driver.ready_straight_assist():
# self.drivetrain.drive_straight(driver_y)
# elif self.drivetrain.state == DrivetrainState.AIDED_DRIVE_STRAIGHT:
# self.drivetrain.state = DrivetrainState.MANUAL_DRIVE
# revert to manual control if enabled
if self.driver.get_manual_control_override():
self.drivetrain.state = DrivetrainState.MANUAL_DRIVE
except:
print("DRIVETRAIN ERROR")
try:
# move intake lift
if self.sysop.get_intake_lower():
self.intake_lift.send_down()
elif self.sysop.get_intake_raise():
self.intake_lift.send_up()
except:
print("INTAKE LIFT ERROR")
try:
#print(self.intake_roller_motor.getSelectedSensorPosition());
# intake rollers
if self.sysop.get_intake_intake():
self.intake_roller.intake()
elif self.sysop.get_intake_outtake():
self.intake_roller.outtake()
else:
self.intake_roller.stop()
except:
print("INTAKE ROLLER ERROR")
try:
if self.sysop.get_target_aim():
self.shooter.aim_and_fire()
elif self.sysop.get_shooter_stop():
self.shooter.stop_fire()
self.drivetrain.reset_state()
elif self.sysop.get_change_shooter_power() != 0:
self.shooter.adjust_power(
self.sysop.get_change_shooter_power())
except:
traceback.print_exc()
print("AUTO AIM ERROR")
try:
self.climber.set_power(self.sysop.get_climb_axis())
except:
print("CLIMBER ERROR")
try:
manual_fortune_input = self.sysop.get_manual_fortune_axis()
fms_color_position = self.ds.getGameSpecificMessage()
dash.putString("Color sensor color", self.color_sensor.get_color())
dash.putString("FMS Color", fms_color_position)
if manual_fortune_input != 0:
self.fortune_controller.manual_power(manual_fortune_input)
# elif self.sysop.get_position_control() and fms_color_position != "":
# self.fortune_controller.position_control({
# "B": WheelOfFortuneColor.BLUE,
# "R": WheelOfFortuneColor.GREEN,
# "G": WheelOfFortuneColor.RED,
# "Y": WheelOfFortuneColor.YELLOW,
# }[fms_color_position])
# TODO this is commented out because it'll be more effective to use manual control
# elif self.sysop.get_rotation_control():
# self.fortune_controller.rotation_control()
elif self.fortune_controller.state == ColorWheelState.MANUAL:
self.fortune_controller.manual_power(0)
except:
print("COLOR WHEEL ERROR")
