Recently I discovered the library "ArduPID" which will allow me to use a PID controller for one variable input. I am planning on using it for a TVC rocket but can't figure out how to run it for two variables at once. Is this possible?
Here is the example code provided by the library:
#include "ArduPID.h"
ArduPID myController;
double setpoint = 512;
double input;
double output;
double p = 0.5;
double i = 0.2;
double d = 0.1;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
myController.begin(&input, &output, &setpoint, p, i, d);
// myController.reverse() // Uncomment if controller output is "reversed"
// myController.setSampleTime(10); // OPTIONAL - will ensure at least 10ms have past between successful compute() calls
myController.setOutputLimits(-30, 30);
//myController.setBias(255.0 / 2.0);
myController.setWindUpLimits(-10, 10); // Groth bounds for the integral term to prevent integral wind-up
myController.start();
}
void loop() {
// put your main code here, to run repeatedly:
input = analogRead(A0); // Replace with sensor feedback
myController.compute();
myController.debug(&Serial, "myController", PRINT_INPUT | // Can include or comment out any of these terms to print
PRINT_OUTPUT | // in the Serial plotter
PRINT_SETPOINT |
PRINT_BIAS |
PRINT_P |
PRINT_I |
PRINT_D);
analogWrite(3, output); // Replace with plant control signal
}
Also here is the function code that I found in the library:
#include "ArduPID.h"
void ArduPID::begin(double* _input,
double* _output,
double* _setpoint,
const double& _p,
const double& _i,
const double& _d,
const pOn& _pOn,
const dir& _direction,
const unsigned int& _minSamplePeriodMs,
const double& _bias)
{
input = _input;
output = _output;
setpoint = _setpoint;
setCoefficients(_p, _i, _d);
setPOn(_pOn);
setBias(_bias);
setDirection(_direction);
setSampleTime(_minSamplePeriodMs);
reset();
start();
}
void ArduPID::start()
{
if (modeType != ON)
{
modeType = ON;
reset();
}
}
void ArduPID::reset()
{
curError = 0;
curSetpoint = 0;
curInput = 0;
lastError = 0;
lastSetpoint = 0;
lastInput = 0;
pOut = 0;
iOut = 0;
dOut = 0;
timer.start();
}
void ArduPID::stop()
{
if (modeType != OFF)
modeType = OFF;
}
void ArduPID::compute()
{
if (timer.fire() && modeType == ON)
{
kp = pIn;
ki = iIn * (timer.timeDiff / 1000.0);
kd = dIn / (timer.timeDiff / 1000.0);
if (direction == BACKWARD)
{
kp *= -1;
ki *= -1;
kd *= -1;
}
lastInput = curInput;
lastSetpoint = curSetpoint;
lastError = curError;
curInput = *input;
curSetpoint = *setpoint;
curError = curSetpoint - curInput;
double dInput = *input - lastInput;
if (pOnType == P_ON_E)
pOut = kp * curError;
else if (pOnType == P_ON_M)
pOut = -kp * dInput;
iOut += (ki * ((curError + lastError) / 2.0)); // Trapezoidal integration
iOut = constrain(iOut, windupMin, windupMax); // Prevent integral windup
dOut = -kd * dInput; // Derrivative on measurement
double newOutput = bias + pOut + iOut + dOut;
newOutput = constrain(newOutput, outputMin, outputMax);
*output = newOutput;
}
}
void ArduPID::setOutputLimits(const double& min, const double& max)
{
if (max > min)
{
outputMax = max;
outputMin = min;
if (modeType == ON)
*output = constrain(*output, outputMin, outputMax);
}
}
void ArduPID::setWindUpLimits(const double& min, const double& max)
{
if (max > min)
{
windupMax = max;
windupMin = min;
}
}
void ArduPID::setDeadBand(const double& min, const double& max)
{
if (max >= min)
{
deadBandMax = max;
deadBandMin = min;
}
}
void ArduPID::setPOn(const pOn& _pOn)
{
pOnType = _pOn;
}
void ArduPID::setBias(const double& _bias)
{
bias = _bias;
}
void ArduPID::setCoefficients(const double& _p, const double& _i, const double& _d)
{
if (_p >= 0 && _i >= 0 && _d >= 0)
{
pIn = _p;
iIn = _i;
dIn = _d;
}
}
void ArduPID::setDirection(const dir& _direction)
{
direction = _direction;
if (modeType == ON)
reset();
}
void ArduPID::reverse()
{
if (direction == FORWARD)
direction = BACKWARD;
else if (direction == BACKWARD)
direction = FORWARD;
if (modeType == ON)
reset();
}
void ArduPID::setSampleTime(const unsigned int& _minSamplePeriodMs)
{
timer.begin(_minSamplePeriodMs);
}
double ArduPID::B()
{
return bias;
}
double ArduPID::P()
{
return pOut;
}
double ArduPID::I()
{
return iOut;
}
double ArduPID::D()
{
return dOut;
}
void ArduPID::debug(Stream* stream, const char* controllerName, const byte& mask)
{
if (mask & PRINT_INPUT)
{
stream->print(controllerName);
stream->print("_input ");
}
if (mask & PRINT_OUTPUT)
{
stream->print(controllerName);
stream->print("_output ");
}
if (mask & PRINT_SETPOINT)
{
stream->print(controllerName);
stream->print("_setpoint ");
}
if (mask & PRINT_BIAS)
{
stream->print(controllerName);
stream->print("_bias ");
}
if (mask & PRINT_P)
{
stream->print(controllerName);
stream->print("_P ");
}
if (mask & PRINT_I)
{
stream->print(controllerName);
stream->print("_I ");
}
if (mask & PRINT_D)
{
stream->print(controllerName);
stream->print("_D ");
}
stream->println();
if (mask & PRINT_INPUT)
{
stream->print(*input);
stream->print(" ");
}
if (mask & PRINT_OUTPUT)
{
stream->print(*output);
stream->print(" ");
}
if (mask & PRINT_SETPOINT)
{
stream->print(*setpoint);
stream->print(" ");
}
if (mask & PRINT_BIAS)
{
stream->print(bias);
stream->print(" ");
}
if (mask & PRINT_P)
{
stream->print(pOut);
stream->print(" ");
}
if (mask & PRINT_I)
{
stream->print(iOut);
stream->print(" ");
}
if (mask & PRINT_D)
{
stream->print(dOut);
stream->print(" ");
}
stream->println();
}
Any help would be greatly appreciated!