Programming: where it was, where it is, where it isn’t
By subtracting where it is, from where it isn’t, or where it isn’t, from where it is, whichever is greater, SciBorgs programming obtains a difference, or deviation.
I’m Asa, programming head along with @infinite_arity. We’ll be making programming specific posts pretty frequently throughout the season.
This post outlines developments from last year, goals for Crescendo, and initial planning.
2023
Over the past years, we’ve made significant progress toward becoming a top tier (I hope) programming department. Some developments include:
Code Structure
While we made initial progress on sim in 2022, it wasn’t until 2023 that we fully simulated everything from the start.
Very quickly, it became clear that REV’s support for the SimDeviceSim api was not going to cut it for our needs, since it didn’t support onboard closed loop control or duty cycle absolute encoder position. Additionally, we wanted a cleaner way to represent the sim/real split that did not rely on storing every class for sim and real hardware in all scenarios. Based on the code structure of AdvantageKit and swerve modules in template repositories, we decided to use a system of IO interfaces and classes.
For example, here’s what we did for swerve:
/** Generalized SwerveModule with closed loop control */
public interface ModuleIO extends Sendable, 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) {
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);
}
}
Additionally, our code this year has stuck very closely to what’s considered pragmatic in the modern command-based framework. Notably, we created a CommandRobot class for use instead of Robot and RobotContainer that provides triggers for game states (auto, teleop, etc). The triggers have found their way into WPILib 2024 thanks to @DeltaDizzy, and I’m currently working on a restructuring of the command-based templates to use CommandRobot for 2025.
// 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));
We were also able to organize our code like this, without separating by commands vs subsystems. This is enormously aesthetically pleasing to me.
Advanced Controls
In 2023, to control our abomination interestingly designed 3DoF robot, we looked into trajectory optimization with CasADi. However, we did not just have a double jointed arm. Instead, in our infinite wisdom, we made a double jointed arm on an elevator, the true pinnacle of KISS. This would prove to be a terrible, horrible, very educational idea. To derive an inverse dynamics model for this arm via the lagrange method, we referenced Underactuated and [the unofficial frc discord programming-discussion crew]. After getting our inverse dynamics model, we adapted and mildly refactored 6328’s Kairos to use it (and yaml config), and renamed the library to Chronos.
In the end, our robot loaded and ran pregenerated trajectories that were deployed off-field, with a system for falling back on trapezoid profiles. The entire system uses high quality (unbiased source: me) modern command-based code.
Examples
/**
* Goes to a {@link ArmState} in the most optimal way, this is a safe command.
*
* <p>Uses {@link #followTrajectory(ArmTrajectory)} based on {@link #findTrajectory(ArmState)} if
* a valid state is cached for the inputted parameters. Otherwise, falls back on {@link
* #safeFollowProfile(ArmState)} for on the fly movements.
*
* @param goal The goal state.
* @param gamePiece The selected game piece.
* @return A command that goes to the goal safely using either custom trajectory following or
* trapezoid profiling.
*/
public CommandBase goTo(Goal goal, Supplier<GamePiece> gamePiece) {
return goTo(() -> ArmState.fromGoal(goal, gamePiece.get()));
}
/**
* Goes to a {@link ArmState} in the most optimal way, this is a safe command.
*
* <p>Uses {@link #followTrajectory(ArmTrajectory)} based on {@link #findTrajectory(ArmState)} if
* a valid state is cached for the inputted parameters. Otherwise, falls back on {@link
* #safeFollowProfile(ArmState)} for on the fly movements.
*
* @param goal The goal state supplier.
* @return A command that goes to the goal safely using either custom trajectory following or
* trapezoid profiling.
*/
public CommandBase goTo(Supplier<ArmState> goal) {
return new DeferredCommand(
() -> {
var trajectory = findTrajectory(goal.get());
return trajectory
.map(this::followTrajectory)
.orElse(safeFollowProfile(goal))
.alongWith(
Commands.print(
String.format(
"Arm goTo Command:\nStart: %s\nGoal: %s\nFound trajectory: %b\n",
getSetpoint(), goal.get(), trajectory.isPresent())));
},
this);
}
/**
* A (mostly) safe version of {@link #followProfile(ArmState)} that uses {@link ArmState#side()}
* and {@link ArmState#end()} to reach the other side without height violations or destruction.
*
* <p>This is implemented by going to a safe intermediate goal if the side of the arm will change,
* which is slow, and mostly prevents circumstances where the arm hits the ground.
*
* @param goal The goal goal.
* @return A safe following command that will run to a safe goal and then until all mechanisms are
* at their goal.
*/
private CommandBase safeFollowProfile(Supplier<ArmState> goal) {
var toSide =
Commands.either(
followProfile(() -> ArmState.passOverToSide(goal.get().side())),
Commands.none(),
() -> goal.get().side() != getState().side());
var toOrientation =
Commands.either(
followProfile(
() ->
ArmState.fromRelative(
getState().elevatorHeight(),
goal.get().side().angle,
goal.get().wristAngle().getRadians())),
Commands.none(),
() -> getState().end() != goal.get().end());
return toSide
.andThen(toOrientation)
.andThen(followProfile(goal))
.withName("safe following profile");
}
/** Follows a {@link Trajectory} for each joint's relative position */
private CommandBase followTrajectory(ArmTrajectory trajectory) {
return Commands.parallel(
trajectory.elevator().follow(elevator::updateSetpoint),
trajectory.elbow().follow(elbow::updateSetpoint),
trajectory.wrist().follow(wrist::updateSetpoint, this))
.withName("following trajectory");
}
More arm GIFs
Goals for 2024
Going into the 2024 season, we have several (ambitious) goals, focusing on consistency and long term success as a department:
- 0 code rewrites
- 100% uptime
- Unit testing
- Run AprilTag pose estimation in actual matches
- Run consistent high scoring auto routines
- Finish comprehensive rookie documentation and lessons
- Hand-off robot to drive practice in a week after it’s finished
- Effectively use GitHub projects, issues, pull requests
- Some stupidly difficult project to sink time into, TBD
Since we will have a simpler robot this year than last year, and a week 2 event, we need to be able to finish characterizing and testing on the real robot in ~a week and let the drive team have it as soon as possible. This means most shenanigans will be saved for NYC.
Thoughts on Crescendo
Localization will be incredibly important in Crescendo. We plan on having, at a minimum, auto alignment to a single position for shooting. Given the target’s size, shooting from variable positions or while moving isn’t out of the question. While the climb (and our current lack of practice space) make it unlikely, we might be able to automate the operator away.
For our AprilTag detection hardware, we bought two OV9281 cameras last year, and are currently waiting for an Orange Pi 5 to ship.
Thus far, all departments on the SciBorgs have been collaborating to develop a unified robot design. I cannot stress how important this is, since, in previous years, we’ve had much less inter-department collaboration in the design process and the robot was segmented into mechanism groups far too early.
Some important design considerations that we will make sure to enforce:
- Every position controlled rotating mechanism must have a 1:1 geared REV throughbore encoder
- The robot must have a reasonable starting configuration and practical movements
- No single mechanism will have more than 1 DoF, it’s unnecessary this year
- There must be room (we had ~5cm last year) for electronics in the belly pan
- The shooter and intake must have adequate sensors
Now, speaking of sensors in the shooter, our design will necessitate keeping track of the position of songs within our conveyor system in order to move. To do this, we’re most likely going to use several beam break or range sensors, such as the LaserCAN.
This week
We plan on finishing most of the initial code for our robot this week (running into next week), meaning subsystems, hardware, and simulations. Additionally, we will work towards verifying the movements of our design by abusing Mechanism2d and AdvantageScope.
Expect updates, especially on some projects with an AMU of 1, soon!
Finished electronics and chassis.
