PID controller problems

[gooddude546]

Hi I am making an drone using a Arduino Nano ESP32 as the FC.

I have the drone constructed and running this code:

#include <ESP32Servo.h>

#include <Wire.h>
float RatePitch, RateRoll, RateYaw;
float RateCalibrationPitch, RateCalibrationRoll, RateCalibrationYaw;
int RateCalibrationNumber;


uint32_t LoopTimer;
float DesiredRateRoll, DesiredRatePitch, DesiredRateYaw;
float ErrorRateRoll, ErrorRatePitch, ErrorRateYaw;
float InputRoll, InputThrottle, InputPitch, InputYaw;
float PrevErrorRateRoll, PrevErrorRatePitch, PrevErrorRateYaw;
float PrevItermRateRoll, PrevItermRatePitch, PrevItermRateYaw;
float PIDReturn[]={0, 0, 0};

float PRateRoll=0.4 ; float PRatePitch=PRateRoll; float PRateYaw=2;
float IRateRoll=3 ; float IRatePitch=IRateRoll; float IRateYaw=12;
float DRateRoll=0.03 ; float DRatePitch=DRateRoll; float DRateYaw=0;

float MotorInput1, MotorInput2, MotorInput3, MotorInput4;

#include "CRSFforArduino.hpp"
#define USE_SERIAL_PLOTTER 0


Servo ESC1;
Servo ESC2;
Servo ESC3;
Servo ESC4;

int servoPin1 = D9;
int servoPin2 = D10;
int servoPin3 = D11;
int servoPin4 = D12;

int  channel1;
int  channel2;
int  channel3;
int  channel4;

int throttleSafety;
int armHoverSwitch;

CRSFforArduino *crsf = nullptr;

int rcChannelCount = crsfProtocol::RC_CHANNEL_COUNT;
const char *rcChannelNames[] = {
    "A",
    "E",
    "T",
    "R",
    "Aux1",
    "Aux2",
    "Aux3",
    "Aux4",

    "Aux5",
    "Aux6",
    "Aux7",
    "Aux8",
    "Aux9",
    "Aux10",
    "Aux11",
    "Aux12"};

void onReceiveRcChannels(serialReceiverLayer::rcChannels_t *rcChannels);


