Code Structure
(we know a thing or two because we’ve seen a thing or two)
Organized code can make the difference in your build season
Robot Simulation
WPILib State Space Sims
private final DCMotorSim sim =
new DCMotorSim(
LinearSystemId.createDCMotorSystem(FF.kV, FF.kA),
DCMotor.getNEO(1),
GEARING.in(Radians.per(Radians)));
private final ElevatorSim sim = new ElevatorSim(
DCMotor.getNEO(2), // Gearbox of two NEO motors
gearRatio,
mass,
spoolRadius,
minHeight,
maxHeight,
true, // simulate gravity?
VecBuilder.fill(0.005));
sim.setInputVoltage(volts);
sim.update(Constants.PERIOD);
How do we use this????
REV “Simulation”
public class EncoderSim {
private final SimDouble position, velocity, appliedOutput;
public EncoderSim(int deviceID) {
var device = new SimDeviceSim("SPARK MAX [" + deviceID + "]");
position = device.getDouble("Position");
velocity = device.getDouble("Velocity");
appliedOutput = device.getDouble("Applied Output");
}
public double getAppliedOutput() {
return appliedOutput.get();
}
}
Existing Approaches: 6328 (🐐) and AdvantageKit
Pros: effective simulation, match replay
Cons: very heavy dependence on 3rd party library
Motivating the Switch: Swerve
Hardware Abstraction
We have RealElbow and RealWrist implementations for each part of our arm
Subsystem
Arm.java
IO Interface
JointIO.java
IO Implementation
RealJoint.java
IO Implementation
SimJoint.java
IO Implementation
NoJoint.java
Example: Basic Elevator
/** Generalized elevator with closed loop control */
public interface ElevatorIO extends AutoCloseable {
/** Returns the height of the elevator in meters */
public double getHeight();
/** Returns the current state (m, m / second) of the elevator */
public State getCurrentState();
/** Returns the target state (m, m / second) of the elevator */
public State getDesiredState();
/**
* Sets the setpoint to the specified state (m, m / second) of the elevator
*/
public void updateSetpoint(State setpoint);
}
/** Class to encapsulate an elevator on a real robot */
public class RealElevator implements ElevatorIO {
private final CANSparkMax motor;
private final Encoder encoder;
private final ElevatorFeedforward ff;
private final PIDController pid;
private State lastSetpoint;
private double lastVoltage;
public RealElevator() {
motor = new CANSparkMax(PORT, MotorType.kBrushless);
encoder = new Encoder(ENCODER[0], ENCODER[1]);
encoder.setDistancePerPulse(CONVERSION);
ff = new ElevatorFeedforward(kS, kG, kV, kA);
pid = new PIDController(kP, kI, kD);
}
@Override
public double getHeight() { return encoder.getDistance(); }
@Override
public State getCurrentState() { return new State(getHeight(), encoder.getRate()); }
@Override
public State getDesiredState() { return lastSetpoint; }
@Override
public void updateSetpoint(State setpoint) {
double ffOutput = ff.calculate(setpoint.position, setpoint.velocity, Constants.PERIOD);
double fbOutput = pid.calculate(getHeight(), setpoint.position);
lastVoltage = ffOutput + fbOutput;
motor.setVoltage(lastVoltage);
lastSetpoint = setpoint;
}
Example: Basic Elevator
/** Generalized elevator with closed loop control */
public interface ElevatorIO extends AutoCloseable {
/** Returns the height of the elevator in meters */
public double getHeight();
/** Returns the current state (m, m / second) of the elevator */
public State getCurrentState();
/** Returns the target state (m, m / second) of the elevator */
public State getDesiredState();
/**
* Sets the setpoint to the specified state (m, m / second) of the elevator
*/
public void updateSetpoint(State setpoint);
}
public class SimElevator implements ElevatorIO {
private final ElevatorSim sim;
private final PIDController pid;
private final ElevatorFeedforward ff;
private State lastSetpoint = new State();
private double lastVoltage;
public SimElevator() {
sim = new ElevatorSim(
DCMotor.getNEO(3),
...,
VecBuilder.fill(0.005));
pid = new PIDController(0, 0, 0);
ff = new ElevatorFeedforward(0, 0, 0, 0);
}
@Override
public double getHeight() { return sim.getPositionMeters(); }
@Override
public State getCurrentState() { return new State(getHeight(), sim.getVelocityMetersPerSecond()); }
@Override
public State getDesiredState() { return lastSetpoint; }
@Override
public void updateSetpoint(State setpoint) {
double ffVolts = ff.calculate(lastSetpoint.velocity, setpoint.velocity, 0.02);
double pidVolts = pid.calculate(getHeight(), setpoint.position);
lastVoltage = ffVolts + pidVolts;
sim.setInputVoltage(MathUtil.clamp(lastVoltage, -12, 12));
sim.update(0.02);
lastSetpoint = setpoint;
}
Example: Swerve
/** Generalized SwerveModule with closed loop control */
public interface ModuleIO extends AutoCloseable {
/** Returns the current state of the module. */
public SwerveModuleState getState();
/** Returns the current position of the module. */
public SwerveModulePosition getPosition();
/** Sets the desired state for the module. */
public void setDesiredState(SwerveModuleState desiredState);
/** Returns the desired state for the module. */
public SwerveModuleState getDesiredState();
/** Zeroes all the drive encoders. */
public void resetEncoders();
}
/** Class to encapsulate a rev max swerve module */
public class MAXSwerveModule implements ModuleIO {
private final CANSparkMax driveMotor; // Regular Neo
private final CANSparkMax turnMotor; // Neo 550
private final RelativeEncoder driveEncoder;
private final AbsoluteEncoder turningEncoder;
private final SparkMaxPIDController driveFeedback;
private final SparkMaxPIDController turnFeedback;
private final SimpleMotorFeedforward driveFeedforward =
new SimpleMotorFeedforward(Driving.FF.S, Driving.FF.V, Driving.FF.A);
private final Rotation2d angularOffset;
private SwerveModuleState setpoint = new SwerveModuleState();
public MAXSwerveModule(int drivePort, int turnPort, Measure<Angle> angularOffset) { ... }
@Override
public SwerveModuleState getState() {
return new SwerveModuleState(
driveEncoder.getVelocity(),
Rotation2d.fromRadians(turningEncoder.getPosition()).minus(angularOffset));
}
@Override
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(
driveEncoder.getPosition(),
Rotation2d.fromRadians(turningEncoder.getPosition()).minus(angularOffset));
}
// This class goes on for a while...
}
Example: Swerve
/** Generalized SwerveModule with closed loop control */
public interface ModuleIO extends AutoCloseable {
/** Returns the current state of the module. */
public SwerveModuleState getState();
/** Returns the current position of the module. */
public SwerveModulePosition getPosition();
/** Sets the desired state for the module. */
public void setDesiredState(SwerveModuleState desiredState);
/** Returns the desired state for the module. */
public SwerveModuleState getDesiredState();
/** Zeroes all the drive encoders. */
public void resetEncoders();
}
/** Class to encapsulate a simulated rev max swerve module */
public class SimModule implements ModuleIO {
private final DCMotorSim drive =
new DCMotorSim(
LinearSystemId.createDCMotorSystem(Driving.FF.V, Driving.FF.A),
DCMotor.getNEO(1),
Driving.GEARING.in(Radians.per(Radians)));
private final DCMotorSim turn =
new DCMotorSim(
LinearSystemId.createDCMotorSystem(Turning.FF.V, Turning.FF.A),
DCMotor.getNeo550(1),
Turning.MOTOR_GEARING.in(Radians.per(Radians)));
private final PIDController driveFeedback =
new PIDController(Driving.PID.P * 115, Driving.PID.I, Driving.PID.D * 115.0 / 1000);
private final PIDController turnFeedback =
new PIDController(Turning.PID.P * 2, Turning.PID.I, Turning.PID.D * 2.0 / 1000);
private final SimpleMotorFeedforward driveFeedforward =
new SimpleMotorFeedforward(Driving.FF.S, Driving.FF.V, Driving.FF.A);
private SwerveModuleState setpoint = new SwerveModuleState();
public SimModule() {
turnFeedback.enableContinuousInput(0, Turning.CONVERSION.in(Radians.per(Radians)));
}
@Override
public SwerveModuleState getState() {
return new SwerveModuleState(
drive.getAngularVelocityRadPerSec(),
Rotation2d.fromRadians(turn.getAngularVelocityRadPerSec()));
}
// This class also goes on for a while
}
Example: Swerve
/** Generalized SwerveModule with closed loop control */
public interface ModuleIO extends AutoCloseable {
/** Returns the current state of the module. */
public SwerveModuleState getState();
/** Returns the current position of the module. */
public SwerveModulePosition getPosition();
/** Sets the desired state for the module. */
public void setDesiredState(SwerveModuleState desiredState);
/** Returns the desired state for the module. */
public SwerveModuleState getDesiredState();
/** Zeroes all the drive encoders. */
public void resetEncoders();
}
/** Ideal swerve module, useful for debugging */
public class IdealModule implements ModuleIO {
private SwerveModuleState state = new SwerveModuleState();
private double distance;
@Override
public SwerveModuleState getState() {
return state;
}
@Override
public SwerveModulePosition getPosition() {
return new SwerveModulePosition(distance, state.angle);
}
@Override
public void setDesiredState(SwerveModuleState desiredState) {
state = SwerveModuleState.optimize(desiredState, state.angle);
distance += state.speedMetersPerSecond * Constants.PERIOD;
}
@Override
public SwerveModuleState getDesiredState() {
return state;
}
@Override
public void resetEncoders() {}
@Override
public void close() {}
}
Code Structure
Separating code by purpose, rather than category
Our 2023 code structure
We have… a lot of arm code
Category-Based Code Structure: Benefits
But wait, where is RobotContainer?
Do these barbarians write all their code in Robot.java?
CommandRobot
// Runs the selected auto for autonomous
autonomous().whileTrue(new ProxyCommand(autos::get));
// Sets arm setpoint to current position when re-enabled
teleop().onTrue(arm.setSetpoints(arm::getState));
// STATE SWITCHING
operator.b().onTrue(Commands.runOnce(() -> gamePiece = GamePiece.CONE));
operator.a().onTrue(Commands.runOnce(() -> gamePiece = GamePiece.CUBE));
// SCORING
operator.povUp().onTrue(arm.goTo(Goal.HIGH, () -> gamePiece));
operator.povRight().onTrue(arm.goTo(Goal.MID, () -> gamePiece));
operator.povDown().onTrue(arm.goTo(Goal.LOW, () -> gamePiece));
operator.povLeft().onTrue(arm.goTo(ArmState.stow()));
CommandRobot and 2024/2025 WPILIB Changes
/**
* A class containing static {@link Trigger} factories for running callbacks when the robot mode
* changes.
*/
public final class RobotModeTriggers {
// Utility class
private RobotModeTriggers() {}
public static Trigger autonomous() {
return new Trigger(DriverStation::isAutonomousEnabled);
}
public static Trigger teleop() {
return new Trigger(DriverStation::isTeleopEnabled);
}
public static Trigger disabled() {
return new Trigger(DriverStation::isDisabled);
}
public static Trigger test() {
return new Trigger(DriverStation::isTestEnabled);
}
}
One more thing
Unplugged encoders and other hardware faults are typically something you’d want to detect
Fallibles v2 (a WIP)
Fallibles v2 (a WIP)
/** Functional interface to represent a hardware or software component that can fail. */
@FunctionalInterface
public interface Fallible extends Sendable {
/** An individual fault, containing necessary information. */
public static record Fault(String description, FaultType type, double timestamp) {
public Fault(String description, FaultType type) {
this(description, type, Timer.getFPGATimestamp());
}
}
/**
* The type of fault, used for detecting whether the fallible is in a failure state and displaying
* to NetworkTables.
*/
public static enum FaultType {
INFO,
WARNING,
ERROR,
}
/**
* Returns a list of all current faults.
*
* @return A list of all current faults.
*/
public List<Fault> getFaults();
}
Fallibles v2 (a WIP)
(in MAXSwerveModule.java)
@Override
public List<Fault> getFaults() {
return Fallible.from(from(driveMotor), from(turnMotor));
}
/** Generalized SwerveModule with closed loop control */
public interface ModuleIO extends Fallible, AutoCloseable {
/** Returns the current state of the module. */
public SwerveModuleState getState();
/** Returns the current position of the module. */
public SwerveModulePosition getPosition();
/** Sets the desired state for the module. */
public void setDesiredState(SwerveModuleState desiredState);
/** Returns the desired state for the module. */
public SwerveModuleState getDesiredState();
/** Zeroes all the drive encoders. */
public void resetEncoders();
@Override
default void initSendable(SendableBuilder builder) {
Fallible.super.initSendable(builder);
builder.addDoubleProperty("current velocity", () -> getState().speedMetersPerSecond, null);
builder.addDoubleProperty("current angle", () -> getPosition().angle.getRadians(), null);
builder.addDoubleProperty("current position", () -> getPosition().distanceMeters, null);
builder.addDoubleProperty(
"target velocity", () -> getDesiredState().speedMetersPerSecond, null);
builder.addDoubleProperty("target angle", () -> getDesiredState().angle.getRadians(), null);
}
}
1155
We open source all our code on GitHub: https://github.com/SciBorgs
Find us on OpenAlliance: https://www.chiefdelphi.com/t/frc-1155-the-sciborgs-2024-build-thread-open-alliance/441531
Questions?