Self Balancing isn't balancing

Hello
I was wondering if anybody could help me. At the moment, I’m building a self balancing robot. It’s task is to stay balanced when activated and recover it’s balance when I give a litte push at the robot. I’m using an Arduino Uno with an l3gd20 3-axis gyro carrier and an L298N motor driver.
Now here’s the problem:
the robot seems to balance itself for about a second but after that, it immediately fails balancing.
Maybe an error is located in my arduino code, but I can’t seem to find anything wrong. When I verify and upload my code, I don’t get any error messages. Maybe somebody else kan spot a error in my code. Any help is very much appreciated.

Here is my code

byte countS = 0;   
int recOmegaI[10];   
int omegaI = 0;   
long thetaI = 0;   
long sumPower = 0;  
long sumSumP = 0;  
const int kAngle = 50;   
const int kOmega = 500;   
const long kSpeed = 60;   
const long kDistance = 20;   
long powerScale;   
int power; 
long vE5 = 0;  
long xE5 = 0;  
#include <SPI.h>   
int ry;  
long R;  
void L3GD20_write(byte reg, byte val) {  
  digitalWrite(10, LOW); 
  SPI.transfer(reg);  
  SPI.transfer(val);  
 digitalWrite(10, HIGH);  
}

byte L3GD20_read(byte reg) { 
 byte ret = 0; 
 digitalWrite(10, LOW);  
 SPI.transfer(reg | 0x80); 
 ret = SPI.transfer(0);  
 digitalWrite(10, HIGH);  
 return ret;  
} 

void setup () {  
  Serial .begin(115200); 
 pinMode(4, OUTPUT); 
pinMode(5, OUTPUT);  
 pinMode(6, OUTPUT);  
 pinMode(7, OUTPUT); 
 pinMode(8, OUTPUT);  
 pinMode(9, OUTPUT);  
 for ( int i = 0 ; i < 10 ; i++ ) { recOmegaI[i] = 0; }


 pinMode(10, OUTPUT);  
digitalWrite(10, HIGH);  
 SPI.begin();  
SPI.setBitOrder(MSBFIRST);  
 SPI.setDataMode(SPI_MODE3);  
SPI.setClockDivider(SPI_CLOCK_DIV2);  
 L3GD20_write(0x20, B11001111);  
 L3GD20_write(0x23, B00000000);  

  delay(300); 
  }

 void loop () {  
 chkAndCtl();  
if ( power > 0 ) {  
 analogWrite( 6, power );     
 digitalWrite( 4, HIGH );            
 digitalWrite( 5, LOW );  
  analogWrite( 9, power );    
  digitalWrite( 7, HIGH );           
  digitalWrite( 8, LOW );    
  } else {    
    analogWrite( 6, - power );
 digitalWrite( 4, LOW );    
 digitalWrite( 5, HIGH );    
 analogWrite( 9, - power );    
 digitalWrite( 7, LOW );    
 digitalWrite( 8, HIGH );  
  } 
}

void chkAndCtl() {
   R = 0;  
for ( int i = 0 ; i < 45 ; i++ ) {  
 ry = ( (L3GD20_read(0x2B) << 8) | L3GD20_read(0x2A) );  
  R = R + ry;  
   delayMicroseconds(90);  
   }  
   
 omegaI = R * 0.00875 / 45; 
 if ( abs( omegaI ) < 2 ) { omegaI = 0; }  
 recOmegaI[0] = omegaI;  thetaI = thetaI + omegaI;  countS = 0;  
for ( int i = 0 ; i < 10 ; i++ ) {  
if ( abs( recOmegaI[i] ) < 4 ) { countS++; }  
}  if ( countS > 9 ) {    thetaI = 0;  
   vE5 = 0;    
   xE5 = 0;    
   sumPower = 0;    
   sumSumP = 0;  
   }
for ( int i = 9 ; i > 0 ; i-- ) { recOmegaI[ i ] = recOmegaI[ i-1 ]; }  
  powerScale = ( kAngle * thetaI / 100 ) +  ( kOmega * omegaI / 100 ) + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 );  
 power = max ( min ( 95 * powerScale / 100 , 255 ) , -255 );  
 sumPower = sumPower + power;  
 sumSumP = sumSumP + sumPower;

}

