Closed Loop Control

Search…
Closed Loop Control
SPARK MAX can operate in several closed-loop control modes, using sensor input to tightly control the motor velocity, position or current. The internal control loop follows a standard PID algorithm with a feed-forward (F) term to compensate for known system offsets. 
Additionally, an arbitrary feedforward signal is added to the output of the control loop after all calculations are done. The units for this signal can be selected as either voltage or duty cycle. This feature allows more advanced feedforward calculations to be performed by the controller. More details about the types of feedforward calculations can be found on the WPILib documentation. Using the voltage units for arbitrary feedforward allows the user to send the calculated feedforward voltage from the WPILib API directly to the control loop.
Below is a diagram and the firmware implementation of the internal SPARK MAX PIDF.
1
//Synchronous PID, call at desired frequency
2
float pid_run(pid_instance_t* pid, float setpoint, float pv,
3
        const pid_constants_t* constants)
4
{
5
    float error = setpoint - pv;
6
​
7
    float p = error * constants->kP;
8
​
9
    if(fabsf(error) <= constants->iZone || constants->iZone == 0.0f) {
10
        pid->iState = pid->iState + (error * constants->kI);
11
    } else {
12
        pid->iState = 0;
13
    }
14
​
15
    float d = (error - pid->prev_err);
16
    pid->prev_err = error;
17
    d *= constants->kD;
18
​
19
    float f = setpoint * constants->kF;
20
​
21
    float output = p + pid->iState + d + f;
22
    pid->output = fminf(fmaxf(output,constants->kMinOutput),constants->kMaxOutput);
23
​
24
    return output;
25
}
Copied!
For more information on utilizing the built-in closed-loop control modes, please take a look at our SPARK MAX Code Examples.
Operating Modes - Previous
Control Interfaces
Next - Operating Modes
Recovery Mode
Last modified 5mo ago
Export as PDF
Copy link