Software Update 3: Saying Goodbye to the Alpha Robot: Hello Yellow 
It’s been quiet here on the blog from the software side for a bit, so we figured it’s time to share some progress!
The above videos show the silver alpha robot in its final form, running with its pneumatic wrist, “beta” version of the arm control software, and “spinny no pinchy” claw (see above).
When we built the alpha bot, its primary goal was to give the software team adequate time to lay a solid foundation for arm control that wasn’t rushed or hacked together, while also providing valuable insights for the design teams working on updates to the arm and intakes. Additionally, we wanted to address even the smallest of control irregularities or odd behaviors from the onset, to leave as little tech debt as possible for us to address later in the season.
In the videos above, the arm moves smoothly between its setpoints and holds position well, so we can say with confidence that the alpha robot has done its job. Let’s dive into the software powering it!
Arm Control Software
Our initial instincts were to use Motion Magic with the CTRE api, but since our REV through bore encoders are wired to the RoboRIO, not to the TalonFXes on each Falcon 500 powering both joints, we didn’t find a good software solution for configuring the sensors to the talon. It was at this point we pivoted to on-RIO control.
We started with two PIDControllers, one for each joint, tuning setpoints and gains for each joint independently. We found this to have precise enough control, but the arm wasn’t moving very smoothly, and so we knew we needed to use trapezoidal motion control. After a few days of struggling through the semantics of the api, we got ProfiledPIDController implemented with each joint of the arm having its own controller. Note that it’s necessary to call reset() on your ProfiledPIDControllers upon enabling the robot or switching back to PID after using an open loop control mode - this had us scratching our heads till we discovered it! After tuning the gains and trapezoidal constants, we landed on a solid implementation but lacked the final piece - feed forwards.
We noticed that as the arm approached horizontal angles, it would struggle to reach the setpoint until the integral term would build up after a few seconds of waiting, and then seem to slowly oscillate. This was suboptimal to us and we knew we needed a feedforward solution. WPILib offers ArmFeedforward which works well for single jointed arms, however we realized that for a double jointed arm, the dynamics are significantly more complex.
We then brought in 6328’s Double Jointed Arm Feedforward class, which is an implementation of the dynamics model published in an excellent paper by @rafi of Team 449. After some tweaking to make sure all of our directions and “zero points” were correct, this ended up being the final piece of the puzzle we needed, stabilizing the arm at steady-state positions and significantly reducing slow oscillations.
Cardinal Direction Drive Control
One thing we realized from the game reveal was the orthogonality of the scoring and pickup locations, that is to say, they all lie at 90 degree angles to one another. This presents a great use case for Cardinal Direction Control - shoutout Teams 111 & 6329 for inspiration - wherein the A B X Y buttons on the driver’s controller correspond to 90 degree angle increments for the drivebase heading. This also still allows translational control as normal, so this can aid in quick alignment to scoring or pickup locations.
Architecturally, this is a simple PIDController closing the loop on the gyro, which takes over control of the swerve drive’s theta value when a button is held. A neat trick is to enableContinuousInput() and use inputModulus() in the WPILib MathUtil library on your gyro value, so that your drivebase never spins the long way around, and always takes the shortest path to a given heading.
Goodbye Alpha!
Starting today, the software team will begin working on porting the code to the Gold robot, which is the first of two competition-spec robots the team will be building this year. The Gold robot is similar, but not identical, to the Alpha robot. The Gold robot for example has a Spinny + Pinchy claw, with a pneumatic release to make dropping cones much more smooth than the Spinny No Pinchy claw.
Once we feel confident with the performance of the Gold robot, we’ll dismantle the Alpha robot’s drivebase so its SDS Mk4i modules can be installed on the Black robot (the second competition robot currently undergoing assembly). The arm will stay together to serve as a pit display and test platform should it be needed. So in a few days we’ll say farewell to the Alpha robot, the one that started it all!
