Dual input Dual Output PID Controller

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!

Why not 2 PID?

What is your controlled system?

From my understanding, I only see one input variable and don't see how to make another. My controlled system is the y and z angles coming out of the bno055 meant to control two servos.

Make two ArduPID controllers myController1 and myController2?

Yeah I think that should work, I'll give it a shot with my angles later and let you know. Thanks!

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.