PID loop for Two-wheeled Self-balancing bot

Hi all, I am so close to having my very own self balancing bot, except well, it doesn't balance yet. I need to get the PID loop tuned up so that it will balance. I believe I have all of the relevant code below. I am using a sabertooth 2x12 RC controller and have the motors setup as servos so that the outputs are properly received at 50hz by the sabertooth controller, which accepts a standard 1000us to 2000us pwm signal where 1000us is full reverse, 1500us is stop, and 2000us is full forward.

My output values properly run between servo outputs of 0 to 90 and 90 to 180 to generate the required pulse outputs. However, I can't figure out how to get the darn thing to balance. Any tips on this?

I have read tons about how to tune PID loops. My method has been to increase proportion until I get some decent movement, and then up the integral to try minimize the time it takes to hit the stable state. However, I have been miserably unsuccessful. The bot seems to have a slow response time and tips far one way before sending the motors to high speed to try and compensate. The motors are plenty strong/fast enough, really too fast at full speed.

void FilterData()
{
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //Get accelerometer/gyroscope readings
  Angle_Raw = (atan2(ay, ax) * 180 / pi + Angle_offset); //Angle Position
  omega =  gz / Gyr_Gain + Gry_offset; // Angular Velocity
  unsigned long now = millis(); //Time program has been running
  float dt = (now - preTime) / 1000.00; //Sample time in seconds
  preTime = now;
  float K = 0.8;
  float A = K / (K + dt);
  Angle_Filtered = A * (Angle_Filtered + omega * dt) + (1 - A) * Angle_Raw;
}

void PIDloop()
{   
	kp = 10; //10
	ki = 0; //
	kd = 0; //

  unsigned long now = millis(); //Time program has been running
  timeChange = (now - lastTime); //Prevent integer overflow issue
  lastTime = now; //Set time measurement was taken
  
  error = Angle_Filtered;  // Proportion
  errSum += error * timeChange;  // Integration
  dErr = (error - lastErr) / timeChange;  // Differentiation
  Output = kp * error + ki * errSum + kd * dErr; //Calculated Output
  lastErr = error; //Reset for next loop
  
  Motor1 = constrain(Output - Turn_Speed + Run_Speed, -90, 90);
  Motor2 = constrain(Output + Turn_Speed + Run_Speed, -90, 90);
}
if(Motor1 > 0) //Forwards Motor1, same code for motor2
  {
   S1.write(map(Motor1 + Motor1_offset, 0, 90, 90, 140));
  }
  else if(Motor1 < 0) //Backwards Motor1
  {
     S1.write(map(Motor1 + Motor1_offset, -90, 0, 30, 90));
  }

Any help on this would be greatly appreciated. Thank you guys so much for any advice that you may have. Also, I have already read through the 16 page thread from a few years ago on balancing bots and have done tons of googling on the topic.

webdev77:
My output values properly run between servo outputs of 0 to 90 and 90 to 180 to generate the required pulse outputs. However, I can't figure out how to get the darn thing to balance. Any tips on this?

I have read tons about how to tune PID loops. My method has been to increase proportion until I get some decent movement, and then up the integral to try minimize the time it takes to hit the stable state. However, I have been miserably unsuccessful. The bot seems to have a slow response time and tips far one way before sending the motors to high speed to try and compensate. The motors are plenty strong/fast enough, really too fast at full speed.

This is about a year late. But hopefully you are still around and made good progress. From my recent experience, I found that changing the serial communications rate from say 115200 baud to something slower - such as 9600 baud will have a significantly bad impact on the control system. I had forgotten that I had changed the baud rate, which slowed everything down. I was oblivious to it for a quite a while. Then discovered the issue. Once I had set the serial speed back to some 'high enough' rate, my balance bot actually started working well. The 'lethargy' behaviour disappeared - the bot started reacting in the desired way.

The other thing is.... the motors have dead-zone, where input voltage is too low to move the wheels. So they have to figure out what minimum average voltage to apply to the motors to clear the dead-zones. Also, make sure that your Arduino is sampling at a fast enough rate. Otherwise, too slow a control system prevents the bot from responding fast enough, which can also lead to the oscillatory build-up effect, where the forward backwards motion just keeps growing and growing - instead of steadily reducing.

One more thing..... your code showed Ki = 0 as well as Kd = 0. Your Kp might be fine, but you will certainly need to have a finite value for Kd, because Kd is a setting that helps your bot to automatically pull back on the throttle, which is adding damping to the system. Without this, the bot won't be able to work towards reducing the amount of oscillations (going back and forth). There is a limit for Kd though. Too high a value for Kd can cause problems too. Kd is essential though. As for Ki..... you may need a little bit of Ki, but not too much. Ki can help to slowly nudge the bot a tiny bit in the right direction whenever the bot is fairly well balanced, providing some subtle error correction.

Anyway, I arrived here because I did a search on google, and was very interested in the origins of the first two lines of the following Arduino sketch code.....

float K = 0.8;
float A = K / (K + dt);
Angle_Filtered = A * (Angle_Filtered + omega * dt) + (1 - A) * Angle_Raw;

I didn't know what the K = 0.8 refers to, and wasn't sure what "A = K/(K+dt)" was for.

The 3rd line I can understand, it is a complementary filter type equation.

At the moment, after thinking about it, my guess is that "A" is a weighting factor calculated from A = K / (K + dt); so that when the time difference 'dt' (units of milliseconds) increases, then "A" becomes smaller and smaller with time, which has the effect of providing more weight toward the gyroscope-based angle measurements [ie. Angle_Filtered + omega * dt] for the short term, while putting more weight toward the accelerometer-based angle [ie. Angle_Raw] measurements for the long term.

My bot is stock standard sainsmart v3. It's working properly. The only thing is that the stock Arduino code has a motor-off setting in the code, where the code is: if (abs(r_angle)<45)

So when the inclination angle is more than 45 degrees, then the bot is meant to cut the motors, otherwise it'll just keep spinning its wheels if the bot falls over or something. At the moment, I have set the limit to 85 degrees, and I found that my bot will cut the motors even when the physical inclination angle is less than 20 degrees. So what I need to do later is to log the angle data to see what's going on.

I've hooked up the bot to dump angle values via serial comms, and the raw angles measured using the two-axis accelerometer method seemed quite well behaved. I should be able to figure out what's happening - eventually.