Advice regarding an issue with PID outputs on X and Y axis for a Thrust Vector Control

Projects Programming
1

Hi guys, I'm triyng to write a code for my flight computer in order to build a thrust vector control for my model rocket.I have encountered a challenge with the PID output values, specifically the variables named pidx and pidy on the code below. Despite having distinct values for errorX and errorY, the output results from the PID are identical. The system is designed to measure pitch and roll using the Adafruit BNO055 sensor, connected through STEMMA connectors.
I am uncertain whether this discrepancy is due to a conceptual error or if there's an issue in my implementation. I believe you could provide valuable insights into this matter. I would be very grateful for any suggestion or help. (I'm still learning everyday something new about PID and control system so probably I'm doing something wrong).

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>
#include <math.h>

//#define  RESTRICT_PITCH 60
Adafruit_BNO055 bno = Adafruit_BNO055();

int targetX = 0; //90
int targetY = 0;
int max_angle = 10;
int min_angle = -10;
float errorX, errorY, pidx,pidy;

Servo servoX;
Servo servoY;

//PID parameters 
unsigned long lastTime;
double   lastInput;
float output;
float kp;
float ki;
float kd;
float smoothedOutputX = 0;  //  smoothing term
float smoothedOutputY = 0;  // smoothing term
float smoothingFactor = 0.2;  //smoothing factor(0.0 - 1.0)
int SampleTime = 5;

void setup() {
Serial.begin(115200);
  if (!bno.begin()) {
    Serial.println("Error in BNO055, check connectrions!");
    while (1);
  }
  servoX.attach(10);
  servoY.attach(9);
  servoX.write(90);
  servoY.write(90);
}

void loop() {
  /* sensors_event_t accelEvent;
  bno.getEvent(&accelEvent);
  int accX = accelEvent.acceleration.x;
  int accY = accelEvent.acceleration.y;
  int accZ = accelEvent.acceleration.z;
  #ifdef RESTRICT_PITCH // Eq. 25 and 26
  double roll  = atan2(accY, accZ) * RAD_TO_DEG;
  double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ))*RAD_TO_DEG;
#else // Eq. 28 and 29
  double roll  = atan(accY / sqrt(accX * accX + accZ * accZ))*RAD_TO_DEG;
  double pitch = (atan2(-accX, accZ) * RAD_TO_DEG);
#endif
*/
sensors_event_t event;
  bno.getEvent(&event);
  float pitch = event.orientation.y;
  float roll = event.orientation.x;
roll = map(roll,0,-50, 0,50);
pitch = map(pitch,0,-50, 0,50);
errorX = targetX-roll;
errorY = targetY-pitch;
SetTunings(0.005,0.02,0.05); //0.005,0.02,0.05
pidx = PID(errorX,roll);
pidy= PID(errorY,pitch);


 // if (abs(pidx) > 1) {  // Only update servos if there is a significant change
  // smoothedOutputX = smoothingFactor * pidx + (1 - smoothingFactor) * smoothedOutputX;
    servoX.write(90 - pidx);
  //}
//if( abs(pidy) > 1)
//{
 //smoothedOutputY = smoothingFactor * pidy + (1 - smoothingFactor) * smoothedOutputY;
 servoY.write(90 - pidy);
//}
  Serial.print("\nroll: ");
  Serial.println(roll);
  Serial.print("\nPitch: ");
  Serial.println(pitch);
    Serial.print("\npidx: ");
  Serial.println(pidx);
  Serial.print("\npidy: ");
  Serial.println(pidy);
  Serial.print("\nerrorX: ");
Serial.println(errorX);
 Serial.print("\nerrorY: ");
Serial.println(errorY);
 Serial.print("\targetX ");
Serial.println(targetX);
  delay(10);
}

double PID(float error, float input)
{
unsigned long now = millis();
int dt = (now - lastTime);
if(dt >= SampleTime)
{
float p_error = error * kp;

  float i_error = 0;
 i_error += error*ki;
if(i_error > max_angle) i_error = max_angle;
else if(i_error < min_angle) i_error = min_angle;

float d_error = (lastInput-input)*kd;

 output = p_error + i_error - d_error;
  if(output > max_angle) output = max_angle;
  else if(output< min_angle ) output = min_angle;


lastInput = input;
lastTime = now;

}
return output;
}

void SetTunings(double Kp, double Ki, double Kd)
{
  double SampleTimeInSec = ((double)SampleTime)/1000;
  kp = Kp;
  ki = Ki*SampleTimeInSec;
  kd = Kd/SampleTimeInSec;
}
2

