Go Down

### Topic: Balancing Robot PID Problem (Read 1 time)previous topic - next topic

#### hadrizx

##### Mar 10, 2017, 11:04 am
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,
Hadri Zulkarnaen.

#### zhomeslice

#1
##### Mar 10, 2017, 03:38 pm
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: https://youtu.be/uyHdyF0_BFo
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
HC

#### hadrizx

#2
##### Mar 11, 2017, 03:32 am
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?

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?

Here's a video I made showing what I mean: https://youtu.be/uyHdyF0_BFo
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,
Hadri

#### zhomeslice

#3
##### Mar 11, 2017, 04:40 am
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,
Hadri
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.

Quote
so can you assist me how to get a deadband for balancing robot
Code: [Select]
`  // 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:
Code: [Select]
`  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
Quote
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
Code: [Select]
` 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
HC

#### zhomeslice

#4
##### Mar 11, 2017, 08:44 pm
Made some additional changes to your code to help you get started.
• 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
HC

#### hadrizx

#5
##### Mar 13, 2017, 12:39 am
thanks for your answer sir, sorry for late reply. i will try your advice and report to you what is the result

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

Hadri Zulkarnaen

#### hadrizx

#6
##### Mar 13, 2017, 12:54 am
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.

Code: [Select]
`  // 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:
Code: [Select]
`  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
Code: [Select]
` 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

#### hadrizx

#7
##### Mar 14, 2017, 01:57 am
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.

Hadri

#### zhomeslice

#8
##### Mar 14, 2017, 04:46 am
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.

Hadri
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
HC

#### hadrizx

#9
##### Mar 16, 2017, 02:29 am
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 https://youtu.be/mAOvG-07Sfw, 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

Hadri

#### hadrizx

#10
##### Mar 16, 2017, 03:02 am
another video : https://youtu.be/eMAaR8Db28k
https://youtu.be/u_cWsQxAMsY

#### zhomeslice

#11
##### Mar 16, 2017, 04:30 am
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:
Code: [Select]
`// 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 3int PowerOffset = 0;// makes one side run stronger than the other +- 10 would be good limits adjust for turning when "bool TurnControlEnable = false;" is falsedouble PIDOutMax = 255; // +- 255int PWMMax = 255; // max PID value for analogWright it is 255 or lessint MinPower = 36; // motors should be almost ready to go but they are stopped with no torque off the groundvoid 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 enabledThe following code focuses only on the High/Low side selectSo 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
HC

#### hadrizx

#12
##### Mar 17, 2017, 02:01 am
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

hadri

#### zhomeslice

#13
##### Mar 17, 2017, 05:37 am
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.

Quote
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

hadri
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
HC

#### hadrizx

#14
##### Mar 17, 2017, 06:18 am
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

Hadri

Go Up