Drone PID output

DaveX · April 19, 2023, 2:28pm

A tether is good.

crownous · April 19, 2023, 3:34pm

I wish that was the problem. I keep getting some extremely high values and for example the roll and pitch values on the controller (two potentiometers) are as one value in the drone (I moved both pots, but only one number changes). The same situation sets for altitude and yaw. What happens with the camera is that when I press the button on the controller, instead of writing a 1 to the bool in the drone, it writes a nonsensical value to rollInput. Any ideas?

crownous · April 19, 2023, 3:36pm

Thank you for your advice. I've already made the tuning stand, so testing is relatively safe.

johnwasser · April 19, 2023, 9:16pm

A programming mistake seems likely. Hard to tell without seeing the code.

crownous · April 19, 2023, 9:26pm

This is simplified code:

//TRANSMITTER
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>

int osax1 = A0;
int osay1 = A1;
int osax2 = A2;
int osay2 = A3;

RF24 radio(10, 9); // CE, CSN
const byte address[10] = "sjSEmD3xK";



int data[10];

void setup() {
  Serial.begin(9600);
  pinMode(osax1, INPUT);
  pinMode(osay1, INPUT);
  pinMode(osax2, INPUT);
  pinMode(osay2, INPUT);
  radio.begin();
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();
}

void loop() {
  data[0] = analogRead(osax1); // yaw
  data[1]  = analogRead(osay1); // altitude
  data[2]  = analogRead(osax2) ; // roll
  data[3]  = analogRead(osay2) ;// pitch

  Serial.print("yaw");
  Serial.println(data[0]);
  Serial.print("altitude");
  Serial.println(data[1]);
  Serial.print("roll");
  Serial.println(data[2]);
  Serial.print("pitch");
  Serial.println(data[3]);
  radio.write(&data, sizeof(data));
}

//RECEIVER
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>

int axis[4];

RF24 radio(5, 4); // CE, CSN
const byte address[10] = "sjSEmD3xK";

void setup() {
  Serial.begin(115200);
  radio.begin();
  radio.openReadingPipe(0, address);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();
};

void loop() {
  if (radio.available()) {
    radio.read(&axis, sizeof(axis));
    for (int i = 0; i < 4; i++) {
    Serial.println(axis[i]);
    }
  }
}

Receiver output (all potentiometers have non-zero values at this point):

johnwasser · April 19, 2023, 9:44pm

Looks to me like you are sending 20 (or 40) bytes and reading 8 (or 16). That looks wrong to me. From the output it appears that your receiver is using 32-bit integers and your transmitter is sending 16-bit integers.

49086465 -> 0x02ED0001 -> 0x02ED 0x0001 -> 749, 1
2687482 -> 0x002901FA -> 0x0029 0x01FA -> 41, 506
0
0

The byte order may also be different between the two.

crownous · April 20, 2023, 8:41am

Thank you, I updated my code according to your post and it works!

crownous · April 20, 2023, 11:00pm

Hi, is there a way to speed up my code? On esp32 it now takes about 36 milliseconds, which is really similar to ATmega328 and the speed was the reason I upgraded. I've read on the internet that changing algorithms or libraries can help, but I have a good feeling about my code and would like to increase the speed otherwise than by changing it. My current code is:

#include<Wire.h>
#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>
#include <BME280I2C.h>
#include <ESP32Servo.h>

#define esc1 33 // right back 
#define esc2 32// right front
#define esc3 25 // left back 
#define esc4 26// feft front
// 2
Servo escLF;
Servo escRF;
Servo escRB;
Servo escLB;

BME280I2C bme;
const int MPU_addr = 0x68;
RF24 radio(5, 4);
const uint64_t address[] = {0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL};

double seaLevelPres = 1013.25;
bool camera = false;

