Software Update #5: The Kitchen Sink
We haven’t done a dedicated software post for (checks notes) two months…
There’s a lot to talk about, so here’s a collection of mostly-unrelated-but-hopefully-interesting software projects we’ve been working on. As always, we’re happy to answer any questions.
Autos & “The Questionnaire”
We were honored to receive the autonomous award at the WPI District Event. So let’s talk more about autos…
We decided early on that there were some key improvements we needed to make in structuring our autos compared to 2022. First, we wanted to create autos that were flexible without needing to individually manage a large number of routines. For example, we might want to disable the balance at the end of a two game piece auto when an alliance partner was filling that role. To support this flexibility, we developed an interface that we colloquially call “The Questionnaire”.
Each auto routine is associated with a set of “questions” that the drive team answers before the match. For example, the two game piece auto asks whether it’s running on the field or wall side, what level to score on, and whether to balance. We created a SwitchableChooser class, which is displayed as a standard chooser in Shuffleboard but with options that can be dynamically changed by the code. Based on the selected auto routine, the questions and their responses are displayed on the dashboard. Here’s what the final interface looks like:
The AutoSelector class manages all of the SwitchableChooser objects and returns the set of responses. Of course, the auto routines need to support all of these possible variants. We switched from defining each auto routine as a separate class and instead combined all of the auto routines into a single AutoCommands class with factory methods. This makes it much easier to share constants, waypoints, and subroutines between all of the possible autos.
As in previous years, our drive trajectories are defined in code relative to known field coordinates. We’re using our custom trajectory generator which extends the WPILib tools to support holonomic drives. This approach makes it exceptionally easy to support multiple variants of an auto routine. For example, we can simply swap out the ending waypoint of a trajectory to score on a different node. And since waypoints are shared between trajectories, changing an intake location, scoring position, etc. will apply to every relevant auto routine with no extra effort required. We currently have over 100 auto routine variants, which would be nearly impossible to support if we needed to manually maintain each routine and keep them in sync.
The actual switching of the routines is handled by a combination of SelectCommand objects and suppliers. For example, this auto routine instantiates several variants to support scoring on three possible levels. We always structure the commands so that every variant of the drive trajectories is generated ahead of time.
Another key feature of the command factory approach is that it allows us to define high level tasks as subroutines. We have command factories for driveAndIntake, driveAndScore, and driveAndBalance, which can be combined to create most of our auto routines. For example, our field-side three game piece auto is just a sequence like this:
return sequence(
reset(startingPose),
score0Sequence.command(),
intake0Sequence.command(),
score1Sequence.command(),
intake2Sequence.command(),
score2Sequence.command());
In fact, this subroutine approach makes building autos simple enough that we created our main three game piece auto between back-to-back practice matches at the WPI event. While we don’t have video of that practice match (where it worked on the first try 
), here’s the first run during a qualification match:
We’d be remiss to talk about auto routines without mentioning auto balancing. We opted not to use auto balancing in teleop, so we only deal with the case of a single robot balancing. Here’s our current approach:
- Measure the current angle of the charge station based on roll and/or pitch (taking into account the current yaw direction).
- Drive at 15 in/s upward. Pause whenever the charge station is tilting downward more than 8°/s.
- Once the charge station is within 3° of level, stop and lock the wheels to an X configuration.
This approach has been mostly effective, though we are still exploring ways to increase the speed. Below is an example from district champs (this actually the only time we used it at the event…)
Finally, we are pleased to demonstrate a working version our three game piece auto for the cable bump side of the field. Our relatively high center of mass and small wheel base made this a challenge, but we found that moving across the bump at a ~30° angle made a big difference by spreading the force of the impact across all four modules.
Auto Scoring (From Theory to Practice)
We’ve been successfully using our automated scoring and pickup systems since week 2, but one important issue we identified is that there were always clunky transitions between human and software control (see below). This is problematic because of course smooth = fast.
This behavior was due to two main issues, which we managed to work through by the end of DCMP (district champs, not the Daly division 
):
- Initial velocity: The auto scoring uses a trapezoidal profile based on distance, and the initial version always started the profile with a velocity of zero. If we tried to start the auto score while moving, this would cause the robot to jolt to a stop briefly. It was a relatively easy fix to measure the field-relative velocity of the robot and use it to reset the trapezoidal profile. We also discovered that doing proper feedforward control based on the profile velocity was critical to smoothing out the transition.
- Obstacle avoidance: We want to start auto-scoring while still at the side of the charge station, but this means that moving in a straight line to the target doesn’t always work. Even within the community, we want to stay back from the nodes to avoid incidents like this:
For obstacle avoidance, we settled on an approach of shifting the profile setpoint based on the robot’s current position. We’ve found this to be more robust than full trajectory generation with splines, plus it adapts immediately as the robot’s pose estimate is updated from vision data. The setpoint roughly follows these movements:
- When the robot is approaching the community, the setpoint stays completely to the left or right of the charge station.
- As the robot enters the community, the setpoint moves along the grids to the target node. This creates a smooth turn, and during this move it stays roughly centered between the nodes and charge station to avoid hitting either.
- As the robot approaches the target node, the setpoint moves forward to its final position.
Here’s an example from a log showing how the setpoint (in green) moves together with the robot to form a smooth path. This animation is at 50% speed.
The final result running on the robot looks something like this:
The same improvements to the transition between human and software control apply to our automated pickup. Below is a match video with many more examples of the system in action. When the LEDs show a rainbow pattern (as much as it’s visible in the video), the robot is autonomous.
Avoiding Arm Self-Destruction
This is an older feature, but a particularly interesting one. From the very first time we enabled the arm, we included an automatic emergency stop feature. There are numerous ways that the arm could damage itself if something went wrong, and we want to be ready for every possibility. For example, the lack of hard stops on the joints means that a badly tuned feedback controller could cause the arm to break its wiring. Similarly, if the arm got stuck on part of the field it could overly stress the joints.
When E-stopped, no voltage will be applied to any joint motors until the drivers disable and reenable the arm using a manual override. There are three conditions which will cause the arm to automatically E-stop itself:
- If any joint (shoulder, elbow, or wrist) goes more than 5° past its software limits, the arm will be stopped immediately. This is to prevent damaging any wiring around the arm.
- If any joint is more than 20° from its setpoint for 1 second continuously, the arm will be stopped. This detects lots of miscellaneous failures, like the arm getting stuck on a field element, a failed motor, or a disconnected encoder.
- If the code starts with the elbow pointed upwards (away from the shoulder joint), the arm will not run until it is manually moved to point downwards. This is because the joints are zeroed using absolute encoders. If the elbow was pushed upwards and slightly past its limits at the top of its range of motion, it could initialize 360° off from the actual location. This would badly twist its wires if the code tried to move it towards its homed position. Waiting until the elbow is pointed downwards means the position is unambiguous.
The arm E-stop has already proved itself in multiple matches. It’s indicated by flashing red LEDs on the shoulder segment. For example, when we broke the elbow chain during DCMP (as @Advaith discussed), the arm couldn’t maintain its setpoint and it stopped itself to prevent any uncontrolled flailing:
Similarly, the arm E-stopped itself when we lost our CAN bus during the WPI event. This would be particularly important if the CAN connection had been intermittent, since randomly enabling and disabling motors is likely to only do more damage in that case.
An extra “bonus” safety feature we added is monitoring on the temperature of the NEO 550 powering our gripper. Even with current limiting, intaking and holding game pieces for extended periods can heat it up (in the lengths of time we’d be running for driver practice, not during a match). To avoid damaging the motor, it’s disabled if the temperature is above 80°C for two seconds continuously. The motor is re-enabled with a manual override or if the temperature falls below 65°C.
How To Not Smash Game Pieces
We redesigned the arm after our first event, and one of the changes lowered the position of the gripper by ~0.5in in its homed position. This meant that we could no longer extend the wrist downwards while holding a game piece, because it would comfortable smash into the shaft and electronics in the bellypan:
But why fix a geometry issue in hardware when it can be fixed in software?
We use a numerical optimization system (Kairos) to generate arm trajectories, but it only deals with the shoulder and elbow angles. We run the wrist using a separate trapezoidal profile to reduce the complexity of the optimization problem. This decreases the solve time for Kairos, and the relatively small movements of the wrist don’t significantly affect the arm’s weight distribution.
A straightforward solution to this problem would be to incorporate the wrist movement into Kairos; we could just add a constraint preventing the tip of the wrist from moving too close to the bellypan, and Kairos would generate a trajectory that sequences the movements to prevent this. However, the solve time becomes unreasonably long.
Instead, we opted to simply delay either the arm or wrist movement when starting a new trajectory. The wrist is not allowed to be within 45° of straight while the wrist joint is within 20cm of the center of the robot. When starting a trajectory, we calculate the time range where the arm is in that restricted region and whether the wrist needs to pass through the 45° restricted range. Based on that data, it decides on one of three approaches to follow the trajectory:
- Neither constraint will be violated, so move both the arm and wrist immediately. This is the same as the original behavior before the constraints were implemented.
- Move the wrist first, and delay the arm trajectory such that the wrist will have moved past 45° by the time the arm enters its 20cm restricted range.
- Move the arm first, and delay the wrist trajectory such that the wrist enters the 45° range just as the arm leaves its 20cm restricted range.
These delays are also managed in conjunction with any necessary movements of the cube intake. For example, both trajectories might need to be delayed further if the arm will immediately enter the restricted region around the intake.
Below is an example where the robot reaches to a scoring location by passing the wrist through the center of the robot. This falls under case 3 where the wrist movement is delayed until the arm has safely passed through the robot.
Standing Our Ground (Automatically)
Early on in our testing with swerve, we experimented with turning the wheels to an X configuration to resist movement. After several incidents of slipping off of the charge station, we’re putting this mode to good use. After DCMP, we added two conditions under which this mode will be activated automatically:
- When the robot is stationary on the charge station. This is based on the robot’s localization data, which often drifts a bit while starting to climb the charge station but is reliable enough to determine if we are actively trying to balance. Locking the wheels here means that the robot is more stable if our partners are balancing.
- Just before the end of the match. The FMS consistently provides about 300ms between reporting a match time of “0” and the robot being disabled. Luckily, 300ms is almost exactly the right length of time to turn the modules to an X configuration. In both auto and teleop, we now use those final milliseconds to lock the wheels; it’s preferable to stay docked and unengaged on the charge station than risk falling off completely.
Odometry Logs
With 70 matches under our belt, here’s an interesting visualization of the robot’s localization data. This is all of our matches overlaid and flipped to the red alliance:
There are still some glitches (especially from previous events), but our preferred auto routines and overall driving lines are very clear.
Battery Scanner
In an effort to gather more complete data about battery usage and performance, we’ve brought back automated battery scanning. Using a simple barcode reader connected over serial, the name of each battery is recorded as metadata in the robot’s log files:
We used QR codes on top of the batteries last year but switched to barcodes given the space constraints of Banana Split’s battery mount:
While we are still determining how best to use this data when analyzing long-term trends, one of the most useful features we added is an alert on the LEDs when the robot detects its battery has not been changed since its last match. When the robot is powered in the pits, the lowest LEDs flash red:
While we never rely solely on this feature to ensure batteries are swapped between matches, it’s always useful to have an extra line of defense against careless mistakes (especially when the pit crew might be rushing to address issues between matches).
Pretty Lights!
Banana Split has LEDs on the first two segments of its arms. We’ve packed a lot of functional signals onto the LEDs this year. For anyone looking for potential new uses of robot LEDs, here’s a complete list of all of our current patterns:
- Disabled: Team colors (gold and blue) in waves, or red/blue waves when connected to the FMS.
- Autonomous: Fast waves of team colors (gold and blue). The LEDs turn green once the routine is finished, then the green lights slowly fade in the 3s period between auto and teleop.
- Auto Score/Auto Substation: Rainbow on the upper segment.
- Game Piece Secure: Solid green on the upper segment. Activates when the gripper wheels stop turning.
- Cube Intake Ready: Solid purple on the upper segment. Activates once the cube intake is up to speed and the arm is in position.
- HP Signal/Single Substation: Solid yellow/purple on the lower segment.
- HP Signal/Double Substation: Stripes of yellow/purple on the lower segment.
- ”Get The HP’s Attention”: Flashing white on the upper segment.
- Endgame Starting: Flashing blue on the upper segment. This alert also uses the rumble on the driver controllers.
- Fallen Over: Flashing white on both segments (the robot is tilted more than 60° in any direction).
- Arm Coast Mode: Solid white on the lower segment. Activated via an override switch on the operator console and only applies when disabled.
- Arm E-Stopped: Flashing red on the upper segment.
- Robot E-Stopped: Solid red on both segments.
- Low Idle Battery: Solid orange on both segments (the battery is under 10 volts while disabled).
- Battery Not Changed: Slow red flashing at the base of the lower segment.
- Code Starting: Slow white flashing at the base of the lower segment.
The “Code Starting” LEDs are one of our newest additions. The robot code usually takes ~20 seconds to initialize between loading arm trajectories, configuring controllers, and generating drive trajectories. During the first few cycles, we use a Notifier thread to run this LED pattern, which indicates that the code has started but is not ready yet. When rushing to connect to the robot, it’s comforting to let everyone around the robot know that the code is starting successfully.
Finally, we’ve made a special change to the robot’s “disabled” pattern specifically for our trip to Texas 
. Before and after every match, we will be displaying the colors of the pride 
and trans 
flags:
(This looks much better in person; the colors aren’t captured very well on video.)
To Houston!
Banana Split has begun the journey to Texas…
…and it seems it’s almost time to face the Archimedes division.
-Jonah