CalculateMaxAngularVelocity now uses the physical radius of the modules

[yapplejack]

When calculating the maxAngularVelocity of the robot a Rotation2d is used. A rotation2d uses a unit circle, which will does not accurately represent the size of the robot.

For example, my robot has modules at (+/- 0.28575m, +/- 0.28575m) and a max velocity of 4.8 m/s (max standard gearing of a MaxSwerve). When using rotation2d we get 6.1 m/s for the max angular velocity:

Using inches instead of meters for the module location yields the same result of ~6.1 rad/s when the velocity is still in m/s (0.28575 -> 11.25 in):

I replaced this with $w = \frac{v}{r}$ which yields $\frac{4.8}{0.404112} = 11.87789 rad/s$. This number can be mentally checked by thinking about the distance traveled as the circumference of the circle that swerves make: $C = 2\pi r \rightarrow C = 2.53782336 -> \frac{4.8}{2.5378} = 1.89$ revolutions per second.

Apologies if I have a misunderstanding for the usage of the unit circle if that was intentional. With this change you may want to update your examples and the tuning webpage to use meters and specify that meters are desired. I would also be happy to make those changes if my push is approved.

[thenetworkgrinch]

Thank you for finding this! Have you been able to test on a real robot. I am hesitant to add this (although it seems very necessary!) until its been tested.

[yapplejack]

I should be able to test it tonight. After more thought this would mess with everyone's code if pushed, so there should probably be steps taken to ensure teams are not left wondering why their robot doesn't drive like it used to.

One solution could be to make maxAngularVelocity required (final), with a warning that leads to a calculator that would yield their previous max velocity. This could then be removed after enough time has passed (maybe late February 2025).