// radio input
short data[6];
short altitude;
//sensors
double pitchInput, rollInput, yawInput, altitudeInput;
int counter = 0;
//IMU
double xAcc, yAcc, zAcc, xGyro, yGyro, zGyro;
double currentGyroMillis, previousGyroMillis, deltaGyroTime;
double pitchInputAcc, rollInputAcc, yawInputAcc;
double pitchInputGyro, rollInputGyro, yawInputGyro;
double rollGyroOffset, pitchGyroOffset, yawGyroOffset, rollAccOffset, pitchAccOffset, yawAccOffset;
//kalman's altitude filter
double altitudeInputOffset;
const double QTemp = 0.91; // Procesní šum
const float RTemp = 0.125;// Měřící šum
float xTemp = 0;  // Stavová proměnná
float pTemp = 1;  // Kovariance stavu
float kTemp;// Kalmanovo zesílení
const float QPres = 0.043; // Procesní šum
const float RPres = 0.007;// Měřící šum
float xPres = 0;  // Stavová proměnná
float pPres = 1;  // Kovariance stavu
float kPres;      // Kalmanovo zesílení
//pid
double pitchKp = 1.4, pitchKi = 0.0000002 , pitchKd = 0.05;
double rollKp = 1.5, rollKi = 0.0000002, rollKd = 0.05;
double yawKp = 0.12, yawKi = 0.0000005, yawKd = 0.052;
double altitudeKp = 0.99, altitudeKi = 0.0000001, altitudeKd = 0.053;
double pitchSetpoint = 0, rollSetpoint = 0, yawSetpoint = 0, altitudeSetpoint = 0;  //radio transmitter values
double pitchLastError, rollLastError, yawLastError, altitudeLastError;
double pitchIntegral, rollIntegral, yawIntegral, altitudeIntegral;
double pitchOutput, rollOutput, yawOutput, altitudeOutput;
unsigned long previousMillis;
unsigned long  currentMillis;
const unsigned long interval = 35;
//motor speeds
double sp1 = 0, sp2 = 0, sp3 = 0, sp4 = 0;
//functions initialization
void InitInput();
void GetRadio(double& yawSetpoint, double& rollSetpoint, double& pitchSetpoint, double& altitudeSetpoint, double& seaLevelPres, bool& camera);
void ReadSensors(double& pitchInput, double& rollInput, double& yawInput, double& altitudeInput);
double CalculatePID(double Input, double Kp, double Ki,  double Kd, double Setpoint, double& LastError, double& Integral);
void ControlMotors();

void setup() {
  delay(3000);
  InitInput();
}

void loop() {
  GetRadio(yawSetpoint, rollSetpoint, pitchSetpoint, altitudeSetpoint, seaLevelPres, camera);
  ReadSensors(pitchInput, rollInput, yawInput, altitudeInput);
  currentMillis = millis();
  if ((currentMillis - previousMillis) > interval)
  {
    Serial.println((currentMillis - previousMillis));
    previousMillis = currentMillis;
    pitchOutput = CalculatePID(pitchInput, pitchKp, pitchKi, pitchKd, pitchSetpoint, pitchLastError, pitchIntegral);
    rollOutput = CalculatePID(rollInput, rollKp, rollKi, rollKd, rollSetpoint, rollLastError, rollIntegral);
    yawOutput = CalculatePID(yawInput, yawKp, yawKi, yawKd, yawSetpoint, yawLastError, yawIntegral);
    altitudeOutput = CalculatePID(altitudeInput, altitudeKp, altitudeKi, altitudeKd, altitudeSetpoint, altitudeLastError, altitudeIntegral);
  }

  /*Serial.println(pitchOutput);
    Serial.println(rollOutput);
    Serial.println(yawOutput);
    Serial.println(altitudeOutput);
    Serial.print("Altitude sea level pressure");
    Serial.println(seaLevelPres);
    Serial.print("altitudeSetpoint");
    Serial.println(altitudeSetpoint);
    Serial.print("pitchSetpoint");
    Serial.println(pitchSetpoint);
    Serial.print("rollSetpoint");
    Serial.println(rollSetpoint);
    Serial.print("yawSetpoint");
    Serial.println(yawSetpoint);
    Serial.print("Pitch");
    Serial.println(pitchInput);
    Serial.print("Roll");
    Serial.println(rollInput);
    Serial.print("Yaw");
    Serial.println(yawInput);
    Serial.print("Altitude");
    Serial.println(altitudeInput);
    Serial.print("Altitude Offset");
    Serial.println(altitudeInputOffset);*/

  ControlMotors();

}

double CalculatePID(double Input, double Kp, double Ki,  double Kd, double Setpoint, double &LastError, double &Integral) {

  double error = Setpoint - Input;
  //Serial.println(error);
  Integral += error * interval;
  double derivative = (error - LastError) / interval;
  double Output = Kp * error + Ki * Integral + Kd * derivative;
  LastError = error;

  return Output;
}