void gyro_signals(void) {
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();
  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission(); 
  Wire.requestFrom(0x68,6);
  int16_t GyroX=Wire.read()<<8 | Wire.read();
  int16_t GyroY=Wire.read()<<8 | Wire.read();
  int16_t GyroZ=Wire.read()<<8 | Wire.read();
  RateRoll=(float)GyroX/65.5;
  RatePitch=(float)GyroY/65.5;
  RateYaw=(float)GyroZ/65.5;
}
void pid_equation(float Error, float P , float I, float D, float PrevError, float PrevIterm) {
  float Pterm=P*Error;
  float Iterm=PrevIterm+I*(Error+PrevError)*0.004/2;
  if (Iterm > 400) Iterm=400;
  else if (Iterm <-400) Iterm=-400;
  float Dterm=D*(Error-PrevError)/0.004;
  float PIDOutput= Pterm+Iterm+Dterm;
  if (PIDOutput>400) PIDOutput=400;
  else if (PIDOutput <-400) PIDOutput=-400;
  PIDReturn[0]=PIDOutput;
  PIDReturn[1]=Error;
  PIDReturn[2]=Iterm;
}
void reset_pid(void) {
  PrevErrorRateRoll=0; PrevErrorRatePitch=0; PrevErrorRateYaw=0;
  PrevItermRateRoll=0; PrevItermRatePitch=0; PrevItermRateYaw=0;
}
void setup() {
  Serial.begin(115200);

  ESC3.attach(servoPin3,989,2000);
  ESC4.attach(servoPin4,989,2000);
  ESC1.attach(servoPin1,989,2000);
  ESC2.attach(servoPin2,989,2000);

  delay(2500);

  pinMode(5, OUTPUT);
  digitalWrite(5, HIGH);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);   
  Wire.setClock(400000);
  Wire.begin();
  delay(250);
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  for (RateCalibrationNumber=0; RateCalibrationNumber<2000; RateCalibrationNumber ++) {
    gyro_signals();
    RateCalibrationRoll+=RateRoll;
    RateCalibrationPitch+=RatePitch;
    RateCalibrationYaw+=RateYaw;
    delay(1);
  }
  RateCalibrationRoll/=2000;
  RateCalibrationPitch/=2000;
  RateCalibrationYaw/=2000;
  

  crsf = new CRSFforArduino();
    if (!crsf->begin())
    {
        crsf->end();

        delete crsf;
        crsf = nullptr;

        Serial.println("CRSF for Arduino initialisation failed!");
        while (1)
        {
            delay(10);
        }
    }

    rcChannelCount = rcChannelCount > crsfProtocol::RC_CHANNEL_COUNT ? crsfProtocol::RC_CHANNEL_COUNT : rcChannelCount;

    crsf->setRcChannelsCallback(onReceiveRcChannels);

    // Show the user that the sketch is ready.
    Serial.println("RC Channels Example");
    delay(1000);
    Serial.println("Ready");
    delay(1000);


  while (channel3 < 980 || channel3 > 1050) {
    crsf->update();
    delay(4);
  }
  LoopTimer=micros();
}
void loop() {
  gyro_signals();
  RateRoll-=RateCalibrationRoll;
  RatePitch-=RateCalibrationPitch;
  RateYaw-=RateCalibrationYaw;


  crsf->update();

  DesiredRateRoll=0.15*(channel1-1500);
  DesiredRatePitch=0.15*(channel2-1500);
  InputThrottle=channel3;
  DesiredRateYaw=0.15*(channel4-1500);

  ErrorRateRoll=DesiredRateRoll-RateRoll;
  ErrorRatePitch=DesiredRatePitch-RatePitch;
  ErrorRateYaw=DesiredRateYaw-RateYaw;


  pid_equation(ErrorRateRoll, PRateRoll, IRateRoll, DRateRoll, PrevErrorRateRoll, PrevItermRateRoll);
       InputRoll=PIDReturn[0];
       PrevErrorRateRoll=PIDReturn[1]; 
       PrevItermRateRoll=PIDReturn[2];
  pid_equation(ErrorRatePitch, PRatePitch, IRatePitch, DRatePitch, PrevErrorRatePitch, PrevItermRatePitch);
       InputPitch=PIDReturn[0]; 
       PrevErrorRatePitch=PIDReturn[1]; 
       PrevItermRatePitch=PIDReturn[2];
  pid_equation(ErrorRateYaw, PRateYaw,
       IRateYaw, DRateYaw, PrevErrorRateYaw,
       PrevItermRateYaw);
       InputYaw=PIDReturn[0]; 
       PrevErrorRateYaw=PIDReturn[1]; 
       PrevItermRateYaw=PIDReturn[2];
  if (InputThrottle > 1800) InputThrottle = 1800;
  MotorInput1= 1.024*(InputThrottle-InputRoll-InputPitch-InputYaw);
  MotorInput2= 1.024*(InputThrottle-InputRoll+InputPitch+InputYaw);
  MotorInput3= 1.024*(InputThrottle+InputRoll+InputPitch-InputYaw);
  MotorInput4= 1.024*(InputThrottle+InputRoll-InputPitch+InputYaw);
if (MotorInput1 > 2000)MotorInput1 = 1999;
  if (MotorInput2 > 2000)MotorInput2 = 1999; 
  if (MotorInput3 > 2000)MotorInput3 = 1999; 
  if (MotorInput4 > 2000)MotorInput4 = 1999;
  int ThrottleIdle=1180;
  if (MotorInput1 < ThrottleIdle) MotorInput1 =  ThrottleIdle;
  if (MotorInput2 < ThrottleIdle) MotorInput2 =  ThrottleIdle;
  if (MotorInput3 < ThrottleIdle) MotorInput3 =  ThrottleIdle;
  if (MotorInput4 < ThrottleIdle) MotorInput4 =  ThrottleIdle;
  int ThrottleCutOff=1000;
  if (channel3<1050) {
    MotorInput1=ThrottleCutOff; 
    MotorInput2=ThrottleCutOff;
    MotorInput3=ThrottleCutOff; 
    MotorInput4=ThrottleCutOff;
    reset_pid();
  }
  if(throttleSafety == 2000)
  {
  ESC1.write(MotorInput1);
  ESC2.write(MotorInput2);
  ESC3.write(MotorInput3);
  ESC4.write(MotorInput4);
  }
  else
  {
  ESC1.write(989);
  ESC2.write(989);
  ESC3.write(989);
  ESC4.write(989);
  }
  
  while (micros() - LoopTimer < 4000);
  LoopTimer=micros();
}

