Self Balancing Robot (issue, not balancing yet)

Hello,

I submit in attachment my code for a self balancing robot.
I have been reading a lot on the topic and experimenting many samples.
However, my robot does not balance. I have been close to it with multiple back and forth before falling, but my actual code just does one move and fall ! I think the motors do not have a qucik response enough.

I tried to make a code as clear as possible to understand what I was doing (I do not like copy-pasting without understanding).

If you would like to review my code and tell me if you see any flaw in it, I would be gratefull.

Thanks for your help.

_MyBalanceRobot.ino (7.15 KB)

First thing to do is to make sure your angle measurements are reliable. Also.... avoid using print serial in the code if you use a lot of it.

Don't put delay functions in your loop. That will slow down your system.

To get time differences.... just use code to measure time(in microseconds) between two events. You can use that time difference for your pid implementation.

At this moment.... you should probably hold the robot in your hand .... and use code that logs the angle (to the vertical) versus time ..... every 0.1 second .... just for testing. And then use your hand to change the angle physically. Copy the test data (serial printed) from the serial monitor to a file. Then plot the angle in degrees versus time. This at least shows that the angle measurements are good.

You also need to have the angle measuring device calibrated.... have you done the accelerometer calibrations?

Hello,

Thanks for your quick reply.

The angle calculation i reliable. When I hold the robot by hand, it is good. Oscilate between -1 and 1. When I tilt the robot, the wheels turn in the proper direction to balance it.
The IMU has been calibrated and event when I tilt the robot in many direction for long, and when I put it back vertical, angle goes back to 0 (-1 to 1). So, yes, the IMU and complementary filter seems to work great.

My worry is more on the PID calculation and also to see if the loop is properly organized, because I cannot see any problem in it.

I have used the calculated delay for the PID implementation.

In fact, the robot does not try to balnce, it goes straight on one side and falls. I have tried many P values, from 0 to 100 and it does not change the behavior... That is why I suspect the code...

Thanks.

Thanks. So you have removed the delay (1); statement from your code now?

Are these the correct calibration values? If not, use the correct ones.

  // Set offsets based on calibration process
  mpu.setXAccelOffset(1896);
  mpu.setYAccelOffset(-1090);
  mpu.setZAccelOffset(2132);
  mpu.setXGyroOffset(55);
  mpu.setYGyroOffset(34);
  mpu.setZGyroOffset(36);

Hello @jremington.

Yes the values are the correct ones.
As far as I understand none of you finds any problem in the code ?!

Thanks for your help anyway.

In fact, the robot does not try to balnce, it goes straight on one side and falls.

There are many possibilities:

  1. Perhaps the motors are not powerful enough to correct the fall.

  2. You have vastly underestimated the required Kp and are otherwise not following the rules for "tuning" the PID
    algorithm. Why are Kd and Ki not zero for initial testing?

  3. There are lots of mysterious constants in the code, including restraints, conditionals etc. Did you write the code, and are these necessary, or did you just download the code hoping it would work?

  4. What is all the NeoPixel crap doing in there? Get rid of anything that is not absolutely necessary, until you have solved the basic balance problem.

Tarasbulba:
As far as I understand none of you finds any problem in the code ?!

I mentioned that you should get rid of 'delay' statements, and recommended that you remove the delay (1); in your loop. The recommendation is to remove it. That line there is at least 1 problem.

I agree with jrem ..... remove all extraneous code that is absolutely not needed ..... this is to begin with.

Also...... you have this:

mpu.setXGyroOffset(55);
  mpu.setYGyroOffset(34);
  mpu.setZGyroOffset(36);

