Balancing robot for dummies

SnakeLT: Very impressive robot, can you give us any details on the algorithms you use? Normal PID loop as anyone else here or anything fancier ;)?

I'm using interactive (series) PID.

Happy New Year!

@kas: yes, that is my long term goal, but I fear it's way too hard for me to do, as I lack solid programming skills and have no time to learn and experiment

Lately I've been swamped by work, but I started some other projects that take me away from my poor balancing bot. I did not built a proper frame for my bot, I've been trying to see if a regular low profile robot can do balancing. I've come to the conclusion that large wheels are important, high COG improves balancing, high speed is required for fast corrections. That being said, my poor low height robot with small wheels and not enough speed has trouble balancing even for a split second.

So keep it up guys, I'll do some more work on my bot in a few weeks when I'll be off work.

Cheers!

@kas: Thank you for your answers. I will try your suggestions when I have access to my robot :slight_smile:

@SnakeLT: Could you explain more about this:

At first I want to say a word about bk-bot control algorithm, it will not work correctly, main error is that balance pid output is torgue not speed, so...

Why is it that the balance pid output must be torque, not speed?

Thank you,

Villa

It does not must be torgue, but in this case it is. At output you got torgue and you interpret it like speed. Look at my algorithm, and dont use motor speed pid.

Hi, kas. Happy New Year to you!
I am doing my own balancing robot. However, there is something wrong with the fusing.
My filtered angle seems to lag the real angle for about several seconds. My accelerometer is ADXL202, and the gyro is ENC03, the Kalman filtering code which I use is from this thread .Here is the imagine on my filtering.
http://user.qzone.qq.com/513837087?ADUIN=513837087&ADSESSION=1293883100&ADTAG=CL...
Could you give some suggestions on it? Thank you!

Hi niebing, nice to see you here
I noticed some asian characters (now gone) in your code, out of curiosity, where are you based ??

I can't access the Web address you indicated, from my PC.
Seems you use only one ACC and asin() vs atan().
Your code is too different from Kasbot, sorry, I can't help :frowning:
Please let us have a photo of your bot

I am from China.

Seems you use only one ACC and asin() vs atan().

I just used one axis to measure the angle by accelerometer. I want to know is it right?
About my robot, i do not have built up it, i am only doing the experiment on the fusion.So i do not have the photo of my robot, Sorry :(.
I changed my link on the photo,could you have a try?
the black line is the angle measured by the accelerometer. the read one is angle speed , the blue one is filtered angle
http://niebing1987.tuzhan.com/photo/f1998db0d3002b80.html

It does not must be torgue, but in this case it is. At output you got torgue and you interpret it like speed. Look at my algorithm, and dont use motor speed pid.

Yeah, I understand what you mean, but my question is on what basis/theory did you make that conclusion? Could you please give me a more thorough explanation?

Another thing I wonder is about the sample time of the PID loops? How fast the driving loop, the balance loop and the heading loop should be?

Thanks,

Villa

Hi SnakeLT, thanks for joining the group
I understand you are from Lituania (cold is it??) :wink:
As for most of us here, English is not your native language.
Please take your time, work at your own speed, and produce structured technical information

Your bot is really cute and agile, the frame is... a CD box :slight_smile:
Which development platform are you using ??


You do not use accelerators and the reason you gave...

in long term equilibrium angle is always 0deg, so you only need to know short term angle. So in long term angle, program artifically slowly averaging angle to 0deg.

...is pretty smart :sunglasses:

Also I don't see motor velocity in your diagram. Is it computed in the "Driving PID" bloc ??
Please let us have additional info on your control strategy, together with source code or pseudo code

I now realize that, as Villa mentioned earlier, a single PID loop will never produce a completly still robot
Also, a single encoder is not definitly enough to keep a fixed heading

@niebing1987

You can definitly aquire angle with only one ACC
The angle is small and some folks assume sin(x) = x (in radians)
Now... the ADXL202 is a dual ACC, why don't you use the 2 axis for a more linear mesurement ?? :slight_smile:

      Acc = MeasureAcc() + ACC_OFFSET;
      MeaAngle = asin((FP32)Acc/GRAVITY_G);      //Measure angle by the accelerometer

Can I assume ACC_OFFSET is sensorZero[] ?
Is GRAVITY_G the gravity offset ? ( (ADC value "upward" - ADC value "downward")/2 )
Why is this value divided rather than substracted ?
Are MeaAngle and MeaAngleDot refering the same units for angle (deg, rad, quids...) ?

      ADData = ReadTLC4541();  //measure the anglespeed by the gyro 5000.0*(ADData-RefAD)/65536.0/0.67/K;
      MeaAngleDot = 0.001995*(RefAD-ADData) / K;

You use a 16 bit serial ADC to access the Murata gyro
Try changing the sign of MeaAngleDot

With so few information, those are only shots in the dark
What microcontroler are you using ? is it fast enough ?

Looking at the diagrams, ACC and Gyro are in phase
(gyro is zero when movement is inverted)
Your Kalman graph is just dead and may need different parameters
see danielaaroe post #157

Good luck in your quest ;), keep us aware