won't work for two different input variables, since "last error" (the "D" part) and "accumulated error" (the "I" part) are stored and used in the same way for each one.

Incidentally, this is wrong. i_error must be declared static and not re-initialized every pass.

  float i_error = 0;
 i_error += error*ki;
2 Likes
3

It is because the two PIDs share the same i_error and last_input persistent variables, blending them across both axes.

With these tunings, the blended integral and derivative terms provide most of the output.

1 Like
4

Than I must declare them outside the function am I right?

5

No, you can declare them static, inside the function. You can't use the same accumulated values for different variables.

  static float i_error = 0;  //initialized only once at startup
6

Ok thank you so much also to @DaveX, I've basically declared lastTime and lastInput for X and Y separately.

unsigned long lastTimeX, lastTimeY;
double lastInputX, lastInputY;
//code
///
//
double PID(float error, float input, unsigned long &lastTime, double &lastInput, float &i_error, float &output) {
  unsigned long now = millis();
  int dt = (now - lastTime);
  if (dt >= SampleTime) {
    float p_error = error * kp;
    i_error += error * ki;
    if (i_error > max_angle) i_error = max_angle;
    else if (i_error < min_angle) i_error = min_angle;
    float d_error = (lastInput - input) * kd;

    output = p_error + i_error - d_error;
    if (output > max_angle) output = max_angle;
    else if (output < min_angle) output = min_angle;

    lastInput = input;
    lastTime = now;
  }
  return output;
}
///other code
7

Where is i_error declared?

Always post all the code.

8

You're right, this is all the code:


#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <Servo.h>
#include <math.h>


Adafruit_BNO055 bno = Adafruit_BNO055();

int targetX = 0;
int targetY = 0;
int max_angle = 10;
int min_angle = -10;
float errorX, errorY, pidx, pidy;

Servo servoX;
Servo servoY;

unsigned long lastTimeX, lastTimeY;
double lastInputX, lastInputY;
float outputX, outputY;
float kp, ki, kd;
static float i_errorX = 0, i_errorY = 0; // Separate integral terms
float smoothedOutputX = 0;
float smoothedOutputY = 0;
float smoothingFactor = 0.2;
int SampleTime = 5;

void setup() {
  Serial.begin(115200);
  if (!bno.begin()) {
    Serial.println("Errore nel BNO055, controlla le connessioni!");
    while (1);
  }
  servoX.attach(10);
  servoY.attach(9);
  servoX.write(90);
  servoY.write(90);
}

void loop() {
  sensors_event_t event;
  bno.getEvent(&event);
  float pitch = event.orientation.y;
  float roll = event.orientation.z;
  roll = map(roll, 0, -50, 0, 50);
  pitch = map(pitch, 0, -50, 0, 50);
  errorX = targetX - roll;
  errorY = targetY - pitch;

  SetTunings(0.005, 0.02, 0.05);
  pidx = PID(errorX, roll, lastTimeX, lastInputX, i_errorX, outputX);
  pidy = PID(errorY, pitch, lastTimeY, lastInputY, i_errorY, outputY);

  servoX.write(90 - pidx);
  servoY.write(90 - pidy);

  Serial.print("\nroll: ");
  Serial.println(roll);
  Serial.print("\nPitch: ");
  Serial.println(pitch);
  Serial.print("\npidx: ");
  Serial.println(pidx);
  Serial.print("\npidy: ");
  Serial.println(pidy);
  Serial.print("\nerrorX: ");
  Serial.println(errorX);
  Serial.print("\nerrorY: ");
  Serial.println(errorY);
  Serial.print("\targetX ");
  Serial.println(targetX);

  delay(10);
}

double PID(float error, float input, unsigned long &lastTime, double &lastInput,  float &i_error, float &output) {
  unsigned long now = millis();
  int dt = (now - lastTime);
  if (dt >= SampleTime) {
    float p_error = error * kp;
    i_error += error * ki;
    if (i_error > max_angle) i_error = max_angle;
    else if (i_error < min_angle) i_error = min_angle;
    float d_error = (lastInput - input) * kd;

    output = p_error + i_error - d_error;
    if (output > max_angle) output = max_angle;
    else if (output < min_angle) output = min_angle;

    lastInput = input;
    lastTime = now;
  }
  return output;
}

void SetTunings(double Kp, double Ki, double Kd) {
  double SampleTimeInSec = ((double)SampleTime) / 1000;
  kp = Kp;
  ki = Ki * SampleTimeInSec;
  kd = Kd / SampleTimeInSec;
}
9

Now it seems to work.

10

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