Modify the lift commands to kill scheduler in auto mode if the lift is stalling out

[jcorcoran]

The PIDPosition and Zero commands are currently used to move the lift in auto mode.

They both had a feature added at RIDE which attempts to detect a stall condition (lowering the lift on top of a container) and kill the scheduler as a result so that damage isn't done to the lift belts. This feature attempts to detect stall by monitoring the current being drawn by the lift motors from the PDP.

When the lift is being driven down, the stall current isn't sufficiently different from what's seen during normal operation. A more reliable means of detecting stall is needed. We should be able to just monitor the change in drive position over time.

There is currently a class in use called Debouncer. It is set up to detect if a condition has persisted for a specific period of time. Check out the comments in the Debouncer class to see how to use it. It's also used in Robot.Java for detecting Gyro drift (a very similar condition to what we need in the lift commands).

Something like the following should work:

Math.abs(currentPosition - lastPosition) > RobotMap.MIN_LIFT_POS_CHANGE;
//where MIN_LIFT_POS_CHANGE is the minimum distance we expect to move in a single loop iteration
//It might make more sense to get velocity of the system out of the controller (in the lift subsystem)
//  as a rate of travel is likely easier to think in terms of (inches in a loop iteration vs inches/sec)

The only risk in using this implementation is if the position controller isn't tuned sufficiently well enough to reach its destination position tolerance. This will lead to the command timing out normally, but after this code is added (since the drive isn't moving and the command is actively executing), it will be interpreted as a drive stall condition, which in turn will kill the scheduler = no more auto mode.

It would be prudent to run the auto mode with no timeouts on the lift commands to verify that the velocity limit chosen for the Debouncer object will not trip during normal operations.

[NotInControl]

Spoke to Omar from CTRE yesterday and he provided this information.

"PDP sends all frames once every 25ms. When you call the getters, underneath we just grab the last received signals.

I think in java you have to catch an exception if you call getcurrent before the next update. "

[jcorcoran]

Thursday we tried using velocity limits to detect stall. If that's also determined to be unreliable alone, we may be able to combine distinguish stall as a condition with low velocity and high current use, vs a slow moving lift that's not stalling out (low velocity, low current).