pid problems

here is a video of my balancing robot (from 00:07)
As far as you can see what can I do to improve balance?

/* PID variables */
double Kp = 10 ;            // 10     
double Ki = 0;             // 0.000001     
double Kd = 5;             // 5

coast_mode.ino (13.7 KB)

Just added a better video. mpu6050 imu arduino pid problems - YouTube

Thanks for your code I am extreamily interested in several items I struggled to resolve. I will be looking over several areas in more detail tomorrow. I may even dust off my balancing bot and try it on mine. :slight_smile:

I did find something
found this discrepancy:

Focusing on the range of your PID Loop compared to the range limits and mapping of your motors

Your PID ranges from -799 ~ 799

void PID(double restAngle, double offset, double turning, double dt) {

  /* Update PID values */
  double error = restAngle - pitch;
  double pTerm = Kp * error;
  iTerm += Ki * error * dt;
  if(iTerm> 100) iTerm= 100;
  else if(iTerm< -100) iTerm= -100;
  double dTerm = Kd * (error - lastError)/dt;    
  lastError = error;
  double PIDValue = pTerm + iTerm - dTerm;
  if(PIDValue > 799) PIDValue = 799;            // Your PID ranges from -799 ~ 799
  else if(PIDValue < -799) PIDValue = -799; 

  /* Steer robot sideways */
 double PIDLeft;
 double PIDRight;
 
    PIDLeft = PIDValue;
    PIDRight = PIDValue;

 // PIDRight *= 0.95; // compensate for difference in the motors

  /* Set PWM Values */
  
    moveMotor(left, PIDLeft);  
    moveMotor(right, PIDRight);
 
}

So your PID can’t “wind up” we should adjust the following
In this case windup occurs when you PID loop exceeded the constrain() functions limits of ±100 and had to work its way back within that range before it could make adjustments to your motor speeds.
Note that constrain() isn’t necessary since you are not adding to each motor to adjust its speed for turning
so it could read:
int speed = map(abs (speedRaw) , 0, 799, 90, 799) ;

I used constrain because I added to my Left and right motors a little extra power to make it turn right or Left as you saw in my video. The additional speed added to the motor speed ever Exceeded the map limits it could cause unpredictable results so I added the constrain() to prevent this.

In you moveMotor() funcion Line 301 should read as follows to match you limits in the PID. This should make the PID loop more accurate and less sensitive and automatically prevent windup. This will allow you to adjust your PID loop more accurately.