void onReceiveRcChannels(serialReceiverLayer::rcChannels_t *rcChannels)
{
    if (rcChannels->failsafe == false)
    {
        /* Print RC channels every 100 ms. */
        unsigned long thisTime = millis();
        static unsigned long lastTime = millis();

        /* Compensate for millis() overflow. */
        if (thisTime < lastTime)
        {
            lastTime = thisTime;
        }

        if (thisTime - lastTime >= 100)
        {
            lastTime = thisTime;

            channel2 = (crsf->rcToUs(crsf->getChannel(2)));
            channel1 = (crsf->rcToUs(crsf->getChannel(1)));
            channel4 = (crsf->rcToUs(crsf->getChannel(4)));
            channel3 = (crsf->rcToUs(crsf->getChannel(3)));
          
            channel3 = map(channel3, 989, 2017, 1000, 2000);

            throttleSafety = (crsf->rcToUs(crsf->getChannel(5)));
            armHoverSwitch = (crsf->rcToUs(crsf->getChannel(6)));

            channel2 = map(channel2, 989, 2010, 1000, 2000);
            channel1 = map(channel1, 989, 2010, 1000, 2000);
            channel4 = map(channel4, 994, 2012, 1000, 2000);

            if(channel1 > 2000)
            {
              channel1 = 2000;
            }
            else if(channel1 < 1000)
            {
              channel1 = 1000;
            }

            if(channel2 > 2000)
            {
              channel2 = 2000;
            }
            else if(channel2 < 1000)
            {
              channel2 = 1000;
            }

            if(channel4 > 2000)
            {
              channel4 = 2000;
            }
            else if(channel4 < 1000)
            {
              channel4 = 1000;
            }

            Serial.print("-------------------------------------------------");
            Serial.println();
            Serial.print(channel1);
            Serial.println();
            Serial.print(channel2);
            Serial.println();
            Serial.print(channel3);
            Serial.println();
            Serial.print(channel4);
            Serial.println();
        }
    }
}

It has an ELRS receiver and a gyroscope. For now I copied the code for an PID controller from this video: https://www.youtube.com/watch?v=4vpgjjYizVU&list=PLeuMA6tJBPKsAfRfFuGrEljpBow5hPVD4&index=17

However whenever I try to take of the drone flips. If you want look at the video of what happens:

PID Controller Error.mp4

I know that the values of P, I and D play a big role in the vibrations and stuff, but I have no clue what to set them to.

Any help in resolving this issue is appreciated, because I have no clue for to stabilize the drone

[DaveX]

Looks light a sign, polarity, or orientation error. Are your props on the right way? Is your accelerometer oriented correctly?

[jremington]

@gooddude546

I know that the values of P, I and D play a big role in the vibrations and stuff, but I have no clue what to set them to.

The drone won't work properly until Kp, Ki, and Kd are chosen correctly. Look up "PID tuning" for tutorials on how to do that systematically.

[hallowed31]

@DaveX

Are your props on the right way?

Props look good (if a little undersized IMO), CW and CCW diagonally opposite.

Do the motors spin in the correct, corresponding direction to the props?

Are you applying enough thrust on take off to offset the effect of imperfectly spinning motor RPMs and an imperfectly balanced drone (frame, battery etc)?

Does it take off correctly if you just apply sufficient thrust using only the RC signals (bypass the Arduino, that is)?

[gooddude546]

All of my motors spin the correct direction 2 one way and 2 the other, and the propellers are on the correct way. I also think I am applying enough throttle. If I apply more it flips faster and more violently. I think it is because of the rpm difference because motor 1 is the fastest an it is the one that shoots up and flips the drone. This is a link to my post where I detailed the issue:

Motors dont turn on at the same time

Hi, I am making a drone with an Arduino as an FC and I have my Arduino connected to a 4in1 ESC that controls my 4 motors. When I move the joystick to add power, The drone motors turn on one by one as I increase power (I have to increase the throttle very slowly at the start to see it happen). If you want, this is a video of the problem: https://drive.google.com/file/d/1FqVsEa8BPwgKMY0Tj8fXGpZU21N67Teb/view?usp=drive_link I tried to change the order of the motors in code, but nothing changed. …

On the arduino discord I was told that this is completely normal, and that my pid controller will take care of it, but I think the difference between motor 1 and 4 is just too high. Also I think this is the problem because when I turned the gyro, the drone flipped the same direction (meaning motor 1 was most powerful and motor 4 least)

Another problem could be something in the code, but I doubt it because the pid controller is just copy and paste

[gooddude546]

I am aware of this, but I don’t think that the p, i and d constants would make such a drastic and violent difference. I tied tuning them, but to me, it looked like there was no difference.

[jremington]

@gooddude546

I don’t think