If the code seems to be ok, maybe the reason why the robot fails balancing isn’t my arduino code but the gyroscope. Is it possible it gives a wrong or bad signal to my arduino? How can I fix this?

Sorry way too many questions to be asked :slight_smile:
Once again thanks for your help.

What is the theory of operation?

Inexpensive gyroscopes drift, and can't be used to measure an absolute angle. Most people use a combination of gyroscope and accelerometer, together with a fusion filter, for balancing robots.

To be honest, I don’t know how to answer your question.
The components that I mentioned in my previous post are the main components of my robot. The Arduino Uno, Gyroscope and Motor drivers. Some extra parts too is a double gearbox (3-6 volt motors incuded).
The gyroscope is a three-axis gyroscope, wich measures the angular rates of rotation on x, y and z axis.
The Motor (dual H bridge) driver is used to control my DC motors.
I do know this information, but not how my robot exactly works.
I don’t know wat kind of signal my gyroscope delivers.
I don’t know wat kind of signal my Arduino delivers too.

I chose this robot as a school project but with very limeted knowledge about Arduino and gyroscopes.
also having difficulties about trying to teach arduino software for myself.
Maybe this project wasn’t the best idea, but it’s too late to go back now. Multiple attemps of finding a solution on google but with no results

I found the project on Instructables.com I’ve found a lot of info on the site too, but nog enough to fix my robot issues.
I’ll leave a link here, so maybe you can take a look and teach me something more about my robot
I really appreciate the help

about the code. Its different than the code you can download on Instructables. I’ve deleted all of the comments to make it look less messy.

I chose this robot as a school project but with very limeted knowledge about Arduino and gyroscopes.

Unfortunately, you chose a difficult project. Gyros measure the RATE of rotation, not angles, and without another reference you can't use a gyro to tell whether the robot is standing up or on its side.

Avoid instructables. The vast majority of them are written by people who don't know what they are doing, and are either seriously misleading or wrong.

My admittedly limited experience with a (cheap) gyro is that it was extremely inaccurate. It could probably be used to identify things like gestures. It turned out that what I was looking for, was in fact an accelerometer, since I wanted to create an automatically levelling platform. A comparably priced 3-axis accelerometer (cheap) actually impressed me with its accuracy. Never got around to build that platform, but I could have done so, in theory at least :-)

May I suggest that you abandon the self balancing robot, and instead try using the gyro sensor for detecting gestures, such as twisting a handle, with the sensor inside, one way or the other, to control something. Even this will require some way of quantifying the measures delivered from the sensor, but should be far more doable than your robot.

Alternatively, consider using an accelerometer and a servo, creating a simple self-levelling platform. Balancing a robot is far more complex than just reacting to inputs.

Tip: you still need some concept of how it is going to work, beyond hooking everything together. Typically the arduino will talk with the sensor using i2c or SPI, and you need to identify data values and ranges, and possibly calculate sliding averages or similar for producing usable data.

Best of luck.

First of all thank you for your replies. When I started this project, I didn't expect it to be that difficult. Maybe your suggestion about the self levelling platform isn't a bad idea. I'll try to build the project after I did some more research on the web. A little bit of background about the project doesn't seem like a bad idea, but I appreciate the effort you toke for replying. I'll avoid instructables by now too. Maybe you could give me some quick tips & trick about my robot? I will do some more tests to get it to work before i give up the project.

thanks again for replying.

As was said by another: "Gyros measure the RATE of rotation, not angles, and without another reference you can't use a gyro to tell whether the robot is standing up or on its side."

In short, you can not make a self balancing robot using only a gyro. That's the best and only quick tip regarding your attempt.

Not knowing what is up and down, just measuring a rudimentary speed of rotation, is not enough.

Rupert909: In short, you can not make a self balancing robot using only a gyro. That's the best and only quick tip regarding your attempt.

In fact you can balance with only a gyro. I have modeled it and built it.

