Understanding the 9128 2025 (REEFSCAPE) Robot

Command-Based Structure

For our programming, we chose to use a command-based structure. This type of code has three main components: subsystems, commands, and controllers.

1. Subsystems: These are the various major components of the robot. This year, we had three main subsystems: our drivetrain, arm, and intake.

For each subsysystem, we defined the motors and sensorsused by that subsystem along with the subsystem-specific functions using those motors and sensors. Something unique we did with our arm, however, (which we took inspiration from 2910's 2023 code) is that we contained all three "joints" of the arm (i.e. shoulder, extension, and wrist) in the same subsystem. We did this so that it was easier to control the arm rather than trying to control them using a command (which we found less intuitive than simply combining them into the same subsystem).

Commands: These are the actions we want the robot to do, and can be either subsystem-specific or involve the interaction of multiple subsystems. For example, for our arm subsystem, we had one command that set the position of the shoulder, extension, and wrist components all at the same time. However, for our auto-align, we needed not only the drivetrain to move towards the target, but also the arm to extend when it was within a certain distance calibrated to not only prevent our robot from colliding with the reef, but also from tipping or even just swaying excessively.

Controllers: Finally, we have our controllers, which is what allows the robot to act under driver operation during the tele-operated period. We used a dual-controller setup, with the driver's controller deciding the majority of the robot's actions and the operator's controller only controlling the outtake of coral and the various parameters for the auto-align to relieve the driver of some stress.

Figure showing the command-based structure

Design Highlights

Machine States Storage

Due to the large amount of different states our robot could have, from intaking to L1-4 to de-algaefying both high and low algaes, we realized that we needed a way to store various information about these states and have them easily accessible throughout the program. Therefore, we created the Machine States class, which stores not only the positions of all three parts of our arm, but also outtaking speeds, each position's name, and its ID. This way, if we want to create new positions, add new state-specific features, or configure each position individually, there is one class that allows us to do all of that.

Autonomous Pathing

Another aspect of our machine that we wanted to highlight was our autonomous pathing and general auto routines. We used PathPlanner, a path-creating tool that uses Bézier curves to create smooth, continuous motion of holonomic machines (i.e. swerve drives like our robot). However, not only did we create smooth paths (after carefully tweaking various constraints, such as the maximum translational and rotational velocity and acceleration of our robot), we used event markers to preemptively move our arm to the correct scoring our intaking position, saving us valuable time during auto. Additionally, we used constraint zones to balance speed during long streches of movement with precision when we got close to picking up a coral or scoring it on the reef, making our auto more consistent that it would have been without them.

Smart Auto-Align

The final unique aspect of our programming (that we wanted to highlight!) was our auto align. It used a three-step process to get the robot to the exact location and angle it needed to within ±5º of heading error and ±2 in. of horizontal error; operator input storage; target pose estimation and profiled PID translational and angular velocity control; and dynamic tolerances based on operator inputs.

1. Operator Input Storage: From the start of the season, our team understood the importance of automatically aligning to the reef with as little input from the driver as possible. However, we found that the operator trying to press three different buttons (branch, level, and angle) at the same time was unwieldly and unrealistic, especially during the height of the competition. As such, we realized that we needed to create a separate class to store the operator's inputs once they selected them, which we did through our AutoScoreSelection class. This way, the operator can select the next place for the auto-align to score on as soon as the coral is scored.
2. Target Pose Estimation and Velocity Control: Throughout the season, we had various approaches to try to align accurately and precisely to the reef.
   1. We had three separate PID controllers for the x speed, y speed, and heading. We used the tx and ta values to determine the x and y relative distances from the AprilTag and a HashMap to determine the desired reef angle from the primary AprilTag visible. However, we found that this solution was not precise enough for our needs, as it was inconsistent and was often outside of our allowable tolerance.
   2. We attempted to use full-field localization using Limelight's built-in vision calculations and Pathplanner's holonomic drive controller to create smooth and accurate paths to the target AprilTag. While this was a better solution than before, it was slow to start (as a result of the Profiled PID controller having max acceleration limits), and we felt that we had not explored all of our options.
   3. We decided to use one ProfiledPID controller for translation and one PID controller for heading, taking after other teams such as Mechanical Advantage. To determine the x and y speeds (as the translation controller only gave magnitude, not direction), we normalized the x and y magnitudes baesd on the output magnitude (i.e. the farther the robot was laterally, the faster the robot will move laterally). Additionally, we calculated the heading by rounding the robot's heading to the nearest 60º, since we assumed our drivers would align about 80% correctly with the reef face. This combination of solutions gave us the accuracy, speed, and precision we desired, and it ultimately became our solution to our auto align.
3. Dynamic Tolerances: Finally, we added dynamic tolerances based on the robot's distance to the target for when the arm should move to the desired level and when the auto-align is considered finished. For our arm, we found that our L2 position needed to be farther back than that of L3 or L4. By using dynamic tolerances, we were able to accomodate for this problem and ensure a consistent score on all three levels.

Written by Abdullah Khaled