Balancing Robot PID Problem

Hello everybody,

I’ve been working on a self balancing robot using Arduino UNO, IMU is 6 DOF MPU6050 (GY521) Accel + Gyro, L298N Motor Driver, 2 x 6V minigear motors without encoder.

To get correct angle, I’m using Kalman filter library which in my opinion get me a correct angle with a little noise.

i read the reference from this forum about tuning the robot with little P and go to D to remove the ossilation and then go to I, but after tuning like that i cant get any good result. now im set it to P = 39 , I = 6 and D = 0.5 its a bit balance now but sometimes its just go forward or bacward until its tilt around 7 to 5 degree and fall to the ground. is there something wrong with my code or logic maybe? or should i tune the PID again? . i dont know which need to increase so the robot can quickly recover to balance point. Any help would be much appreciated. dont worry to ask me anything if you need to know more about the robot.

PS: i will upload the video later because i dont have good internet connection & sorry for the bad english

Thanks,

I2C.ino (2.3 KB)

MPU6050.ino (6.64 KB)

Kalman.h (2.1 KB)

Kalman.cpp (3.46 KB)

First thing to consider is when your motors start turning. Do you account for the deadband between PWM signal of 0 to when you have enough power to start rotation even without load. This deadband, if not set correctly, will prevent you from properly setting proportional.

Next when your robot is balanced physically do you achieve the exact value of setpoint. this needs to match within .1°

Do not enable balancing control until you are +-1° of setpoint (almost balanced) and disable control at +-40° to prevent runaway situation.

Here's a video I made showing what I mean: How to Balance Robot PID tutorial in under 2 minutes! #BalanceBot - YouTube
now Start with Proportional control only you can achieve balance with this by itself just not very good.
Integral is not needed on a flat surface so skip this (add it last to adjust setpoint for slopes or other unknown influences)
Derivative will smooth out the wobble you will get from proportional.

Z

thanks for ur reply sir, i have read all your post and watch two of your videos about balancing robot.

im not a control engineer so i dont really familiar with something like deadband, so can you assist me how to get a deadband for balancing robot, im sorry if that was a stupid question.

what is the different of 0 degree and 1 degree setpoint?

zhomeslice:
Do not enable balancing control until you are +-1° of setpoint (almost balanced) and disable control at +-40° to prevent runaway situation.

this is for starting the robot right? dont start balancing until it get the right angle?

zhomeslice:
Here's a video I made showing what I mean: How to Balance Robot PID tutorial in under 2 minutes! #BalanceBot - YouTube
now Start with Proportional control only you can achieve balance with this by itself just not very good.
Integral is not needed on a flat surface so skip this (add it last to adjust setpoint for slopes or other unknown influences)
Derivative will smooth out the wobble you will get from proportional.

i already follow your instruction sir trying to tuning it from P until it get good enough to balance but when i add d D term its always jitter and balance and wandering around just for about 10 second until its fall to the ground

PS: if i use D term like in your video sir its not affect anything until i use around 60 D term

Thanks,

thanks for ur reply sir, i have read all your post and watch two of your videos about balancing robot.

im not a control engineer so i dont really familiar with something like deadband, so can you assist me how to get a deadband for balancing robot, im sorry if that was a stupid question.

what is the different of 0 degree and 1 degree setpoint?

this is for starting the robot right? dont start balancing until it get the right angle?

i already follow your instruction sir trying to tuning it from P until it get good enough to balance but when i add d D term its always jitter and balance and wandering around just for about 10 second until its fall to the ground

PS: if i use D term like in your video sir its not affect anything until i use around 60 D term

Thanks,

Everyone’s program responds differently and we are somewhat relying on your observations to get an idea as to what you are struggling with You have given me several clues as to what your challenges are as well as I can help answer your questions. So don’t get discouraged if your bot doesn’t react exactly like mine with the values I show.

so can you assist me how to get a deadband for balancing robot

// NORMALISASI HASIL PWM NEGATIF
if (MotorKiri < 35) {
MotorKiri=35;
}
if (MotorKanan < 35) {
MotorKanan=35;
}

Am I correct that at the analogWrite(ENA,35) is where your motor start to move?
What happens while the PID loop is transitioning from -35 to 0 and again from 0 to +35?
Answer: Nothing and so your PID loop can’t provide any reaction to needed control requests during this transition
The simplest change would be to Add 35 before sending it to the motors
Suggested change:

MotorKiri = PID;
MotorKanan = PID;

