2-Axis Turret Subsystem/Commands

[isaacpatton72]

For this task, you will be required to create a pitch and yaw subsystem that has built-in software limits for the pitch as there are physical limits that will damage components if exceeded. Note that as long as the gear ratio from driving gm6020 to the gimble or whatever is being driven is 1 to 1, you can use the built-in absolute encoder in the gm6020s to calibrate where they are as long as you measure and calibrate the motors before uploading code to them. Also, be sure to use a smooth PID command for control of both of these axis' as you will get much better performance utilizing the Kalman filters built on top of the normal PID algorithm. You will also have to implement the commands and control interface for working with this Subsystem. for this I believe the mouse value fetches the velocity of the mouse so if you increase the setpoint by the velocity multiplied by some timestamp you should get an integrated position value. Make sure that this target value is put inside a wrapped float value to make sure that the target angles are always achievable. Also as a hint, both of your subsystems should make use of the wrapped float math utility in taproot as it will simplify your life substantially.