Auto path preview and a bit of turret cleanup

[henrymilbert]

Add a preview for the selected auto path and simplify some turret code by moving the motor to turret ratio into the sensorToMechanismRatio config and directly calling setPosition instead of setting an offset value.

[github-actions]

Code Coverage Report

There is no coverage information present for the Files changed

[shishghate]

You and @mark-teskey , need to figure out what values to use for the turretMotor pid/motion magic as they are wildly different.

Mark's
turretMotor:
deviceType: TalonFX
id: 28
motorRotation: Clockwise_Positive
pidConfig:
slot0:
kS: 0.1 # static friction - increase if turret won't move at small errors
kP: 4 # position error gain - increase if still lagging, decrease if oscillating at setpoint
kD: 0.28 # damping - increase if oscillating, keep roughly kP/20
staticFeedforwardSign: UseClosedLoopSign
motionMagic:
expoKV: 0.04 #0.10 # cruise speed - lower = faster moves, higher = slower. Start here, decrease if turret feels sluggish
expoKA: 0.1 #0.2 # acceleration smoothness - increase if dead zone reversals are too violent, decrease if turret feels mushy

Your's
turretMotor:
deviceType: TalonFX
id: 28
motorRotation: Clockwise_Positive
sensorToMechanismRatio: 15.625 # 250/16 gear ratio
pidConfig:
slot0:
kS: 0.1 # static friction - increase if turret won't move at small errors
kP: 62.5 # position error gain - increase if still lagging, decrease if oscillating at setpoint
kD: 4.375 # damping - increase if oscillating, keep roughly kP/20
staticFeedforwardSign: UseClosedLoopSign
motionMagic:
expoKV: 0.625 #0.10 # cruise speed - lower = faster moves, higher = slower. Start here, decrease if turret feels sluggish
expoKA: 1.5625 #0.2 # acceleration smoothness - increase if dead zone reversals are too violent, decrease if turret feels mushy

[henrymilbert]

You and @mark-teskey , need to figure out what values to use for the turretMotor pid/motion magic as they are wildly different.

The values I am using here should be equivalent to the old values, but they are different numbers since the sensor to mechansim ratio now lives on the motor instead of being added in code. This means that the rotation values that the motors directly set and read are now 1/15.625 times the old values. Since most feedforward and feedback gains are output per rotations (or rps or rps/s), and the rotations read by the motor are now 15.625 times smaller than the previous rotations, the previous values (except for kS, which is a flat added value in units of the output type) have to be multiplied by 15.625 for the control loop to have the same behavior. Feel free to double-check that I did the math right.