// NORMALISASI HASIL PWM NEGATIF
MotorKanan=abs(MotorKanan);
MotorKiri=abs(MotorKiri);

// This will work to allow instant shifting in direction
MotorKiri += 35;
MotorKanan += 35;

if (MotorKiri > 255) {
MotorKiri=255;
}
if (MotorKanan > 255) {
MotorKanan=255;
}

// or this would work also with a little different reaction curve
MotorKiri = map(MotorKiri ,0,255,35,255);
MotorKanan = map(MotorKanan,0,255,35,255);

// Try either of the above options:
analogWrite(ENA, MotorKanan);
analogWrite(ENB, MotorKiri);

The other quick observation I spotted was that you D term is higher than mine

if i use D term like in your video sir its not affect anything until i use around 60 D term

The cause of this is that you Change in Error is small because your sample rate is much higher delay(2); and you are not converting it to a known time interval as I did. Is this a problem Not at all yours should work fine and your Kd will represent your timing. I used a 1 second interval to reference my Ki and Kd terms My actual sample rate is 10 Milliseconds and so everything is calculated with this in mind.
My PID Code

unsigned long Now;
if ( (unsigned long)((Now = micros()) - lastTime) >= (SampleTime)) {
unsigned long DeltaTuS = (Now - lastTime);
lastTime = Now;
double Input = *myInput;
double Setpoint = *mySetpoint;
double error = Setpoint - Input;// Calculate error
double PTerm = kp * error;// Proportional term
double DeltaTS = ((double)DeltaTuS * 0.000001);//

if(ki){
ITerm += error *  DeltaTS * ki; // Integral term
ITerm = (ITerm <outMin )? outMin:((ITerm > outMax)? outMax : ITerm); // Prevemts Windup
}
double DTerm = 0;
if(kd){
//Derivative term using error change (Possible infinity errors could occure)
//DTerm = kd * ((error - pre_error)  / DeltaTS); //Derivative term
//pre_error = error;
/********************************************************************************************************************************
Kd*derror(temperature)/dtime = -Kd*dangle(temperature)/dttime // The two equations are equal Second one avoids infinity errors
*********************************************************************************************************************************/
// Derivative term using angle change
DTerm = -kd * ((Input - lastInput)  / DeltaTS); //Derivative term  avoid infinity errors
lastInput = Input;
}
double Output = PTerm + ITerm + DTerm; //Compute PID Output
Output = (Output <outMin )? outMin:((Output > outMax)? outMax : Output); // Limits Output
*myOutput = Output;

I will look over your code more and comment as I see other differences or suggestions
Let me know if this helps.
Z

• Sample rate of 10 milliseconds worked better than 2 milliseconds
• added code to allow for exact sample rate to allow for proper PID calculations
• added spam timers and such for Serial output
• added Start stop for Balance control (nothing will happen until bot is almost balanced)
• among other changes

I tested it on a new balancing bot I’m working on and had decent success.
Try it on your bot and see how it works for you. My PID setting and motor minimum probably won’t match your needs.
I Like your version of code it has been the closest to working on my bots I’ve seen in awhile

Z

thanks for your answer sir, sorry for late reply. i will try your advice and report to you what is the result

zhomeslice:
I Like your version of code it has been the closest to working on my bots I've seen in awhile

Glad to hear that sir

Thanks

zhomeslice:
Everyone’s program responds differently and we are somewhat relying on your observations to get an idea as to what you are struggling with You have given me several clues as to what your challenges are as well as I can help answer your questions. So don’t get discouraged if your bot doesn’t react exactly like mine with the values I show.

// NORMALISASI HASIL PWM NEGATIF

if (MotorKiri < 35) {
MotorKiri=35;
}
if (MotorKanan < 35) {
MotorKanan=35;
}

Am I correct that at the analogWrite(ENA,35) is where your motor start to move?
What happens while the PID loop is transitioning from -35 to 0 and again from 0 to +35?
Answer: Nothing and so your PID loop can't provide any reaction to needed control requests during this transition
The simplest change would be to Add 35 before sending it to the motors
Suggested change:

MotorKiri = PID;
MotorKanan = PID;

// NORMALISASI HASIL PWM NEGATIF
MotorKanan=abs(MotorKanan);
MotorKiri=abs(MotorKiri);

// This will work to allow instant shifting in direction
MotorKiri += 35;
MotorKanan += 35;

if (MotorKiri > 255) {
MotorKiri=255;
}
if (MotorKanan > 255) {
MotorKanan=255;
}

// or this would work also with a little different reaction curve
MotorKiri = map(MotorKiri ,0,255,35,255);
MotorKanan = map(MotorKanan,0,255,35,255);

// Try either of the above options:
analogWrite(ENA, MotorKanan);
analogWrite(ENB, MotorKiri);

------------------
The other quick observation I spotted was that you D term is higher than mineThe cause of this is that you Change in Error is small because your sample rate is much higher delay(2); and you are not converting it to a known time interval as I did. Is this a problem Not at all :) yours should work fine and your Kd will represent your timing. I used a 1 second interval to reference my Ki and Kd terms My actual sample rate is 10 Milliseconds and so everything is calculated with this in mind.
My PID Code :)

