class SetDistanceToBox(Command):
"""
Drive until the robot is the given distance away from the box. Uses a local
PID controller to run a simple PID loop that is only enabled while this
command is running. The input is the averaged values of the left and right
encoders.
"""
def __init__(self, robot, distance):
super().__init__()
self.robot = robot
self.requires(self.robot.drivetrain)
self.pid = PIDController(
-2,
0,
0,
)
self.pid.setSetpoint(distance)
def initialize(self):
"""Called just before this Command runs the first time"""
# Get everything in a safe starting state.
self.robot.drivetrain.reset()
self.pid.reset()
self.pid.enable()
def execute(self):
"""Called repeatedly when this Command is scheduled to run"""
def isFinished(self):
"""Make this return true when this Command no longer needs to run execute()"""
return self.pid.onTarget()
def end(self):
"""Called once after isFinished returns true"""
# Stop PID and the wheels
self.pid.disable()
self.robot.drivetrain.driveManual(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 Drive:
"""
Handle robot drivetrain.
All drive interaction must go through this class.
"""
limelight: Limelight
train: wpilib.drive.DifferentialDrive
navx: navx.AHRS
left_motors: wpilib.SpeedControllerGroup
right_motors: wpilib.SpeedControllerGroup
y = will_reset_to(0.0)
rot = will_reset_to(0.0)
aligning = will_reset_to(False)
deadband = will_reset_to(0.1)
auto = will_reset_to(False)
left_voltage = will_reset_to(0)
right_voltage = will_reset_to(0)
speed_constant = will_reset_to(1.05)
rotational_constant = will_reset_to(0.8)
squared_inputs = False
angle_p = ntproperty('/align/kp', 0.04)
angle_i = ntproperty('/align/ki', 0)
angle_d = ntproperty('/align/kd', 0.0)
angle_reported = ntproperty('/align/angle', 0)
angle_to = ntproperty('/align/angle_to', 0)
def setup(self):
"""
Run setup code on the injected variables (train)
"""
self.angle_controller = PIDController(self.angle_p, self.angle_i,
self.angle_d)
self.angle_controller.setTolerance(2, 1)
self.angle_controller.enableContinuousInput(0, 360)
self.angle_setpoint = None
self.calculated_pid = False
def set_target(self, angle: float, relative=True):
if relative and angle is not None:
self.angle_setpoint = (self.angle + angle) % 360
self.angle_to = self.angle_setpoint
else:
self.angle_setpoint = angle
if angle is not None:
self.angle_controller.setSetpoint(self.angle_setpoint)
else:
self.calculated_pid = False
self.angle_controller.reset()
def align(self):
self.aligning = True
self.deadband = 0
def voltageDrive(self, left_voltage, right_voltage):
self.left_voltage = left_voltage
self.right_voltage = right_voltage
self.auto = True
self.deadband = 0
def move(self, y: float, rot: float):
"""
Move robot.
:param y: Speed of motion in the y direction. [-1..1]
:param rot: Speed of rotation. [-1..1]
"""
self.y = y
self.rot = rot
@property
def target_locked(self):
# self.logger.info(f'SET {self.angle_controller.getSetpoint()} ANG {self.navx.getAngle()}')
return self.angle_controller.atSetpoint() and self.calculated_pid
@property
def angle(self):
raw = self.navx.getAngle()
while raw < 0:
raw += 360
return raw % 360
def execute(self):
"""
Handle driving.
"""
self.train.setDeadband(self.deadband)
self.angle_reported = self.angle
if self.aligning and self.angle_setpoint is not None:
self.right_motors.setInverted(False)
if self.angle_controller.atSetpoint() and self.calculated_pid:
self.train.arcadeDrive(0, 0, squareInputs=False)
return
# Use new network tables variables for testing
self.angle_controller.setP(self.angle_p)
self.angle_controller.setD(self.angle_d)
self.angle_controller.setI(self.angle_i)
output = self.angle_controller.calculate(self.angle)
self.logger.info(
f'Output: {output}, Error: {self.angle_controller.getPositionError()}'
)
# Manual I-term zone (15 degrees)
if abs(self.angle_controller.getPositionError()) <= 3:
self.angle_controller.setI(self.angle_i)
# Minumum and Maximum effect of integrator on output
self.angle_controller.setIntegratorRange(-0.05, 0.05)
else:
self.angle_controller.setI(0)
self.angle_controller.setIntegratorRange(0, 0)
if output < 0:
output = max(output, -0.35)
else:
output = min(output, 0.35)
self.train.arcadeDrive(0, output, squareInputs=False)
self.calculated_pid = True
elif self.auto:
self.right_motors.setInverted(True)
self.right_motors.setVoltage(self.right_voltage)
self.left_motors.setVoltage(self.left_voltage)
else:
self.right_motors.setInverted(False)
self.train.arcadeDrive(
self.speed_constant * self.y,
self.rotational_constant * self.rot,
squareInputs=self.squared_inputs,
)
if not self.limelight.targetExists():
self.limelight.target_state = 0
elif self.target_locked:
self.limelight.target_state = 2
else:
self.limelight.target_state = 1
class SwerveModule:
# Get the motors, encoder and config from injection
driveMotor: ctre.WPI_VictorSPX
rotateMotor: ctre.WPI_VictorSPX
encoder: wpilib.AnalogInput
cfg: ModuleConfig
def setup(self):
"""
Called after injection
"""
# Config
self.sd_prefix = self.cfg.sd_prefix or 'Module'
self.encoder_zero = self.cfg.zero or 0
self.inverted = self.cfg.inverted or False
self.allow_reverse = self.cfg.allow_reverse or True
# SmartDashboard
self.sd = NetworkTables.getTable('SmartDashboard')
self.debugging = self.sd.getEntry('drive/drive/debugging')
# Motor
self.driveMotor.setInverted(self.inverted)
self._requested_voltage = 0
self._requested_speed = 0
# PID Controller
# kP = 1.5, kI = 0.0, kD = 0.0
self._pid_controller = PIDController(1.5, 0.0, 0.0)
self._pid_controller.enableContinuousInput(
0.0, 5.0) # Will set the 0 and 5 as the same point
self._pid_controller.setTolerance(
0.05, 0.05) # Tolerance where the PID will be accpeted aligned
def get_voltage(self):
"""
:returns: the voltage position after the zero
"""
return self.encoder.getAverageVoltage() - self.encoder_zero
def flush(self):
"""
Flush the modules requested speed and voltage.
Resets the PID controller.
"""
self._requested_voltage = self.encoder_zero
self._requested_speed = 0
self._pid_controller.reset()
@staticmethod
def voltage_to_degrees(voltage):
"""
Convert a given voltage value to degrees.
:param voltage: a voltage value between 0 and 5
:returns: the degree value between 0 and 359
"""
deg = (voltage / 5) * 360
if deg < 0:
deg += 360
return deg
@staticmethod
def voltage_to_rad(voltage):
"""
Convert a given voltage value to rad.
:param voltage: a voltage value between 0 and 5
:returns: the radian value betwen 0 and 2pi
"""
return (voltage / 5) * 2 * math.pi
@staticmethod
def degree_to_voltage(degree):
"""
Convert a given degree to voltage.
:param degree: a degree value between 0 and 360
:returns" the voltage value between 0 and 5
"""
return (degree / 360) * 5
def _set_deg(self, value):
"""
Round the value to within 360. Set the requested rotate position (requested voltage).
Intended to be used only by the move function.
"""
self._requested_voltage = (
(self.degree_to_voltage(value) + self.encoder_zero) % 5)
def move(self, speed, deg):
"""
Set the requested speed and rotation of passed.
:param speed: requested speed of wheel from -1 to 1
:param deg: requested angle of wheel from 0 to 359 (Will wrap if over or under)
"""
deg %= 360 # Prevent values past 360
if self.allow_reverse:
"""
If the difference between the requested degree and the current degree is
more than 90 degrees, don't turn the wheel 180 degrees. Instead reverse the speed.
"""
if abs(deg - self.voltage_to_degrees(self.get_voltage())) > 90:
speed *= -1
deg += 180
deg %= 360
self._requested_speed = speed
self._set_deg(deg)
def debug(self):
"""
Print debugging information about the module to the log.
"""
print(self.sd_prefix, '; requested_speed: ', self._requested_speed,
' requested_voltage: ', self._requested_voltage)
def execute(self):
"""
Use the PID controller to get closer to the requested position.
Set the speed requested of the drive motor.
Called every robot iteration/loop.
"""
# Calculate the error using the current voltage and the requested voltage.
# DO NOT use the #self.get_voltage function here. It has to be the raw voltage.
error = self._pid_controller.calculate(self.encoder.getVoltage(),
self._requested_voltage)
# Set the output 0 as the default value
output = 0
# If the error is not tolerable, set the output to the error.
# Else, the output will stay at zero.
if not self._pid_controller.atSetpoint():
# Use max-min to clamped the output between -1 and 1.
output = max(min(error, 1), -1)
# Put the output to the dashboard
self.sd.putNumber('drive/%s/output' % self.sd_prefix, output)
# Set the output as the rotateMotor's voltage
self.rotateMotor.set(output)
# Set the requested speed as the driveMotor's voltage
self.driveMotor.set(self._requested_speed)
self.update_smartdash()
def update_smartdash(self):
"""
Output a bunch on internal variables for debugging purposes.
"""
self.sd.putNumber('drive/%s/degrees' % self.sd_prefix,
self.voltage_to_degrees(self.get_voltage()))
if self.debugging.getBoolean(False):
self.sd.putNumber('drive/%s/requested_voltage' % self.sd_prefix,
self._requested_voltage)
self.sd.putNumber('drive/%s/requested_speed' % self.sd_prefix,
self._requested_speed)
self.sd.putNumber('drive/%s/raw voltage' % self.sd_prefix,
self.encoder.getVoltage()
) # DO NOT USE self.get_voltage() here
self.sd.putNumber('drive/%s/average voltage' % self.sd_prefix,
self.encoder.getAverageVoltage())
self.sd.putNumber('drive/%s/encoder_zero' % self.sd_prefix,
self.encoder_zero)
self.sd.putNumber('drive/%s/PID Setpoint' % self.sd_prefix,
self._pid_controller.getSetpoint())
self.sd.putNumber('drive/%s/PID Error' % self.sd_prefix,
self._pid_controller.getPositionError())
self.sd.putBoolean('drive/%s/PID isAligned' % self.sd_prefix,
self._pid_controller.atSetpoint())
self.sd.putBoolean('drive/%s/allow_reverse' % self.sd_prefix,
self.allow_reverse)
class Follower:
kinematics: DifferentialDriveKinematics
drive_feedforward: SimpleMotorFeedforwardMeters
trajectories: dict
odometry: Odometry
drive: Drive
use_pid = tunable(True)
trajectory = will_reset_to(None)
sample_time = will_reset_to(0.0)
def setup(self):
self.trajectory_name = None
self.left_controller = PIDController(0.07, 0, 0)
self.right_controller = PIDController(0.07, 0, 0)
def on_disable(self):
self.trajectory_name = None
self.left_controller.reset()
self.right_controller.reset()
def setup_trajectory(self, trajectory: Trajectory):
self.controller = RamseteController()
self.left_controller.reset()
self.right_controller.reset()
self.prev_time = 0
self.speed = DifferentialDriveWheelSpeeds()
self.controller.setTolerance(
Pose2d(0.05, 0.05, Rotation2d(math.radians(5))))
self.initial_state = self.trajectory.sample(0)
speeds = ChassisSpeeds()
speeds.vx = self.initial_state.velocity
speeds.omega = self.initial_state.velocity * self.initial_state.curvature
self.prevSpeeds = self.kinematics.toWheelSpeeds(speeds)
def follow_trajectory(self, trajectory_name: str, sample_time: float):
try:
self.trajectory = self.trajectories[trajectory_name]
except KeyError:
self.logger.error(
f'The trajectory with name "{trajectory_name}" could not be found.'
)
return
self.sample_time = sample_time
# Check if trajectory was restarted or if a new one has begun
if sample_time <= self.prev_time or trajectory_name != self.trajectory_name:
self.setup_trajectory(self.trajectory)
self.trajectory_name = trajectory_name
def is_finished(self, trajectory_name: str):
if self.trajectory_name != trajectory_name or self.trajectory is None:
self.logger.warning(
'Method "is_finished" called with no trajectory running.')
return True
return self.prev_time >= self.trajectory.totalTime() + 0.04
def execute(self):
if self.trajectory is None:
return
if self.use_pid:
self.dt = self.sample_time - self.prev_time
if self.dt == 0:
self.dt = 0.02
targetWheelSpeeds = self.kinematics.toWheelSpeeds(
self.controller.calculate(
self.odometry.get_pose(),
self.trajectory.sample(self.sample_time)))
leftSpeedSetpoint = targetWheelSpeeds.left
rightSpeedSetpoint = targetWheelSpeeds.right
leftFeedforward = self.drive_feedforward.calculate(
leftSpeedSetpoint,
(leftSpeedSetpoint - self.prevSpeeds.left) / self.dt)
rightFeedforward = self.drive_feedforward.calculate(
rightSpeedSetpoint,
(rightSpeedSetpoint - self.prevSpeeds.right) / self.dt)
leftOutput = leftFeedforward + self.left_controller.calculate(
self.odometry.left_rate, leftSpeedSetpoint)
rightOutput = rightFeedforward + self.right_controller.calculate(
self.odometry.right_rate, rightSpeedSetpoint)
else:
leftOutput = leftSpeedSetpoint
rightOutput = rightSpeedSetpoint
self.drive.voltageDrive(leftOutput, rightOutput)
self.prev_time = self.sample_time
self.prevSpeeds = targetWheelSpeeds