void ControlMotors() {
  sp1 = rollOutput + pitchOutput + altitudeOutput + yawOutput;
  sp2 = rollOutput + -pitchOutput + altitudeOutput - yawOutput;
  sp3 = -rollOutput + pitchOutput + altitudeOutput - yawOutput;
  sp4 = -rollOutput - pitchOutput + altitudeOutput + yawOutput;

  sp1 = constrain(sp1, 0, 360) / 2;
  sp2 = constrain(sp2, 0, 360) / 2;
  sp3 = constrain(sp3, 0, 360) / 2;
  sp4 = constrain(sp4, 0, 360) / 2;

  /*Serial.print("left front");
    Serial.println(sp1);
    Serial.print("ritght front");
    Serial.println(sp2);
    Serial.print("right backt");
    Serial.println(sp3);
    Serial.print("left back");
    Serial.println(sp4);*/


  escLF.write(sp1);
  escRF.write(sp2);
  escRB.write(sp3);
  escLB.write(sp4);
}

void GetRadio(double& yawSetpoint, double& rollSetpoint, double& pitchSetpoint, double& altitudeSetpoint, double& seaLevelPres, bool& camera) {
  if (radio.available())  {
    radio.read(&data, sizeof(data));
    yawSetpoint = data[0];
    altitudeSetpoint = data[1];
    rollSetpoint = data[2];
    pitchSetpoint = data[3];
    seaLevelPres = data[4];
    camera = data[5];
  }
  radio.stopListening();
  radio.openWritingPipe(address[1]);
  altitude = int(altitudeInput);
  radio.write(&altitude, sizeof(altitude));
  radio.openReadingPipe(1, address[0]);
  radio.startListening();
}

void InitInput() {
  Serial.begin(115200);
  radio.begin();
  Wire.begin();
  bme.begin();

  escLF.attach(esc1, 1000, 2000);
  escRF.attach(esc2, 1000, 2000);
  escRB.attach(esc3, 1000, 2000);
  escLB.attach(esc4, 1000, 2000);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  //gyro config
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B);
  Wire.write(0x10);
  Wire.endTransmission(true);
  //accelometer config
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission(true);
  radio.openWritingPipe(address[1]);
  radio.openReadingPipe(1, address[0]);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();
  currentGyroMillis = millis();
  if (rollAccOffset == 0) {
    for (int i = 0; i < 200; i++) {

      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr, 6, true);

      xAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;
      yAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;
      zAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;

      pitchAccOffset += (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);
      rollAccOffset += (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG);

      if (i == 199) {
        rollAccOffset = rollAccOffset /  200;
        pitchAccOffset = pitchAccOffset / 200;
      }
    }
  }

  if (rollGyroOffset == 0) {
    for (int i = 0; i < 200; i++) {

      Wire.beginTransmission(MPU_addr);
      Wire.write(0x43);
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr, 6, true);

      xGyro = (int16_t(Wire.read() << 8 | Wire.read()));
      yGyro = (int16_t(Wire.read() << 8 | Wire.read()));
      zGyro = (int16_t(Wire.read() << 8 | Wire.read()));

      rollGyroOffset += yGyro / 32.8 ;
      pitchGyroOffset += xGyro / 32.8;
      yawGyroOffset += zGyro / 32.8;
      if (i == 199) {
        rollGyroOffset = rollGyroOffset / 200;
        pitchGyroOffset = pitchGyroOffset / 200;
        yawGyroOffset = yawGyroOffset / 200;
      }
    }
  }
}

