Implement FeedForward

[El1zabethS]

FeedForward takes current inputs to the arm (whether that be static inputs from things like gravity, or updating inputs like voltage to the motor) and calculates where the arm is GOING to be.

Feedforward will be useful for preventing drift in systems like:

• [x] Arm
• [ ] Telescoper
• [x] Drivetrain

[El1zabethS]

video useful for drivetrain tuning and system optimization

[El1zabethS]

https://www.youtube.com/watch?v=gRbBkdinq0o

[El1zabethS]

https://docs.wpilib.org/en/stable/docs/software/advanced-controls/controllers/feedforward.html

[El1zabethS]

https://docs.wpilib.org/en/stable/docs/software/advanced-controls/introduction/tuning-vertical-arm.html

[El1zabethS]

https://www.chiefdelphi.com/t/holding-up-a-wrist-with-a-neo/425787

[J-Palomino]

So i have a question, but i cant answer it on my own because im not sure which branch is tied to this issue,

are we using position control on the neos in most recent attempt to control the arm. i see a link to that in your most recent comment. in that chief delphi thread someone links this : https://codedocs.revrobotics.com/java/com/revrobotics/sparkmaxpidcontroller#setReference(double,com.revrobotics.CANSparkMax.ControlType,int,double)

apparently we can use position mode, you might already be using this but im not sure, gonna look through codebase to see if i see SetReference() using a position signature.

[El1zabethS]

added the branch (feat#97-feedForward)

also i don't believe we are using position mode, i will take a look

I also think I have some other ideas of implementation, as well as the fact that it wants the position in radians, so i need to see what "position" we pass into it. and i think i have an idea of how to offset feedforward so we can still keep our standardized method of positions

[J-Palomino]

If you need to you can use degreesToRadians()

https://github.wpilib.org/allwpilib/docs/release/java/src-html/edu/wpi/first/math/util/Units.html#line.66

[J-Palomino]

Thanks for linking the branch!

[El1zabethS]

https://v5.docs.ctr-electronics.com/en/latest/ch16_ClosedLoop.html#gravity-offset-arm

[El1zabethS]

^^^ above link:

Despite being ctre, it explains the math behind "arm feed forward" which gives a deeper understanding of how it works. It also enables us to implement it better because we understand it lol, its a short but insightful read (just the gravity offset arm part)