Another option could be to add a boolean that allows you to opt in to the new maxAngularVelocity formula, as the 6.1 rads/s might actually be more controllable than the 11.89 rad/s we could have had (the starter code for the Maxswerve limits the angular speed to 2pi rad/s and the Phoenix6 example Swerve code limits the max angular velocity to 1.5pi rad/s). That being said I am very curious how the change would change playing into defense (we don't have a good way to test this).

[thenetworkgrinch]

Absolutely! Your concerns are very valid. Which is why I haven't merged it yet. If it does work and is worthwhile I will include it in the next update and include a warning about it with every startup for the next few months and post updates calling it out everywhere.

[yapplejack]

Current YAGSL code with unit circle for max angular rotation:

https://github.com/user-attachments/assets/127b8972-f430-444e-8134-1e61a5cab5c7

https://github.com/user-attachments/assets/1e4a99b6-f807-4e06-adfd-2d8a495dd9c8

With Math.hypot():

https://github.com/user-attachments/assets/a3523dfb-9472-4734-89e6-5707d6544ead

https://github.com/user-attachments/assets/8affe084-dcfa-4ccf-b7a4-6c9c242df97d

The floor is horrific and our tread is not fresh so don't mind the veering off course. Here is a video of in action to verify that our robot functions correctly, our shooter had just taken a hit before this so we miss a single shot but the robot was correctly positioned. This is also a brand new driver, we are very happy that we switched to YAGSL from the default Max Swerve code.

https://github.com/user-attachments/assets/6ac1b76a-3950-45ae-a6f3-082916cde68c

We control the heading with angular velocity on the right stick (my students prefer driving with this):

It feels the same to drive besides rotating faster. Maybe it moves a tad slower when rotating at max speed but that makes sense and could certainly be desirable into defense. It is a bit touchy and could probably use some input altercation.

On that note this is also problematic in the example code:

My team competed this weekend so I haven't had time to look into it but I confirmed that there is an issue on the robot and in Advantage Scope's swerve visualizer. When giving it 100% of the the right stick it will barely rotate when driving at max speed. It will rotate more the slower you go but it barley rotates at max speed. Not sure if this intended but my students and I heavily prefer the implementation showcased on the videos above, here is a video of the issue:

https://github.com/user-attachments/assets/c22d7ab4-3ab5-48f0-aba4-229886bac34d

Finally is there a way that I could add a page on the gitbook site about configuring a MaxSwerve with YAGSL. It was very tough to navigate especially with all the oddities of configuring modules correctly. I found a reliable method to use the provided calibration tool while still making sure that the zeroed positioned ends with the bevel gear facing to the left. I love documentation, here is a sample of my most recent work: https://robonauts-everybot.github.io/Everybot-Docs/manual/chassis/gearbox-and-motor

[thenetworkgrinch]

I accept PRs on the gitbook. The repo is here if you want to submit any changes. I recommend you create your own clone to mess with it in their environment.

https://github.com/thenetworkgrinch/YAGSL-gitbook

As for the PR this looks promising, could you send a video with the controller in view while spinning left and right. It looked like there was alot of skew and i just want to verify there isnt. How does it perform in odom too?

Could you also test this with field oriented direct angle?

[yapplejack]

Apologies for the delay, my team is taking a 2 week break after our event, I was able to get briefly get in and do some testing.

I didn't realize how horrible our bot's skew was, so apologies if this testing is not useful. The skew scales with the rotational velocity as can be seen in the videos. During rotation in the videos I held a button that prevented left X axis input from the controller so the robot would drive straight up and down the field:

Rotation on right stick, current YAGSL drive function, 6.1 m/s angular velocity (with the new scaleTranslation):

https://github.com/user-attachments/assets/57862b35-77be-4e16-bc2e-63e90ee32156

Rotation on right stick, non unit circle drive function, 11.89 m/s angular velo:

https://github.com/user-attachments/assets/ec924647-2681-48ff-b322-0e32b5b7a794

Direct angle 6.1 m/s:

https://github.com/user-attachments/assets/efbefccc-7527-4e8c-8d70-c71ffb249d8e

Direct angle 11.89 m/s:

https://github.com/user-attachments/assets/1736c354-03f9-4219-9de2-d93cec698e7d

Advantage scope swerve output for 6.1 m/s in right stick angular velo:

https://github.com/user-attachments/assets/79a715c0-f74e-4b13-a5b4-0c949c54e31d

Advantage scope swerve output for 11.89 m/s in right stick angular velo:

https://github.com/user-attachments/assets/6c838462-7398-4877-bff4-5501eab6f334

Not sure why we have currently have so much skew, the robot does not appear to have issues when moving purely via translation. It does seem like you get roughly 2 times more skew with the roughly doubled max angular velocity which has me hopeful if we can iron out the skew in our robot.

We use heading correction when using direct angle, no heading correction with angular velocity controls and cosine compensation in both configurations. We do use CAN values over 40 if that could be the issue. I will triple check the robot next week to ensure this not an issue on our end. I may be able to test on another team's robot next week for additional clarification. Any advice on the robot's skew would be appreciated, as the skew is still bad with the default code, apologies if this data is not useful at the moment.

[thenetworkgrinch]

Hello! Sorry for not getting back sooner! Have you resolved the skew yet? I really do like this change and want to add it, but I am not sure if i should if that skew is present

[SammyPants37]

I have tested it on our robot and have had no problems. max rotational velocity is faster and the skew is less than when using rotation 2Ds

skew using rotation 2D: https://www.dropbox.com/scl/fi/bmt975qp7uf9d2ecsbpxc/rotation2D-skew.mp4?rlkey=5kn7evubfyby7djwkzfio7xrh&st=c96rcttz&dl=0

skew using hypot: https://www.dropbox.com/scl/fi/qp49sbdggv1jog4883nn7/hypot-skew.mp4?rlkey=gf7tcvhjbviqaticanz9w5ns7&st=o4ofaf35&dl=0

(sorry if the dropbox links don't work perfect the github file size limit hurts)

[thenetworkgrinch]

I will merge this and limit the max angular velocity to 4rad/s per @SammyPants37's testing.

Thank you so much for both of your work!!

[yapplejack]

Thank you @SammyPants37 for the testing!

Sorry for not posting more follow up, we have been on a wild goose chase for our skew. Looking at others code, such as 2910, they calculate angular velocity with hypot and do not limit the the angular velocity in normal teleop conditions. Limiting the angular velocity to 4rad/s as a default is very understandable though, notably this will be less than most teams will have previously had if they were a square (previously ~6.1 rad/s).

On the subject of our skew we believe it to be a Spark Max issue and think we have found a major contributor that we are testing on the robot this Monday. CAN Status Frame 5 relays the information about the position of an attached duty cycle encoder (through bore in our case) and this frame reports every 200ms. Here is a program that askes if the value from the absolute encoder is different from the pervious value seen last periodic loop:

Every 200ms we get 9 stale measurements and a single new measurement, notably impacting motor.getMotor().getAbsoluteEncoder(Type.kDutyCycle).getPosition() and thus the knowledge about the swerve module positions in the pose estimator, changing this frame to 20ms:

Oddly enough we did find that we get stale measurements sometimes with 20ms and instead prefer 19ms. This echos what 3005 does for their configuration, the team who helped develop the Max Swerve. I will make a separate pull request for this issue if it removes our skew and will probably add a few of the consensus bells and whistles that 3005, 111 and 6328 used on their Spark Max based swerve modules to the configuration of the encoders/motors.

We put a lot of work into debugging our skew, here is something fun from that adventure, animations of module states and robot position:

https://github.com/user-attachments/assets/a311c74b-1dee-4e24-bba6-4bda1f373021

[thenetworkgrinch]

Another thought on the skew is it could be the lerping we do to counteract skew here might cause this.

https://github.com/BroncBotz3481/YAGSL-Example/blob/dev/src/main/java/swervelib/SwerveDrive.java#L486

As a note for the version you're using (before 2024.5.0.4) the throughbore is used by the sparkmax natively, you have an option with this in the newer version that would limit you by your loop time on the rio.

Are you using a RIO2 or RIO1? @yapplejack

Could you join the discord so we can chat about this more? https://discord.gg/SFNUH8NB

[thenetworkgrinch]

I think this PR is now deprecated due to your other PR #231, I will close this and refer to the other one if you dont have any objections