[DRAFT] Fix for TalonFX PID wrapping

[7910f6ba7ee4]

Draft for fixing TalonFX wrapping (if it needs to be fixed).

• add placeInAppropriate0To360Scope from BaseTalonSwerve code. Should take the current true position of the encoder in units of degrees, then places the setpoint in the appropriate scope.
• the method could probably be optimized since it uses a while statement
• revise getPosition() to use modulus with 360, since a few methods externally rely on position being within the first rotation. Not sure if this is completely necessary since some of the methods just use .getCos and .getSin for a Rotation2d, including the .optimize which just rotates the angles to find the difference.

[thenetworkgrinch]

I really like this, thank you! I will test it tomorrow.

[thenetworkgrinch]

Thank you for the docs! I may move the function into SwerveMath or just copy it over to the TalonSRXSwerve because this issue would be present in both of them. Probably will move it over to SwerveMath to reduce redundancy in the code.

[thenetworkgrinch]

I tried the fix and couldnt get precise control over talons. Talons do NOT work right now and should be tested with in the air when working with them for now. I pushed my chanhes to the branch talonfx

[thenetworkgrinch]

@7910f6ba7ee4 Let me know how your testing goes tomorrow, can you try and modify https://github.com/BroncBotz3481/YAGSL-Example/tree/talonfx ?

[7910f6ba7ee4]

We'll let you know how it goes and make changes to the TalonFX branch. I'll make sure to keep in mind your suggestion about keeping the wheels in the air too.

[thenetworkgrinch]

I added the normalization function from another team which is smaller and looks to be better than the one from 364. In the sim it ran with a steady state error (I am not sure why yet).

[7910f6ba7ee4]

Swerve is now working for us! Note that we're using the two 2048 integrated encoders and do not use cancoders because of some mechanical issues (I commented out the code that initializes and resets position by cancoder in our own code). Some of the problems could (?) come from setting integrated positions to cancoders, I'm not sure.

Some notes:

• I removed the normalization code, since I was only testing with the placeIn360 from 364.
• Ensured the parameter for placeIn360 was not normalized and in full rotational degrees
• Added a scalar for the feedforward for the motor to make the feedforward work, gotten from 3181
• Removed a lot of the print statements since it was tanking response speed from the controller
• Full code directly after changes can be found here but I changed a few conversions in getVelocity() and getPosition() in a later commit (didn't have any effect on the working code though)

I'll post a video of it working soon.

[thenetworkgrinch]

Why that feedforward multiplier works is existentially confusing. I will gladly accept your changes, Thank you!

I spent all afternoon yesterday trying various fixes so I am excited to see your video too.

[7910f6ba7ee4]

YAGSL-working.mp4