You need to be sure that the gyro offsets are correct, or at least makes sense. For my mpu6050, my Grx gyro offset is around "500" (-assuming +Y-axis is bot's forward movement direction, and +X-axis on the right-hand-side of the forward movement direction. I only use one of the gyro offsets, because I only focus on rotation in a single plane. Your gyro offsets are like a factor of 10 smaller than mine. Sure, this depends on what device you have, and there will be differences among devices of the same model. But still need to make sure the offsets (calibration) is good (correct) ----- because successful operation depends a lot on that.

Tarasbulba:
Hello,

I submit in attachment my code for a self balancing robot.
I have been reading a lot on the topic and experimenting many samples.
However, my robot does not balance. I have been close to it with multiple back and forth before falling, but my actual code just does one move and fall ! I think the motors do not have a qucik response enough.

I tried to make a code as clear as possible to understand what I was doing (I do not like copy-pasting without understanding).

If you would like to review my code and tell me if you see any flaw in it, I would be gratefull.

Thanks for your help.

try getting rid of integral
use PD only in the PID loop
Why you ask :slight_smile: Integral is additive errorSum += error ; and shifts the output (motor drive power) to achieve setpoint. but we absolutely know the output we need when at setpoint.
balanced at setpoint we want 0 power output.
I’ve achieved a decent balance with proportional only but adding derivative cleans up things
Here is a link to my quick how-to video I did several years ago :slight_smile:
How to Balance Robot PID tutorial in under 2 minutes! #BalanceBot
I have achieved balance on several bots I’ve made

Your PID loop needs a delay of a specific time I use 10ms because it is part of the DMP (Digital Motion Processor) of the MPU6050
The derivative value is the problem. If you have no delay you error between readings is so small and the noise is too great that finding the derivative multiplier is almost impossible you could isolate the delay only to the derivative calculation
Example:

  // Calculate Error
  error = currentAngle - targetAngle; 
  static float DOutput;
  static unsigned long _ETimer; //Exact timer
  if ( millis() - _ETimer >= (10)) { // 10 milisecond delay to help calculate a decent dirivitive error 
    _ETimer += (10);
    DOutput =  Kd * (error - previousError) / _DeltaTime;
    previousError = error;
  }

  // Calculate output from P and D values (P= current value,  D= future values
  motorPower = Kp * (error) + DOutput ;

Last suggestion for now :slight_smile:
Block your motors from running until you are close to balancing at the start if your motors are always running even when you are attempting to find the initial balancing point, your startup and tuning will be difficult.
Z
P.S. if you must add integral for some reason you will want to reset the errorSum variable to zero at the start of the balancing.
Something like if fallen over and getting ready to balance (error from setpoint <1°) errorSum = 0 balance go
This sequence triggers once just before balancing and starting the motors to drive.

Hello all,

Thanks for the feedback.

@jremington
-> I have seen many times the same chinese motors as me on SelfBalance robots on the net (so I assume they are good enough)
-> max motor power is 255. a Kp of 255 is full speed with 1degree of tilt. I have tried many values with Ki and Kd at zero. (but you are right I did give a try to add a few Ki and Kd for test).
-> Constants, constraints are not mysterious to me. The code is mostly inspired from existing code picked here and there, but I understand every piece of it. I spent many hours reading and looking for info.
-> Neopixel is for lighting effects. You are right I can remove them until it balances.

@southpark
-> I tried removing the delay(1) and it doesn't solve the issue. I have read many times that the loop should not be too fast. And that is confirmed with @zhomeslice on this post. Right now, with a delay(1), I have an avergae executn loop of 5ms (200 times per second).

@zhomeslice
-> Thanks for the detailed advices.
-> I have thought this night to remove the I. I agree with you, it is not much of a use. And the error is constrained when too high. With a tilt of 1 degree, the max error is reach with 1 second. When I hold the robot by hand, I can keep the errorsum within a -100|100 range, but it is tough.
->I will try to slowdown the loopspeed to 10ms. I agree with your analysis.
-> The motors do not move when I start the robot and hold it still.

In fact, I also suspect a problem, but I have seen videos of robots working with the same hardware as me, but : my motors only start to turn when the speed is around 70 (max is 255). Therefore, I cannot have very low speed of the motor. My H bridge is maybe not appropriate. I also have a Adafruit H bridge much better (higher frequency), but I was not able to have it running on I2C at the same time as the MPU (address conflict I suspect).
My MPU is not placed at the center of the motors but at the top of the robot. I have read only a few times that it should be on the wheel axis. But once again, I have seen many videos where the MPU is higher in the robot.

I'll give a try with the delay and removing the Ki and keep you posted.

Tx.

Tarasbulba:
: my motors only start to turn when the speed is around 70 (max is 255).

I see. This is motor deadband right? Some adjustment or compensation might be needed… such as add 60 or 65 to your current pwm levels…which pushes your motor to the brink of moving. Might have to have a different pwm offset for each motor… due to differences between individual motors.

I believe the 0 to 255 values you’re talking about are PWM levels… not speed (not velocity).

My loop in my balance bots code have no delay functions in them. I avoid putting delay functions in my loop… as delay functions fall under the ‘blocking code’ category.

@southpark : right, this is the deadband, and yes, it is PWN, not speed. anyway, same result...
Not yet tested, but will do.

Tarasbulba:
@southpark : right, this is the deadband, and yes, it is PWN, not speed. anyway, same result…
Not yet tested, but will do.

This is my motor drive function that I use with my bot
It has the motor minimum that is mentioned by @Southpark
And this has a turn feature ± TurnVal set to zero to go strait.

void DirectDrive(int Power, int TurnVal ) {
	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 worng 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);
	else if (PowerA > 0) HBridgeGoA(!DiredtionA);
	if (PowerB < 0)	     HBridgeGoB(DiredtionB);
	else if (PowerB > 0) HBridgeGoB(!DiredtionB);
	if (abs (PowerA) > 0)SetPWMA(OutA);
	if (abs (PowerB) > 0)SetPWMB(OutB);
}

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


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);
}

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);
}

Z