Villa, I'm not physicist and can't explain it. In balancing robot case torgue is something like robot heigthgravitysin(angle), balance pid directly deals with angle. In my robot all PID's run at 100Hz.

kas, yes I'm from Lithuania, not so cold -4degC, but alot of snow.
I'm using GCC for avr, main procesor xmega. Yes speed derived from position (D term).

Angle calculation:

volatile float gOffset = 0;
float KOF = 0.004; //0.005
volatile float G_Angle = 0;
float TimeInterval = 0.01;

void GetGyroData(int GyroRaw)
{
  float GyroSpeed;
  gOffset = KOF * GyroRaw + (1-KOF) * gOffset;
  GyroSpeed = GyroRaw - gOffset;
  G_Angle += gyroSpeed*TimeInterval;
}

Hi,kas

Now... the ADXL202 is a dual ACC, why don't you use the 2 axis for a more linear mesurement ??

When I firstly read the details about the robot, they just used one axis and asin, so i :(.....but i will make the change.

Can I assume ACC_OFFSET is sensorZero[] ?
Is GRAVITY_G the gravity offset ? ( (ADC value "upward" - ADC value "downward")/2 )
Why is this value divided rather than substracted ?
Are MeaAngle and MeaAngleDot refering the same units for angle (deg, rad, quids...) ?

yes,ACC_OFFSET is the sensorZero
however, GRAVITY_G is not the gravity offset, it is the ADC value when the axis is paralleled to the gravity.

What microcontroler are you using ? is it fast enough ?
Looking at the diagrams, ACC and Gyro are in phase
(gyro is zero when movement is inverted)
Your Kalman graph is just dead and may need different parameters

my microcontroller is LPC1114, 32bit, colock 48MHz, it should be fast enough.
For my poor english, i do not understand clearly ACC and Gyro is in phase, is it right like this?

Hopefully a quick one folks, in the v2 code for the serial GUI it has this function:

int getGyroRate() 
{                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
  return int((sensorValue[GYR_Y] * 0.88888888889)*-1);
}

Where does the 0.88888888889 constant come from? it doesn't seem to match any ref voltage etc.

Thanks!

@ richiereynolds

I'm using a nother GYR called LPR510AL and that one has a sensitivity of 10 mV/(°/s) at ±100°/s.

It looks like I have made a miss calculation where it should have been:

// ARef=3.3V, Gyro sensitivity=10mV/(deg/sec)
// in quid/sec:(1024/360)/1024 * 3.3/0.010)
return int((sensorValue[GYR_Y] * 0,9166679)*-1);

Measurement range: ±100°/s and ±400°/s
Sensitivity: 10 mV/(°/s) and 2.5 mV/(°/s)

That could explain some errors I been having..

Sorry :frowning:

I have rebuild my robot lighter but I haven't fixed the electronics on It yet hopefully I will get It done next week.

I have also ported the GUI to the .net platform but I'm not satisfied with the serial communication. Is there any one how has a good idea how to send the amount of data I'm doing in a better way.

Like it is now it will not be able to send it and still be able to keep the fixed loop time at 10ms. I solve it by only sending data to the GUI each 5 loops..

The functions looks like this to day:

int skipOut=0;

void serialOut_GUI() {  
  
  if(skipOut++>=updateRate) {                                                        
    skipOut = 0;
    //Filtered and unfiltered angle
    Serial.print(ACC_angle, DEC);  Serial.print(",");
    Serial.print(actAngle, DEC);   Serial.print(",");
    
    //Raw sensor data
    Serial.print(sensorValue[ACC_X], DEC);   Serial.print(",");
    Serial.print(sensorValue[ACC_Z], DEC);   Serial.print(",");
    Serial.print(sensorValue[GYR_Y], DEC);   Serial.print(",");
   
    Serial.print(pTerm, DEC);      Serial.print(",");
    Serial.print(iTerm, DEC);      Serial.print(",");
    Serial.print(dTerm, DEC);      Serial.print(",");
    Serial.print(drive, DEC);      Serial.print(",");
    Serial.print(error, DEC);      Serial.print(",");
    Serial.print(setPoint, DEC);   Serial.print(",");
    
    //PID Parameters
    Serial.print(K, DEC);    Serial.print(",");
    Serial.print(Kp, DEC);   Serial.print(",");
    Serial.print(Ki, DEC);   Serial.print(",");
    Serial.print(Kd, DEC);   Serial.print(",");
    
    //loop
    Serial.print(STD_LOOP_TIME, DEC);        Serial.print(",");
    Serial.print(lastLoopUsefulTime, DEC);   Serial.print(",");
    Serial.print(lastLoopTime, DEC);         Serial.print(",");
    Serial.print(updateRate, DEC);           Serial.print(",");
    
    Serial.print(motorOffsetL, DEC);         Serial.print(",");
    Serial.print(motorOffsetR, DEC);         Serial.print(",");  
    
    Serial.print(pTerm_Wheel, DEC);     Serial.print(",");
    Serial.print(dTerm_Wheel, DEC);     Serial.print(",");
    Serial.print(Kp_Wheel, DEC);        Serial.print(",");  
    Serial.print(Kd_Wheel, DEC);        Serial.print(",");    
    Serial.print("\n");
  }
}

union u_tag {
  byte b[4];
  float fval;
} u;

int skipIn=0;

void serialIn_GUI(){
  
  if(skipIn++>=updateRate) {                                                        
    skipIn = 0;
    byte id;
    
    if(Serial.available() > 0)
    {               
      char param = Serial.read(); 
      //Serial.println("Have bytes");
      delay(10);
      byte inByte[Serial.available()];
      if(Serial.read() == SPLIT){
        if(Serial.available() >= 4){
          u.b[3] = Serial.read(); 
          u.b[2] = Serial.read(); 
          u.b[1] = Serial.read(); 
          u.b[0] = Serial.read(); 
          Serial.flush();
          
          switch (param) {
            case 'p':
              Kp = int(u.fval);
              break;
            case 'i':
              Ki = int(u.fval);
              break;
            case 'd':
              Kd = int(u.fval);
              break;
            case 'k':
              K = u.fval;
              break;
            case 's':
              setPoint = int(u.fval);
              break;
            case 'u':
              updateRate = int(u.fval);
              break;
            case 'l':
              motorOffsetL = u.fval;
              break;
            case 'r':
              motorOffsetR = u.fval;
              break;
            case 'q':
              Kp_Wheel = int(u.fval);
              break;
            case 'e':
              serial_type = int(u.fval);
              break;
            case 'w':
              Kd_Wheel = int(u.fval);
          }
        }
      }
      
    }
  }
}

It's a realy long line that are beaning send to the GUI maybe it would be better to split it and sending it like bytes like I do when I send data to the Arduino..

Ah, excellent Patrik, thanks, that's that sorted.

My robot's actually working a bit better now, balancing, kind of ...

Going to work on the encoder parts now.
I also put a couple of "emergency wheels" on the bottom that swing down and are also supposed to hold it steady while it does zero calibration at the start, then swing up out of the way to let it balance. Only problem is, the servos only have the torque to hold the robot up when it's near vertical!

Oh well, back to the drawing board ...

On the sending data to the gui question, would it be any quicker to put the strings into a buffer using sprintf and then send it in one Serial.println?
Other than that, I can only think of usng a fixed format that the GUI understands and sending bytes as you say.

@SnakeLT: Its ok. Thank you for your answer. Your robot is really awesome! ^^

@Patrik: I think that you shouldn't perform the data acquisition with a sample time of 10 (ms) in Windows (.NET), because Windows is not a real-time OS and 10 (ms) seems to be a little bit too fast.

@richiereynolds: When sending data to the GUI, You should encode your data using numbers in stead of characters. That way you will save a lot of bandwidth as well as processing time.

kas,thanks for your suggestion.it is the parmeter! After modifying them, the effect of kalmanfilter is much better.

http://niebing1987.tuzhan.com/photo/09ea592eba247adc.html

;D ;D ;D ;D ;D

I have also ported the GUI to the .net platform but I'm not satisfied with the serial communication. Is there any one how has a good idea how to send the amount of data I'm doing in a better way

Hi Patrik
My first idea is to increase data speed transfer to 115200 bps
I went on your blog Prevas Parekring
and checked BalancingBotGUI v1.2

  • In BalancingBotGUI_v_1_2.pde,
  Version log:

    v1.2  ...
        Changed the serial speed to 115200 kb/s
  • in serial_screen.pde,
void setupSerial()
{
  ...
  myPort = new Serial(this, portName, 19200);
  ...
}

I am confused, are you still transmitting @19200 bps ?? :-?
Please clarify