unsigned long Now;
if ( (unsigned long)((Now = micros()) - lastTime) >= (SampleTime)) {
unsigned long DeltaTuS = (Now - lastTime);
lastTime = Now;
double Input = *myInput;
double Setpoint = *mySetpoint;
double error = Setpoint - Input;// Calculate error
double PTerm = kp * error;// Proportional term
double DeltaTS = ((double)DeltaTuS * 0.000001);//

if(ki){
ITerm += error *  DeltaTS * ki; // Integral term
ITerm = (ITerm <outMin )? outMin:((ITerm > outMax)? outMax : ITerm); // Prevemts Windup
}
double DTerm = 0;
if(kd){
//Derivative term using error change (Possible infinity errors could occure)
//DTerm = kd * ((error - pre_error)  / DeltaTS); //Derivative term
//pre_error = error;
/********************************************************************************************************************************
Kd*derror(temperature)/dtime = -Kd*dangle(temperature)/dttime // The two equations are equal Second one avoids infinity errors
*********************************************************************************************************************************/
// Derivative term using angle change
DTerm = -kd * ((Input - lastInput)  / DeltaTS); //Derivative term  avoid infinity errors
lastInput = Input;
}
double Output = PTerm + ITerm + DTerm; //Compute PID Output
Output = (Output <outMin )? outMin:((Output > outMax)? outMax : Output); // Limits Output
*myOutput = Output;

I will look over your code more and comment as I see other differences or suggestions
Let me know if this helps.
Z

i’ll try your suggestion sir your answer give me a hope

good morning sir, yesterday i try to upload your sketch to my bot and try to configure the PID tuning. but its always not as good as in your videos, when i set the kP = 13 and no kD and Ki its a little balance but with only for about 5 or 3 second, its not because of chaos and jittery but because when its lean about 7 or 10 degree its cannot recover to its 0 degree so its just go forward until its go fall down.

after that i see your video and in your video you say "good kD will recover from push so i try to discover what is the good kD but until this morning i cant figure it when its not recover from push and fall fown for 5 or 7 second ".

thanks for your help Z hope you can answer this one.

good morning sir, yesterday i try to upload your sketch to my bot and try to configure the PID tuning. but its always not as good as in your videos, when i set the kP = 13 and no kD and Ki its a little balance but with only for about 5 or 3 second, its not because of chaos and jittery but because when its lean about 7 or 10 degree its cannot recover to its 0 degree so its just go forward until its go fall down.

after that i see your video and in your video you say "good kD will recover from push so i try to discover what is the good kD but until this morning i cant figure it when its not recover from push and fall fown for 5 or 7 second ".

thanks for your help Z hope you can answer this one.

I will need more data, maybe some video or other information to suggest other changes and observations. I also have my code for balancing bots you could try but I am impressed with your version and would like to see you succeed. Video would be best to help figure out what you are experiencing

I will need more data, maybe some video or other information to suggest other changes and observations. I also have my code for balancing bots you could try but I am impressed with your version and would like to see you succeed. Video would be best to help figure out what you are experiencing

hello Z i still didnt get the solution for my PID tuning but i already upload my video on this link balancing robot attempt #1 - YouTube, tell me if you need more information or maybe video.

i just change my robot body but its not really good, because its will always go lean forward and the PID isnt enough to give it a feedback to stabilize the robot, i dont know which term need to increase to get the right tune , i try with 0 I and D but its still not good enough its cant recover from off balance. or did we really need something like motor encoder ? because i see that in other balancing robot project but i still cant afford that. your project is the one closest to mine but seems still not yet close to the balance point.

Thanks

The Shaking jittery is probably too much Kd

The leaning may be caused by setpoint not matching the balancing point.
The motor are not instantly reversing power to quickly catch the bot
or Kp is too week all three could be the part of the cause.

I am not sure where the problem is at but here is a start:
Decrease or remove Kd for now.
Check setpoint matches Balancing degrees.
Increase Kp until it oscillates then back it off until it just stops (then add Kd back in )
Increase the minimum power to the motors see how I do it below.

Here’s my “DirectDrive” code that could help with the motors not reversing quickly:

// For your bot:
#define PWM_A     (11)   // 0-255             Arduino Digital pin 6
#define PWM_B     (9)   // 0-255             Arduino Digital pin 5
#define HBgA1HighEn (13) // Enable/Disable    Arduino Digital pin 8
#define HBgA2HighEn (12) // Enable/Disable    Arduino Digital pin 7
#define HBgB1HighEn (8) // Enable/Disable    Arduino Digital pin 4
#define HBgB2HighEn (7) // Enable/Disable    Arduino Digital pin 3

int PowerOffset = 0;// makes one side run stronger than the other +- 10 would be good limits adjust for turning when "bool TurnControlEnable = false;" is false
double PIDOutMax = 255; // +- 255
int PWMMax = 255; // max PID value for analogWright it is 255 or less
int MinPower = 36; // motors should be almost ready to go but they are stopped with no torque off the ground

void DirectDrive(int Power, int TurnVal ) {
if (!Mode)return;
bool DiredtionA = true; //true/false change to reverse motor A direction (Change if motor is backwards!)
bool DiredtionB = true; //true/false change to reverse motor B direction (Change if motor is backwards!)
TurnVal = 0 - TurnVal; // if turning in wrong direction uncomment;
static double LastPowerA;
static double LastPowerB;
TurnVal = (abs(TurnVal) < 10) ? 0 : TurnVal; // Stops jittering (Deadband)
int PowerA = constrain(Power + TurnVal, 0 - (int)PIDOutMax, (int)PIDOutMax); // Any adjustment to power can't exceed +- PIDOutMax;
int PowerB = constrain(Power - TurnVal, 0 - (int)PIDOutMax, (int)PIDOutMax); // Any adjustment to power can't exceed +- PIDOutMax;
int OutA = map(abs (PowerA), 0, (int) PIDOutMax, MinPower - PowerOffset, 255); // Convert +- PIDOutMax to MinPower to 255 for PWM Motor output
int OutB = map(abs (PowerB), 0, (int) PIDOutMax, MinPower + PowerOffset, 255); // Convert +- PIDOutMax to MinPower to 255 for PWM Motor output
if ((PowerA == 0) || ((LastPowerA > 0) != (PowerA > 0))) {  // check for no power or change in direction Motor A

CoastA();
}
if ((PowerB == 0) || ((LastPowerA > 0) != (PowerB > 0))) {  // check for no power or change in direction Motor B
CoastB();
}
LastPowerA = PowerA;
LastPowerB = PowerB;
if (PowerA < 0) {
HBridgeGoA(DiredtionA);
}
if (PowerA > 0) {
HBridgeGoA(!DiredtionA);
}
if (PowerB < 0) {
HBridgeGoB(DiredtionB);
}
if (PowerB > 0) {
HBridgeGoB(!DiredtionB);
}
if (abs (PowerA) > 0) {
SetPWMA(OutA);
RunA();
}
if (abs (PowerB) > 0) {
SetPWMB(OutB);
RunB();
}

}

void CoastA() {
SetPWMA(0); // Coasting Zero Power
HBridgeLockA(false);
}
void CoastB() {
SetPWMB(0);// Costing Zero Power
HBridgeLockB(false);
}
void RunA() {

}
void RunB() {

}

void FullBreaking () {
HBridgeLockA(false);
HBridgeLockB(false);
SetPWMA(255);
SetPWMB(255);
}

void SetPWMA(int Speed) {
analogWrite(PWM_A, Speed);
}

void SetPWMB(int Speed) {
analogWrite(PWM_B, Speed);
}

/* My HBridge configuration =
I have 2 1/2 H-Bridge chips per motor each chip has one High/Low side select input and an enable input.
The enable input of both 1/2 inputs are tied together and connected to pin 6 for Hbridge A and pin 3 for Hbridge B with PWM enabled
The following code focuses only on the High/Low side select
So by default when the enable input (PWM) is HIGH  The Low side of my HBridge is on.
Now I can enable the High side by connecting 5v to the Select pin (HBgA1HighEn pin 8,HBgA2HighEn pin 7 & HBgB1HighEn pin 4,HBgB2HighEn pin 5).
so for costing all I need to do is set PWM low.
and for full breaking I need to set all High/Low selects to the Low side and enable the H-Bridge (analogWrite(PWM_A, 255);)
Forward motion I put one side high and the other low reverse that and the motor turns in reverse.
Simple :)
*/

void HBridgeLockA(bool HighSide) {
digitalWrite(HBgA1HighEn, HighSide);
digitalWrite(HBgA2HighEn, HighSide);
}

void HBridgeLockB(bool HighSide) {
digitalWrite(HBgB1HighEn, HighSide);
digitalWrite(HBgB2HighEn, HighSide);
}

void HBridgeGoA(bool Reverse) {
digitalWrite(HBgA1HighEn, Reverse);
digitalWrite(HBgA2HighEn, !Reverse);

}

void HBridgeGoB(bool Reverse) {
digitalWrite(HBgB1HighEn, Reverse);
digitalWrite(HBgB2HighEn, !Reverse);
}

The code has some empty functions due to a different H-Bridge configuration remove them if you don’t need them. I left them for place holders to remind me how I accomplished something.

Power is a value from ± PIDOutMax
TurnValue is also from ± PIDOutMax but shouldn’t need that much to make it turn fast
Power offset is a trim for the motors if one is more powerful than the other ± (0~20)
MinPower is where the motors are just moving (about 30 for my bots)

Z

ill post video this afternoon
before implement this getDirective code, what is the !mode refers to?

and i dont know i just change the position of mpu6050 to the middle of the bot and now its need more kP to balance and more shaky now.

and adding more kD didnt help it too

Thanks

ill post video this afternoon
before implement this getDirective code, what is the !mode refers to?

The mode refers to balancing or non balancing on my bot. since my Green tiered tank style bot could be controlled when it has fallen over I had an alternate mode Tank style driving on all 4 wheels. I missed removing that line. you could make "mode" permanently true or remove it all together.

and i dont know i just change the position of mpu6050 to the middle of the bot and now its need more kP to balance and more shaky now.

and adding more kD didnt help it too

Thanks

I will need to do more experiments also. my MPU is at the top of my small bot and near the middle on my big one. We are missing something with the calculations of MPU6050 data or it will need to be close to the middle. I didn't have time to test it more tonight but this weekend look promising.
Z

Is it normal for balancing robot move forward even in balancing point cause my bot do so, i try to adjust the position my battery so it can balance but its not good enough, and i will try to tune its more now with more high kP because if its lean it just go forward and cant recover from its position

Ill wait sir.

Thank

Is it normal for balancing robot move forward even in balancing point cause my bot do so, i try to adjust the position my battery so it can balance but its not good enough, and i will try to tune its more now with more high kP because if its lean it just go forward and cant recover from its position

Ill wait sir.

Thank

More Kp will help with recovering. The bot should be able to oscillate at about balance with only Kp. Having a little Kd while tubing Kp could help. If your bot has a jittery shaking you're Kd value is too high. Adding Ki can correct for setpoint errors, But its best to start with Kp then Kd and add Ki last for final improvements.

I'm Struggling with some kind of delay possibly caused by the Kalman filter. I can get my bot to balance using the MPU6050's DMP code but it isn't responding correctly with your Kalman filter code. Still Testing....

Sorry for late reply sir, just got trouble few days ago and now its fine.

should i go for the MPU6050 DMP ? is that different with kalman filter that i implemented ? actually i dont really know whats better for my bot and for DMP i dont really understand that and not have the example code to implement that on my bot.

i still struggling with the tuning i will try to record the video sir.

zhomeslice:
I'm Struggling with some kind of delay possibly caused by the Kalman filter. I can get my bot to balance using the MPU6050's DMP code but it isn't responding correctly with your Kalman filter code. Still Testing....

is it really the kalman filter i use make this error? can i upload a sketch of your bot to my bot so i can compare what is the different ?

Thanks

Sorry for late reply sir, just got trouble few days ago and now its fine.

should i go for the MPU6050 DMP ? is that different with kalman filter that i implemented ? actually i dont really know whats better for my bot and for DMP i dont really understand that and not have the example code to implement that on my bot.

i still struggling with the tuning i will try to record the video sir.

lets try this code out to see if it works on your bot.

you will need to use The calibration sketch to get good offsets for the MPU6050
Also you will need to modify Balancing_Bot_Works_.ino to work with your motor control
Let me know if this will balance your bot. I’m still trying to figure out how to get your code working

Z

Balanceing_Bot_Works_.ino (17.5 KB)

PID_v2.cpp (8.08 KB)

PID_v2.h (2.94 KB)

MPU6050_calibration.ino (7.64 KB)