Programming Update - Stateflows, PID Loops, and Trigonometry!
We decided not to post a programming update last week, mostly because we had almost nothing to post. It’s the exact opposite now!
Starting to tackle the big list of what we’ve done over the last 2 weeks, let’s get into Stateflows! (Our TEAM has experience with stateflows from BEST Robotics)
Our programming TEAM had a meeting with a few of our mentors about creating a detailed design plan, and not just something like:
But rather, a detailed list that explains every step in the process.
We ended up with this, which explains the process of our “prep to shoot” command:
High Res version
To sum up the stateflow, we run our pivot, shooter, and aim (left-right) PID loops given our limelight data, update them every 20ms, and stop once a note is shot or process is aborted.
We decided to use Lucidcharts to design our stateflows, as they are similar to Matlab’s Stateflows, which is where we have our stateflow experience from.
Why Stateflows?
We decided to use stateflows because they outline exactly what we need to do, rather than just a title of a task on a work managment site like monday.com (still useful but not super in-depth).
Because they go into so much detail, we can reference them while we write our code, and they heavily streamline the process of writing said code.
PID Loops
Taking a look at our actual code that we’ve created these past 2 weeks, we have our shooter speed PID and our pivot angle PID.
Our shooter PID is as follows:
Define P, I & D values in constants.java:
Set up the PIDController in the Shooter subsystem:
Use the PID in a command, also in the Shooter subsystem:
This compares the actual speed of the motor with the commanded speed, different from our pivot PID, which I’ll get into now:
Our pivot PID loop is very similar to our shooter PID, but compares our absolute encoder (angle in degrees) to a degree setpoint.
Comparing to an absolute encoder allows us to use a setpoint for angle, rather than speed.
We hope to test these PID loops as soon as we can.
Auton Paths
@Madelena already refered to these in this post, but I’ll take a bit of a deeper look.
Our TEAM decided to have ~8 total autons (4 per side).
Each path starts at either the top, middle, or bottom of the subwoofer, so we can still run a high scoring auton no matter where our alliance members are.
We also created a “[Color]NoteClear” path, which we would run if our autonomous paths end up to be more reliable / score higher than our teamates’ autons.
We created the paths with shooting in mind, and added rotation targets so our robot will be facing the speaker when we need to fire a note.
Here’s a gif to show the path for one of our autons (keep in mind this hasn’t been tested yet, and is just theoretical):
Trigonometry
While creating our shooter and pivot PIDs, we found that we needed a command that could return distance from known constants (and limelight data)
Here it is:
It takes the equation from
this page, and puts in our own constants .
It was quick and easy to make, but it’s worth mentioning.
Issues
We had some issues these past 2 weeks that are worth mentioning:
-
Accidentally set all of our “isFinished” commands to true, causing all commands to not work.
-
Had some issues declaring subsystems in RobotContainer, just minor misspellings and such (not shown below)
-
Originally created a list of motor speeds in an array with an index for shooting speed / pivot angle, but decided with the equations because the equations are more accurate
-
We had a backwards CAN cable which caused some weird stuff with our robot
-
We tried to make an Interpolation / LUT (Lookup Table), but decided to go with linear equations instead, due to the simplicity of them.
-
Issues with WPILIB (I didnt update it…)
What’s Next?
- Create a PID to aim the swerve chassis to an Apriltag, while still being able to drive (translate, but not rotate)
- Tune the many, many PID loops, and equations
- Finish sequential command that has fully automated intake → ready to shoot
Ending Remarks
Would you rather have videos of testing be in a GIF or a YT video? GIFs have less quality and no audio, but auto-play in posts, and don’t require an external site, like Youtube.
Useful links from this week:
Our Github
Vison Processing
Interpolation Topic
PID Controller Class
Rembrandts (4481) Github (Interpolation Reference)
Rembrandts (4481) Build Thread
On the fifth day of Christmas