void ReadSensors(double& pitchInput, double& rollInput, double& yawInput, double& altitudeInput) {

  //IMU

  previousGyroMillis = currentGyroMillis;
  currentGyroMillis = millis();
  deltaGyroTime = (currentGyroMillis - previousGyroMillis) / 1000;

  //gyro

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xGyro = (int16_t(Wire.read() << 8 | Wire.read()));
  yGyro = (int16_t(Wire.read() << 8 | Wire.read()));
  zGyro = (int16_t(Wire.read() << 8 | Wire.read()));


  xGyro = (xGyro / 32.8) - pitchGyroOffset;
  yGyro = (yGyro / 32.8) - rollGyroOffset;
  zGyro = (zGyro / 32.8) - yawGyroOffset;

  pitchInputGyro = xGyro * deltaGyroTime ;
  rollInputGyro = yGyro * deltaGyroTime;
  yawInputGyro = zGyro * deltaGyroTime;

  //accelometer

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 6, true);
  xAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;
  yAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;
  zAcc = (int16_t(Wire.read() << 8 | Wire.read())) / 4096.0 ;

  pitchInputAcc = (atan((yAcc) / sqrt(pow((xAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - pitchAccOffset;
  rollInputAcc = (atan(-1 * (xAcc) / sqrt(pow((yAcc), 2) + pow((zAcc), 2))) * RAD_TO_DEG) - rollAccOffset;

  //complementary filter

  rollInput = 0.98 * (rollInput + rollInputGyro) + 0.02 * (rollInputAcc);
  pitchInput = 0.98 * (pitchInput + pitchInputGyro) + 0.02 * (pitchInputAcc);
  yawInputAcc = atan2((sin(rollInput) * cos(pitchInput) * xAcc + sin(pitchInput) * yAcc + cos(rollInput) * cos(pitchInput) * zAcc), sqrt(pow(sin(rollInput) * sin(pitchInput) * xAcc - cos(rollInput) * sin(pitchInput) * zAcc, 2) + pow(cos(pitchInput) * xAcc, 2))) - 1;
  yawInput = 0.98 * (yawInput + yawInputGyro) + 0.02 * (yawInputAcc);

  //altitudemeter

  //bmp280
  float temp(NAN), hum(NAN), pres(NAN);
  BME280::TempUnit tempUnit(BME280::TempUnit_Celsius);
  BME280::PresUnit presUnit(BME280::PresUnit_Pa);
  bme.read(pres, temp, hum, tempUnit, presUnit);
  pres = pres / 100;
  counter += 1;
  //kalmans filter
  float xPredTemp = xTemp;
  float pPredTemp = pTemp + QTemp;
  float xPredPres = xPres;
  float pPredPres = pPres + QPres;
  // Výpočet Kalmanova zesílení
  kTemp = pPredTemp  / (pPredTemp  + RTemp);
  kPres = pPredPres  / (pPredPres  + RPres);
  // Korekce stavu
  xTemp = xPredTemp  + kTemp * (temp - xPredTemp );
  xPres = xPredPres  + kPres * (pres - xPredPres );
  // Aktualizace kovariance stavu
  pTemp = (1 - kTemp) * pPredTemp ;
  pPres = (1 - kPres) * pPredPres ;
  altitudeInput = (((pow((1013.15 /  xPres), 1 / 5.257) - 1) * (xTemp  + 273.15)) / 0.0065);
  if (altitudeInputOffset == 0 && counter == 100) {
    altitudeInputOffset = altitudeInput;
  }
  altitudeInput -= altitudeInputOffset;
  altitudeInput = constrain(altitudeInput, 0, 1000);
}

DaveX · April 21, 2023, 2:32am

Well, a simplistic answer looks like reducing interval to less than 35 might work:

A different option might be to split the 4 CalculatePID calls into separate, staggered intervals, or to sequence through them.

Since the 4 PID calculations are all done at one time, once every 36ms, it doesn't make much sense to GetRadio, ReadSensors, and ControlMotors more often than the calculation-- only the last GetRadio and ReadSensors before the calculations matter, and you only need to update the motors once after the calculation. You could split the 36ms (or maybe a shorter time) into 7 separate tasks and do them on different slices.

Do you know how long each task takes?

crownous · April 22, 2023, 12:04pm

I'd like to, but that's the lowest value the loop takes.

I checked the times for each function and found that GetRadio takes 32 ms.
Results:

ReadSensors = 5ms
43, 44, 50,10234, 44 micros - 4x CalculatePID < 1ms
108, 100, 99, 10293, 100 micros - ControlMotors < 1ms
GetRadio = 32ms

DaveX · April 22, 2023, 1:50pm

That’s interesting

I thing the way forward would be to poll the radios less often and do the sensor-calc-motor update more frequently. If you read the radios every 100ms, you could move the control loop down to 10ms.

How fast is the physics of your system?

crownous · April 22, 2023, 1:55pm

That would be great, but wouldn't that be too much? I read on the internet that the speed shouldn't be that high either. I was thinking about 20ms.

I'm sorry, but I don't understand the question.

DaveX · April 22, 2023, 2:00pm

The 20ms from the collective wisdom of the internet is a good answer to my physics question. Use 20ms

crownous · April 22, 2023, 2:11pm

Okay, thanks. I've also read about the limitation of the integral part in PID. My code after modification looks like this:

double CalculatePID(double Input, double Kp, double Ki,  double Kd, double Setpoint, double &LastError, double &Integral, double IntegralLimit) {

  double error = Setpoint - Input;
  //Serial.println(error);
  Integral += error * interval;
  //Serial.println(Integral);
  Integral = constrain(Integral, -IntegralLimit, IntegralLimit);
  double derivative = (error - LastError);
  double Output = Kp * error + Ki * Integral + Kd * derivative;
  LastError = error;

  return Output;
}

I'm not sure how big the IntegralLimit should be. Is there some rule about percentages of integral or something?

DaveX · April 22, 2023, 3:43pm

Limiting the integral is done to minimize integral windup that ends up being un-wound with oscillations over the setpoint.

One way people do it is to multiply it by Ki to get it into output units before integrating, and limit the result to the output limits, that method makes the limit adaptive to changes in the parameters. The Arduino PID_v1 library uses this and explains it well in

http://brettbeauregard.com/blog/2011/04/improving-the-beginner’s-pid-reset-windup/

I'm partial to the conditional integration and back-calculation schemes, where the limit is related to the other components-- If the proportional term Kp*error is enough to get you to the output limits, you shouldn't bank integral signal for later.

github.com/br3ttb/Arduino-PID-Library

Backcalculate integral sum into feasible range

br3ttb:master ← drf5n:backcalculation

opened 06:58PM - 22 Nov 21 UTC

drf5n

+146 -5

Remove excess integral windup per Astrom, K.J. and Rundqwist, L. (1989). "Inte…grator windup and how to avoid it" (PDF). Proc. 1989 Am. Control Confi.: 1693–1698. through using code by Will posted in Brett Beauregard's forum at http://brettbeauregard.com/blog/2011/04/improving-the-beginner%e2%80%99s-pid-reset-windup/#comment-18721

or

and

github.com/br3ttb/Arduino-PID-Library

Expose integral to enable user-space hacking

br3ttb:master ← drf5n:exposeIntegralPublic

opened 03:03PM - 03 Mar 23 UTC

drf5n

+3 -2

Based on the discussion in https://github.com/br3ttb/Arduino-PID-Library/pull/13…2#issuecomment-1452803503 I realized that a low impact way of handling the issues would be to just move some features out of the black box. Moving the `PID::Initialize()` function out to public makes the `SetMode(MANUAL);Output=value;SetMode(MANUAL);` trick easier to discuss and handle. Moving the `PID::outputSum` variable from private to public makes it possible to examine (or hack on) the internal integrator state. Since these functions already exist, it should be a no-cost change and enables easy understanding of what the algorithm does and easy adaptation to special uses. A live example: https://wokwi.com/projects/358190033668210689 <img width="535" alt="image" src="https://user-images.githubusercontent.com/2236516/224490235-28ca4d88-94a2-4561-870d-233bcaafffcd.png"> This minimal-impact change would obviate https://github.com/br3ttb/Arduino-PID-Library/pull/132 ``` /******************************************************** PID Basic simulated heater Example Reading analog input 0 to control analog PWM output 3 ********************************************************/ // This simulates a 20W heater block driven by the PID // Vary the setpoint with the Pot, and watch the heater drive the temperature up // // Simulation at https://wokwi.com/projects/358122536159671297 // // Based on // Wokwi https://wokwi.com/projects/357374218559137793 // Wokwi https://wokwi.com/projects/356437164264235009 #include "PID_v1.h" // https://github.com/br3ttb/Arduino-PID-Library // local copy of .h and .cpp are tweaked to expose the integral per // https://github.com/br3ttb/Arduino-PID-Library/pull/132#issuecomment-1453839425 //Define Variables we'll be connecting to double Setpoint, Input, Output; //Specify the links and initial tuning parameters double Kp = 17, Ki = .1, Kd = 2; // works reasonably with sim heater block //double Kp = 255, Ki = .0, Kd = 0; // works reasonably with sim heater block //double Kp = 255, Ki = 0.0, Kd = 0.0; // bang-bang //double Kp = 2, Ki = 0.0, Kd = 0.0; // P-only //double Kp = 2, Ki = 5, Kd = 1; // commonly used defaults PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, P_ON_E, DIRECT); const int PWM_PIN = 3; // UNO PWM pin const int INPUT_PIN = -1; // Analog pin for Input (set <0 for simulation) const int SETPOINT_PIN = A1; // Analog pin for Setpoint Potentiometer const int SETPOINT_INDICATOR = 6; // PWM pin for indicating setpoint const int INPUT_INDICATOR = 5; // PWM pin for indicating Input const int AUTOMATIC_PIN = 8; // Pin for controlling manual/auto mode const int OVERRIDE_PIN = 12; // Pin for integral override void setup() { Serial.begin(115200); Serial.println(__FILE__); myPID.SetOutputLimits(-4, 255); pinMode(OVERRIDE_PIN, INPUT_PULLUP); pinMode(AUTOMATIC_PIN, INPUT_PULLUP); if (SETPOINT_INDICATOR >= 0) pinMode(SETPOINT_INDICATOR, OUTPUT); if (INPUT_INDICATOR >= 0) pinMode(INPUT_INDICATOR, OUTPUT); Setpoint = 0; //turn the PID on myPID.SetMode(AUTOMATIC); if(INPUT_PIN>0){ Input = analogRead(INPUT_PIN); }else{ Input = simPlant(0.0,1.0); // simulate heating } Serial.println("Setpoint Input Output Integral"); } void loop() { // gather Input from INPUT_PIN or simulated block float heaterWatts = (int)Output * 20.0 / 255; // 20W heater if (INPUT_PIN > 0 ) { Input = analogRead(INPUT_PIN); } else { float blockTemp = simPlant(heaterWatts,Output>0?1.0:1-Output); // simulate heating Input = blockTemp; // read input from simulated heater block } if (myPID.Compute()) { analogWrite(PWM_PIN, (int)Output); Setpoint = analogRead(SETPOINT_PIN) / 4; // Read setpoint from potentiometer if (INPUT_INDICATOR >= 0) analogWrite(INPUT_INDICATOR, Input); if (SETPOINT_INDICATOR >= 0) analogWrite(SETPOINT_INDICATOR, Setpoint); } if(digitalRead(OVERRIDE_PIN)==LOW) mySetIntegral(&myPID,0); // integral override if(digitalRead(AUTOMATIC_PIN)==HIGH != myPID.GetMode()==AUTOMATIC){ myPID.SetMode(digitalRead(AUTOMATIC_PIN)==HIGH ? AUTOMATIC :MANUAL); } report(); } void report(void) { static uint32_t last = 0; const int interval = 1000; if (millis() - last > interval) { last += interval; // Serial.print(millis()/1000.0); Serial.print("SP:");Serial.print(Setpoint); Serial.print(" PV:"); Serial.print(Input); Serial.print(" CV:"); Serial.print(Output); Serial.print(" Int:"); Serial.print(myPID.GetIntegral()); Serial.print(' '); Serial.println(); } } float simPlant(float Q,float hfactor) { // heat input in W (or J/s) // simulate a 1x1x2cm aluminum block with a heater and passive ambient cooling // float C = 237; // W/mK thermal conduction coefficient for Al float h = 5 *hfactor ; // W/m2K thermal convection coefficient for Al passive float Cps = 0.89; // J/g°C float area = 1e-4; // m2 area for convection float mass = 10 ; // g float Tamb = 25; // °C static float T = Tamb; // °C static uint32_t last = 0; uint32_t interval = 100; // ms if (millis() - last >= interval) { last += interval; // 0-dimensional heat transfer T = T + Q * interval / 1000 / mass / Cps - (T - Tamb) * area * h; } return T; } void mySetIntegral(PID * ptrPID,double value ){ ptrPID->SetMode(MANUAL); Output = value; ptrPID->SetMode(AUTOMATIC); } ```

crownous · April 24, 2023, 9:47am

So the best/easiest solution for my problem would be using PID_v1 library?

DaveX · April 24, 2023, 10:49am

Not necessarily. You’ve written lots of a pid. I was trying to answer your q on how to limit the integral term.

With the right tuning parameters, the system might not have a windup problem.

crownous · April 24, 2023, 1:05pm

Okay, thanks for your help.

crownous · April 30, 2023, 7:10am

I would like to add a voltage divider to my drone to measure the battery voltage (fully charged voltage is 12.4 V). This is my first time using a voltage divider, so I would like to make sure that the R1= 1M ohm and R2=330k ohm values I calculated are ok for the ESP32.

DaveX · April 30, 2023, 1:32pm

Schematic?

The output would be either

12.4*1000/(1000+330)

Or

 12.4*330/(1000+330)

…Depending on the schematic.

Topic		Replies	Views
Multiwii (PID). Quad X FC (Arduino + MPU6050) Software	17	262	December 30, 2025
Two wheel balncing robot with adxl345 General Guidance	68	510	March 22, 2026
Quadcopter PID tuning self balancing problems HELP Programming	16	391	October 5, 2025
PID autotuning code, fuzzy logic, Datalogger system heater chauffant 4-20mA Réalisations et Projets Finis	12	373	March 3, 2026
PID control of DC motor Motors, Mechanics, Power and CNC	26	1753	January 10, 2025

Drone PID output

Related topics