You do not need to calculate the angle of tilt. You need only know which direction you are falling and how fast you are falling.

In fact you can balance with only a gyro. I have modeled it and built it.

What charliesixpack failed to mention is that you need a very accurate system model of your robot, in addition to the gyro.

jremington: What charliesixpack failed to mention is that you need a very accurate system model of your robot, in addition to the gyro.

Of course you can have an accurate model. Or you can take the shotgun approach with tuning pots that amateurs routinely use. There is nothing intrinsic in using only a gyro that requires a model.

Then, why not be professional and help DUPPYandCOCO constructively, to get his/her/their robot to balance?

jremington: Then, why not be professional and help DUPPYandCOCO constructively, to get his/her/their robot to balance?

I am being constructive in correcting false information.

That’s the spirit!

charliesixpack: I am being constructive in correcting false information.

charliesixpack: In fact you can balance with only a gyro. I have modeled it and built it.

But ...... your bot needs an accurate and stable reference angle in order to stay well balanced for a 'long' time, which can't be achieved with a gyro alone. So, to start with, accelerometers (or inclinometers) are needed to at least get a measure of the bot's angle.

A self balancing robot is in principle similar to a quadcopter.

Check out Joop Brokking's excellent videos about multi-rotor (drone) auto-level on his Arduino blog here: https://blog.arduino.cc/2016/06/08/teach-your-drone-what-is-up-and-down-with-an-arduino/

Southpark: But ...... your bot needs an accurate and stable reference angle in order to stay well balanced for a 'long' time, which can't be achieved with a gyro alone. So, to start with, accelerometers (or inclinometers) are needed to at least get a measure of the bot's angle.

I think you missed the part where I said that I built it and it works. Only a gyro. No accelerometer. Balances forever.

If you think inside the box and assume that you need to calculate the angle of tilt in order to balance then you are correct.

But, if instead you start with knowing the angular rate of change and implement control to null out the angular rate of change then you can balance with only a gyro. If the angular rate of change is constant and nearly zero then you are balancing. You may drift forward a few inches in an hour but you will be balancing.

A gyro plus feedback from wheel encoders (or motor output drive) to null drift? Or just a gyro (that's hard to believe - convince me if so)

charliesixpack: I think you missed the part where I said that I built it and it works. Only a gyro. No accelerometer. Balances forever.

But, if instead you start with knowing the angular rate of change and implement control to null out the angular rate of change then you can balance with only a gyro.

I see. So you actually do not use gyro alone, since you implement 'control' (aka use other sensors) to try compensate for errors.

Also.....when I mentioned 'balance'.... I meant staying put with no apparent movement. And no wandering .... not even 1 millimetre wander.

Southpark: I see. So you actually do not use gyro alone, since you implement 'control' (aka use other sensors) to try compensate for errors.

Also.....when I mentioned 'balance'.... I meant staying put with no apparent movement. And no wandering .... not even 1 millimetre wander.

Any feedback loop uses a controller in addition to a sensor. PID is one type of controller. A controller is not a sensor.

You will not eliminate drift if you add an accelerometer. The only way to eliminate drift is with wheel encoders. I am guessing you have never built one of these.

charliesixpack: You will not eliminate drift if you add an accelerometer. I am guessing you have never built one of these.

I am tinkering with them. I got 1 video of my commercially bought put-together-yourself bot.....currently fitting a Lipo battery. I know how to put together my own bot with own components though.... and have a reasonably good understanding of control systems and control theory.

http://youtube.com/watch?v=5DzuO9g-Xxk

It's important to make sure angle/sensor readings are relatively stable when the bot is still. So sensor testing is always good before we even use the sensors for the bot. I like the mpu6050 and mpu9050 devices.... with their onboard accelerometers that can be used for angle measurements. The gyros inside them complements the accelerometer features fairly nicely too.

charliesixpack: Any feedback loop uses a controller in addition to a sensor. PID is one type of controller. A controller is not a sensor.

When you mentioned implementing a control system to compensate for drift error in the gyro.... I assumed you implied you use another kind of sensor in that control implementation to deal with the drift error.