Indeed. You clearly have no idea what you are doing, yet reject advice.

Good luck with your project!

[hallowed31]

@gooddude546

but I doubt it because the pid controller is just copy and paste

Ok, so narrowing down the issues...
"Just copy and paste"... well how do you know that whomever you copied this from has it right? @jremington seems to see something and if I were you, I'd listen. I'm a regular Joe "budget for mistakes" kind of builder who's been around this forum long enough to spot the gurus. @jremington is one of those gurus you might want to listen to.

@gooddude546

            Serial.print("-------------------------------------------------");
            Serial.println();
            Serial.print(channel1);
            Serial.println();
            Serial.print(channel2);
            Serial.println();
            Serial.print(channel3);
            Serial.println();
            Serial.print(channel4);
            Serial.println();

With the props off, does this

     Serial.print(channel1);
            Serial.println();
            Serial.print(channel2);
            Serial.println();
            Serial.print(channel3);
            Serial.println();
            Serial.print(channel4);
            Serial.println();

show what it's supposed to? It should give you an idea if the code is working as intended in your application.

I can't comment on the pid stuff because I honestly don't know. I'm just giving you things I would look at.

After testing with the props off and everything ok, put the props on upside down and test again. Does anything seem to change? Is noise being introduced to that causes some imbalance in the signals being sent to the ESCs?

Is the drone overall balanced relative to its center? Engineer types here who know what I mean, what's the correct term for that? Like could you balance it on the center on the tip of your finger sort of idea.

@gooddude546

 MotorInput1= 1.024*(InputThrottle-InputRoll-InputPitch-InputYaw);
  MotorInput2= 1.024*(InputThrottle-InputRoll+InputPitch+InputYaw);
  MotorInput3= 1.024*(InputThrottle+InputRoll+InputPitch-InputYaw);
  MotorInput4= 1.024*(InputThrottle+InputRoll-InputPitch+InputYaw);

Lastly, I'm really looking at this and on first impression, it seems ok. The addition and subtraction terms seem balanced, maybe but almost look as though a couple are missing. Was the code you're building off intended for a quadcopter and never a hexacopter? Just asking. Are you sure these motors correspond correctly to the motors? Like 1 and 3 should be diagonally opposite, no?

[jremington]

@hallowed31

I can't comment on the pid stuff because I honestly don't know. I'm just giving you things I would look at.

Your instincts and points are good. It is worth noting that PID can't be tuned and won't ever work, if any of the signs on the correction terms, or directions of motor rotation are wrong.

[gooddude546]

I checked the directions of the motors and everything looked good. As @jremington said, I have no clue what I am doing. All of the PID tuning sites say don't have it too high or too low, but I don't know where to even start, like what a good baseline number is.

Concerning all of the possible mistakes mentioned by @hallowed31:

• I have the battery connected to the drone with strong Velcro, so I can change the center of mass before every test, so it is quite balanced during every test
• I will try to turn the props upside down tmrw, because it is late here
• The 4in1 ESC I am using has the motors in the order 1- front left, 2-back left, 3-front right, 4-back right. So in my quad 1 and 4 are diagonally opposite. However I did have a mistake in there with the pitch, so thank you for pointing that out
• Yes. The PID controller was meant for a quad and not a hexacopter. It is from a youtube tutorial series on how to build an Arduino Drone. However I had to change It quite a lot due to my hardware. If you are interested this is the series: https://www.youtube.com/watch?v=jY6bBcMtseY&list=PLeuMA6tJBPKsAfRfFuGrEljpBow5hPVD4&index=16
• Lastly the Serial.print(channel1); ... Only gives the value that the receiver gets and not the final calculation. I changed it to show me the final calculation values of all the motors and you were right. I have some huge mistakes in the PID calculations or somewhere, because it gives me absolute crap. For exaple when I increase the throttle to half, It starts of good, all motors around 1450, but as time goes on (keep in mind I am not touching the drone at all) It slowly reduces power of motors 1 and 4 and maxes out power of motors 2 and 3. So in a few seconds the values go from something like:
  Motor 1: 1480.00
  Motor 2: 1499.00
  Motor 3: 1499.00
  Motor 4: 1445.00

To something like:

Motor 1: 1180.00
Motor 2: 1999.00
Motor 3: 1999.00
Motor 4: 1345.26

Which gives no sense considering the drone is not moving or rotating, so the values should stay constant and all around the same value because the drone is on a pretty level surface. I will try to find the mistakes in the code tomorrow and give you an update on how it is going.

Thank you for your help for now