void moveMotor(Command motor, double speedRaw) {
  static double LastOutputLeft, LastOutputRight; // used to check for motor direction change
  
  int speed = map(constrain (abs (speedRaw) , 0, 799)    , 0, 799, 90, 799) ;  //96 adjusted map to meet your 100%  range. Note that PWM min is 30 (minimum motor torque)
int speed = map(abs (speedRaw) , 0, 799, 90, 799) ; 

 ...............................

     iTerm += Ki * error * dt;

//  if(iTerm> 100) iTerm= 100;
//  else if(iTerm< -100) iTerm= -100;

I am now thinking about time fixed loop:

#define STD_LOOP_TIME 10000 // Fixed time loop of 10 milliseconds
unsigned long lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime;

 /* Use a time fixed loop */
 lastLoopUsefulTime = micros() - loopStartTime;
 if (lastLoopUsefulTime < STD_LOOP_TIME) {
 while((micros() - loopStartTime) < STD_LOOP_TIME)
 //Usb.Task();
  }
 loopStartTime = micros();

you can see here Usb.Task(); because this project was supposed to control the balancing robot with a ps3 joystic sending commands to a usb host shield… BUT THIS IS ANOTHER STORY.
I am thinking just about fixing time loop. I think I need that as a sort of myPID.setSampleTime();
what can I add to time function while loop?

I’ve looked at a couple items and I am curious if my logic makes sense

first: in the void loop() { function

 while (!mpuInterrupt && fifoCount < packetSize) {  //***** This next portion of code will run allot because the mpuInterrupt triggers only once every x milliseconds!


    /* Calculate pitch */
    pitch = ypr[1] * 180/M_PI; // Calculate the angle using DMP library // This value only changes each time we get more data from mpuInterrupt  so pitch may stay the same value for many cycles until the next mpuInterrupt is triggered!!!!!! 

    /* Drive motors */
     timer = micros();
     PID(targetAngle,targetOffset,turningOffset,(float)(timer - pidTimer) / 1000000.0f);   // so here is the problem i see if pitch value repeatedly stays the same the PID loop alters the power based on errors and rates could loopin this with the same value confuse the PID algorithm???  
     pidTimer = timer;

My thoughts would be to move the PID Calculation and motor adjustments into the MPU calculation area Yes it slows the number of times the PID is calculated but with no new information from the MPU, why try to make changes?

Suggested locaiton:

             #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

            /* Calculate pitch */
            pitch = ypr[1] * 180/M_PI; // Calculate the angle using DMP library
        
            /* Drive motors */
             timer = micros();
             PID(targetAngle,targetOffset,turningOffset,(float)(timer - pidTimer) / 1000000.0f);   
             pidTimer = timer;


            
            Serial.print("ypr\t");

Don’t really know if better or not… because I do not know how much the pid sample time function is important in your code. In my code timer setting are making things going well together.

I think I’ve done my best but:
-backlash is a problem because robot would tilt a little bit every second and then overshoot

-heavier wheels would be able to transfer down more torque than mine(very soft wheels)

-time fixed loop: I am thinking about that… I think to give a command every 100ms… but serial communication makes code slower

-need to add that on the first “0” from the mpu the robot has to start working… I was not able to adapt your code to mine

Do you manage to try my code on your bot? take your time. you are already helping me a lot.

it drives me crazy
latest code in attachments

coast_mode.ino (13.8 KB)

My Balancing bot is revived with new MPU and Arduino UNO Motor control Clone I created. My first attempt is to restore my code to and get it balancing. Then I will implement your code. We have quite similar robots when it comes to overall concept even though we are using entirely different hardware so this should be fun. I'll keep you updates on my progress.

P.S. First attempt late last night was quite erratic with no success in balancing. I have make many changes since that version of my robot and so I need to re-tune/debug it :slight_smile:

zhomeslice:
My Balancing bot is revived with new MPU and Arduino UNO Motor control Clone I created. My first attempt is to restore my code to and get it balancing. Then I will implement your code. We have quite similar robots when it comes to overall concept even though we are using entirely different hardware so this should be fun. I'll keep you updates on my progress.

P.S. First attempt late last night was quite erratic with no success in balancing. I have make many changes since that version of my robot and so I need to re-tune/debug it :slight_smile:

You are the best! Good work.
I'm working on fixing time loop to 1-10ms because 100Hz (100ms) seems too much.
Stay tuned

Got my Balancing bot code Working Here it is:

altered one other file:

//in "MPU6050_6Axis_MotionApps20.h"
//I've altered line 305 to: 
 0x02,   0x16,   0x02,   0x00, 0x02                // D_0_22 inv_set_fifo_rate

to slow down the fifo buffer filling. corrected garbage values that appeared.

My motor drive will be different but this gives you an idea what i’m doing.

Attempting to test with your code now :slight_smile:

BalancingBotUno.ino (12.4 KB)

void moveMotor(Command motor, double speedRaw) {
  static double LastOutputLeft, LastOutputRight; // used to check for motor direction change
  
 int speed = map(constrain (abs (speedRaw) , 0, 100)    , 0, 100, 85, 799) ;  //Note that PWM min is 30on255 (minimum motor torque)so 85on800
 //int speed = map(abs (speedRaw) , 0, 799, 85, 799);
 
  if (motor == left) {
    if ((speedRaw== 0) || ((LastOutputLeft > 0) != (speedRaw> 0))) { // needs to be looking at speedRaw to see transition through zero from positive value to negitive value
           motorCoast();     
    }
      
    LastOutputLeft = speedRaw; // Store for Direction Change
   
    // Now we can set the speed
    setPWM(45, speed); // Left motor pwm 
      
    if (speedRaw > 0) {
      cbi(PORTA, PINA2);
      sbi(PORTA, PINA3);
    }
    if (speedRaw < 0) {
      sbi(PORTA, PINA2);
      cbi(PORTA, PINA3);
    }
    noMoreCoast(); // make sure direction is set correctly before enabling motors
  }
  else if (motor == right) {
        if ((speedRaw== 0) || ((LastOutputRight > 0) != (speedRaw> 0))) {
      motorCoast();
    }
      
    LastOutputRight = speedRaw; // Store for Direction Change

   // Now we can set the speed
    setPWM(44, speed); // Right motor pwm
    
    if (speedRaw > 0) {
      cbi(PORTA, PINA0);
      sbi(PORTA, PINA1);
    }
     if (speedRaw < 0) {
      sbi(PORTA, PINA0);
      cbi(PORTA, PINA1);
    }
    noMoreCoast(); // make sure direction is set correctly before enabling motors
  }
}

void motorCoast(){
  digitalWrite(35, LOW);

}

void noMoreCoast(){
  digitalWrite(35, HIGH);
 
}

void PID(double restAngle, double offset, double turning) {
    
   unsigned long now = millis();
   int timeChange = (now - lastTime);
   if(timeChange>=SampleTime){

  double error = restAngle - pitch;
  double pTerm = Kp * error;
  double iTerm = iTerm + error;
  iTerm= Ki*iTerm;
  
  if(iTerm > 90) iTerm = 90;
  else if(iTerm < -90) iTerm = -90;
  double lastError;
  double dTerm = Kd * (error - lastError);    
  double PIDValue = pTerm + iTerm - dTerm;
 
  lastError = error;
  lastTime = now;
  
  if(PIDValue > 799) PIDValue = 799;
  else if(PIDValue < -799) PIDValue = -799;

   double PIDLeft;
   double PIDRight;
 
    PIDLeft = PIDValue;
    PIDRight = PIDValue;
   
 // PIDRight *= 0.95; // compensate for difference in the motors
    moveMotor(left, PIDLeft);  
    moveMotor(right, PIDRight);
 // Serial.print(pTerm);Serial.print("\t"); Serial.print(iTerm);Serial.print("\t"); Serial.print(dTerm);Serial.print("\t"); Serial.print(PIDValue);
 // Serial.println();
   }  
}

Soon testing this part because I do not need abs (speedRaw) , 0, 100) and I do not need constrain NOW.
Testing the commented line. maybe better

 int speed = map(constrain (abs (speedRaw) , 0, 100)    , 0, 100, 85, 799) ;  //Note that PWM min is 30on255 (minimum motor torque)so 85on800
 //int speed = map(abs (speedRaw) , 0, 799, 85, 799);