Implement the Arm

[ErikCald]

Poseidon has a 2-axis arm which uses 2 sparkmax controllers and 2 neo motors to move it, as well as 2 REV through bore encoders. It also has 2 pneumatic brakes which can hold the arm in it's current position without needing to use motor/battery power to hold it there. However, the pneumatic brakes will be handled with the ArmPneumaticsSubsystem so you only need to worry about when these brakes should be enabled/disabled.

Incremental development is important, here is a recommended progression:

1. Be able to set the angle of both motors individually and they go there
2. Be able to set an X, Z coordinate and the motors go to the right angles.
3. Be able to traverse waypoints, making the arm go through a specific path and reach a desired X, Z coordinate. (PathPlanner could be a interesting tool to use for this, talk to Erik about it)

However, before even the first step can be achieved, a lot of work needs to be done:

• [x] Create ArmSubsystem.java
• [ ] Construct 2 CANSparkMax objects and set the proper settings
• [ ] Construct 2 DutyCycleEncoder objects for the REV encoders
• [ ] Log neo encoder position and velocity to networktables in periodic()
• [ ] Log REV encoder positions to networktables in periodic()
• [ ] Create methods to calculate the gravity compensation needed for the bottom and top arms (which are used as feedforward)
• [ ] Create methods to set the desired position of the top/bottom arm. Last year we used ProfiledPidControllers in order to have a trapezoid motion (ask Erik about it), you could keep going with this or use a simple position pid inside the sparkmax. These methods should also get the gravity compensation values and pass them to the sparkmax as well.

I recommend using the ProfiledPidControllers (or rather using the ProfileExternalPidController.java file which you will need to copy paste from last year). They worked well but we can use them better than we did last year by doing something like this here.

[ErikCald]

This is not important until much later but a huge benefit of using PathPlanner to control the arm is it will be far easier to switch the code from a 2-axis arm to a 3-axis arm since pathplanner also tracks the rotation at each point.