Implement CasserolePID - Githubissues

WIPlib has a decent PID library, but we've seen some spurious issues with it. I also take issue with how they did their feed-forward programming. Also, it's got many features we don't use. Although some would consider it overhead, I believe it is a worthwhile exercise to go through and write our own.

Basic Requirements: Software must implement a PID Algortihm. Interfacing should be same as wpilib: users will instantiate a superclass of the CasserolePID class and override specific functions for getting the PID input, and using its output. The class should operate by spawning a separate thread in the background to do the actual PID math, gather PID inputs, and set the output. Run at a 10ms rate for now (hardcoded).

There are actually six terms to calculate, described below.

For robustness, the PID algorithm should implement a simple watchdog: Periodic thread should Increment a counter variable every time it runs. A separate method should reset the counter back to zero ("wack" watchdog). This method must be called at regular intervals by the control loop. When it is called, before it resets the watchdog counter, it should check if it's already zero. Under the assumption the PID loop is called faster than the control loop, it should never stick at zero. If it's actually zero, we can surmise the PID loop is no longer running, and should take some user-defined actions.... So just return something nonzero from the method to indicate a problem.

Terminology - Desired = current value of the setpoint Actual = current value read in from the system (sensors) Error = Actual - Desired

Class should support the following methods:

setP()

set the P (proportional error) gain - default 0 setI()
set the I (Integral of error over time) gain - default 0 setD()
set the D (derivative of error) gain - default 0 setF()
set the F (setpoint Feed-Forward) gain - default 0 setdF()
set the dF (setpoint derivative Feed-Forward) gain - default 0 setP2()
set the P squared (error * error with sign preserved) gain - default 0

setDSource()

set whether D term is calculated based off of the derivative of the error or the actual value.
Default to error setDeadbandErr()
if the absolute value of the error is less than this value, Override the output to zero. Do not accumulate any error in the integrator.
default to 0 (off)
The usecase here is to prevent tiny motor commands which don't do anything except just heat up motors. setMaxIntegrationErr()
if the absolute value of the error is more than this value, do not accumulate any error in the integrator and reset it to zero.
Default to inf (off).
The usecase here is preventing huge integrator windup when P would be sufficent would be able to get you where you want to go.

setSetpoint()

determines the new PID setpoint getSetpoint()
return the most recent setpoint of the algortihm getActual()
Get the present value seen from the sensors getError()
Get the present value of the error between setpoint and sensor value

start() - Kick off the PID background thread stop() - kill the background thread if running resetI() - reset the integrator back to zero

checkAndWackWatchdog() - should check if watchdog counter is zero. Return -1 if counter is still zero to indicate an issue. If counter is non-zero, reset it back to zero and return 0.

RobotCasserole1736 / CasseroleLib

Implement CasserolePID #6