Balancing robot for dummies

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

@kas

I have bean running it on 19200bps I tried with 115200 but I went back to 19200 and just sending data each 5 loops..

Is there any downside with using 115200?

What I saw when sending data each loop the arduino could not hold the loop time any more... But when I send it each 5 loops it seams to be ok running around 7-8ms before delay,,,

But maybe I'm fueling my self and each 5 loops the fixed loop time is of and maybe that will give faults in the regulation of the robot..

I want to find a way to be able to send the amount of data I'm sending now in each loop with out going over 10ms.. But still maybe only send It each 5 loops because I don't think the GUI will be able to read it faster..

I have a idea of sending parts of the data under five loops and then start over like.

switch (loop_count){
  case 1:
     //send PID values
  case 2:
     //send PID parameter values
     //K value as float
     //[id byte][split][value][value][value][value][CR]
     //Kp value as int
     //[id byte][split][value][value][CR]
  case 3:
     //send Sensor values
     //send Angel values
  case 4:
     //send Torque values
  case 5:
     //send Settings values
  defualt:
    loop_count = 0;
}
loop_count++;

What do you think about that? And in your opinion what baud rate should we use if possible?

@niebing1987

For my poor english, i do not understand clearly ACC and Gyro is in phase,

My English is not good either :wink:
I mean that the Gyro signal (angle variation, deg/s) can be seen as the first derivative of the ACC signal (deg)
Gyro signal should be zero when ACC signal is horizontal (dX/dt=0)
On the first diagram the red curve cross the zero axis when the black curve reaches a max or a min.

This is also noticiable on the left side of your second diagram.

Kalman parameters are specific to sensors type (noise)
Now your filtered angle is much better, but too much filtered :wink:

@Patrik

What do you think about that? And in your opinion what baud rate should we use if possible?

I will experiment and be back by the end of the week

@kas

I mean that the Gyro signal (angle variation, deg/s) can be seen as the first derivative of the ACC signal (deg)
Gyro signal should be zero when ACC signal is horizontal (dX/dt=0)
On the first diagram the red curve cross the zero axis when the black curve reaches a max or a min.

in my opinion, if the gyro signal is not zero, which means the Angle wil go on changing, the ACC signal will not be the max or min.
And i wil go on modifying the parameter to improve my kalmanfitering.

I'm curious, would a indication of excellent control be the ability for the balancing robot to be able to 'balance' at any desired angle, rather then just at true vertical? Has anyone been able to do that or have witnessed that?

Lefty

I'm curious, would a indication of excellent control be the ability for the balancing robot to be able to 'balance' at any desired angle, rather then just at true vertical? Has anyone been able to do that or have witnessed that?

Lefty

I have seen an inverted pendulum which is capable of doing what you described (An inverted pendulum is similar to a balancing robot).

I think for the robot to move back to the balance angle from any initial angle, the controller must be a non-linear controller, i.e. Fuzzy Logic Controller or Adaptive Controller. A linear controller (like PID) will not do the job because with a broad range of angle's value, the robot can no longer be considered linear.

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 SnakeLT
Thanks for the info
Would you mind posting the complete source code :slight_smile:

Hello All. First, thank you so much for all of the valuable information. It has helped greatly with my balance-bot project. All the replies are amazing.

Question: I'm using Patrik's v1.2 code, with the Processing 'Balance_bot_gui' code (Thank you Patrik, it rocks.) All is displaying well, my bot is balancing... sort of, but I have a question about the output.

When I look at the error displayed on the Processing GUI, it seems delayed. By about 1-2 seconds. If I hold the bot at-error, and error on the screen displays -80, then I quickly whip it over to where it should display +80, it takes about 1.5-2 seconds for the error on the GUI output to update to that error. (Hope that makes sense.)

Any tips? I'm pretty sure my ACC and GYRO are both reading in phase, and my Kalman output all looks fine... just a little late.

I tried to google a bit about alternatives to PID and found some fuzzy logic and neural network papers. It looks complicated and I have no experience in this field so I don't know where to start. But using a self adaptive algorithm would be nice. Any way, if you are interested here is the links:

http://eprints.usq.edu.au/6168/1/Miller_2008.pdf

And some that requires IEEE login, I could only access the first one with my login but they all look interesting.

Hi bryanpearson
Welcome to this thread,
no doubt Patrik will soon jump in to answer your question

@Patrik

What do you think about that? And in your opinion what baud rate should we use if possible?

I will experiment and be back by the end of the week

I ran this (really ;)) striped down version of KasBot

int   STD_LOOP_TIME  = 9;             
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

void setup() {
  Serial.begin(XXXXX);
}

void loop() {
// ********************* print Debug info ********************************
  Serial.println("The quick brown fox jumps over the lazy dog");
  Serial.println("The lazy dog doesn't care and keep sleeping");
  Serial.print("Loop:");        Serial.print(lastLoopUsefulTime);
  Serial.print(",");            Serial.println(lastLoopTime);
  Serial.println();

// *********************** loop timing control **************************
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
}

The results (lastLoopUsefulTime) are, as expected, inversely proportional to the Serial data speed

9600 110 ms
19200 54 ms
38400 27 ms
57600 18 ms
115200 9 ms

Interestingly enough, 28800 bps doesn't work on my system
I used a Diecimila board powered by an ATmega168,
So try using 115200 bps, but YMMV.

Is it related with the lag observed by bryanpearson ??

@ GuzZzt

... It looks complicated...

It does :o :o

@bryanpearson

There are some problems with the control showing the error value and I haven't found it yet..

You can do amazing things with Processing but when you should make a good GUI you half to make all controls from the ground and that takes more time than making it to what you really want. A balancing bot GUI...

I have moved on and start porting it to .net c# to be able to put more focus on the application than the individual controls.

@kas
I have also expremented with the communication between the bot and the computer and made a test program that seams to work. I will try it in my bot when the new one is finished. The last parts should com in the next week..

#define   GYR_Y                 0                      // Gyro Y (IMU pin #4)
#define   ACC_Z                 1                      // Acc  Z (IMU pin #7)
#define   ACC_X                 2                      // Acc  X (IMU pin #9)

#define   LINE_END              10                     // \n
#define   SPLIT                 58                     // :

int   STD_LOOP_TIME  =          9;             

int sensorValue[3]  = { 10, 11, 12};
int sensorZero[3]  = { 0, 0, 0 }; 
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int actAngle;                                                  // angles in QUIDS (360° = 2PI = 1204 QUIDS   <<<
int ACC_angle;
int GYRO_rate;
int setPoint = 0;
int drive = 0;
int error = 0;
int updateRate = 0;            //Nr of loop to skip sending and receving info from PC
long count = 0;
long last_count = 0;

float K = 1.4;
int   Kp = 3;                      
int   Ki = 1;                   
int   Kd = 6;
int   Kp_Wheel = 1;
int   Kd_Wheel = 1;
int   last_error = 0;
int   integrated_error = 0;
int   pTerm = 0, iTerm = 0, dTerm = 0;
int   pTerm_Wheel = 0, dTerm_Wheel = 0;

int error_message = 0;

float motorOffsetL = 1;        //The offset for left motor
float motorOffsetR = 1;        //The offset for right motor

byte buffer[55];

void setup() {                                   
  Serial.begin(115200);
  //Serial.begin(19200);
  buffer[0] = char('#');
  sendErrorToBuffer(1);
}

void loop() {
  
// ********************* print Debug info *************************************
  //serialOut_raw();
  //serialOut_timing();
  serialIn_GUI();       //Processing information from a pc
  serialOut_GUI();      //Sending information to pc for debug
  
// *********************** loop timing control **************************
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
  if(lastLoopUsefulTime>STD_LOOP_TIME) sendErrorToBuffer(1); //Sends error to GUI
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
}

int serial_type=0;
uint8_t loop_count=0;

void serialOut_GUI() {  
  if(error_message == 0)
  {
   //send PID values
   addIntToBuffer(1, pTerm);
   addIntToBuffer(3, iTerm);
   addIntToBuffer(5, dTerm);
   addIntToBuffer(7, error);
   addIntToBuffer(9, pTerm_Wheel);
   addIntToBuffer(11, dTerm_Wheel);
   
   //send PID parameter values
   addFloatToBuffer(13, K);
   addIntToBuffer(17, Kp);
   addIntToBuffer(19, Ki);
   addIntToBuffer(21, Kd);
   addIntToBuffer(23, Kp_Wheel);  
   addIntToBuffer(25, Kd_Wheel);
   
   //send Sensor values
   //send Angel values
   addIntToBuffer(27, ACC_angle);
   addIntToBuffer(29, actAngle);
   addIntToBuffer(31, sensorValue[ACC_X]);
   addIntToBuffer(33, sensorValue[ACC_Z]);
   addIntToBuffer(35, sensorValue[GYR_Y]);
  
   //send Torque values
   addIntToBuffer(37, drive);
   addFloatToBuffer(39, motorOffsetL);
   addFloatToBuffer(43, motorOffsetR);
   
   //send Settings values
   addIntToBuffer(47, setPoint);
   addIntToBuffer(49, STD_LOOP_TIME);
   addIntToBuffer(51, lastLoopUsefulTime);
   addIntToBuffer(53, lastLoopTime);
  
   Serial.write(buffer,55);
  }else{
    sendErrorToBuffer(error_message);
  }
}

void serialIn_GUI(){
  byte id;
  
  if(Serial.available() > 0){               
    byte param = Serial.read(); 

    delay(10);
    byte inByte[Serial.available()];
    if(Serial.read() == SPLIT){

      switch (param) {
        case 1:
          if(Serial.available() >= 2){
            Kp = readIntFromBuffer();
          }
          break;
        case 2:
          if(Serial.available() >= 2){
            Ki = readIntFromBuffer();
          }
          break;
        case 3:
          if(Serial.available() >= 2){
            Kd = readIntFromBuffer();
          }
          break;
        case 4:
          if(Serial.available() >= 4){
            K = readFloatFromBuffer();
          }
          break;
        case 5:
          if(Serial.available() >= 2){
            setPoint = readIntFromBuffer();
          }
          break;
        case 6:
          if(Serial.available() >= 4){
            motorOffsetL = readFloatFromBuffer();
          }
          break;
        case 7:
          if(Serial.available() >= 4){
            motorOffsetR = readFloatFromBuffer();
          }
          break;
        case 8:
          if(Serial.available() >= 2){
            Kp_Wheel = readIntFromBuffer();
          }
          break;
        case 9:
          if(Serial.available() >= 2){
            serial_type = readIntFromBuffer();
          }
          break;
        case 10:
          if(Serial.available() >= 2){
            Kd_Wheel = readIntFromBuffer();
          }
          break;
         case 11:
          if(Serial.available() >= 2){
            error_message = readIntFromBuffer();
          }
          break;
      } //End switch
    } //if(Serial.read() == SPLIT)
  } //if(Serial.available() > 0)
}

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

void addIntToBuffer(int index, int value)
{
  buffer[index] = lowByte(value);
  buffer[index + 1] = highByte(value);
}

void addFloatToBuffer(int index, float value)
{
  u.fval = value;
  
  buffer[index] = u.b[0];
  buffer[index + 1] = u.b[1];
  buffer[index + 2] = u.b[2];
  buffer[index + 3] = u.b[3];
}

void sendErrorToBuffer(int error){
  byte buffer_error[3];
  
  error_message = error;
  
  buffer_error[0] = char('!');
  buffer_error[1] = lowByte(error);
  buffer_error[2] = highByte(error);
   
  Serial.write(buffer_error, 3);  
}

int readIntFromBuffer(){
  uint8_t lsb = Serial.read();
  uint8_t msb = Serial.read();
  Serial.flush();
  return (lsb + (msb <<8));
}

float readFloatFromBuffer(){
  u.b[0] = Serial.read(); 
  u.b[1] = Serial.read(); 
  u.b[2] = Serial.read(); 
  u.b[3] = Serial.read(); 
  Serial.flush();
  return u.fval;
}

I have added a error handler so we can send errors to the GUI that you half to acknowledged by sending a 0 value to the error_message to get it to stop sending the error to the GUI..

What do you think about it so far? The code is real dirty but I think you understand it.. The loop time is around 4-6ms when running it...

What do you think about it so far?

Possible options for speeding the transmital process:

  • reduce number of parameters to be tansmitted
  • transmit parameter only when value have changed
  • if you have 50 data to transmit, split the information and try sending 10 values per cycle.
    all info will be passed within 5 cycles = 500ms, which is acceptable
  • Combine these options

hello.
I'm looking for the kasBot v2.0 code, involving quadrature encoders. Couldn't find it on the "official" site.
Can you help me?

Andreas
Norway

Hi Andreas,
Sorry for that, I will send it today to Patrik for posting in his blog
http://www.x-firm.com/?page_id=145

I've decided to tackle an Arduino balancing robot for my final design project for my two year college program. I may make my own thread documenting my progress (similar to yours), but maybe not (your walk through is impressive).

Brian!

I was thinking about using:

sensors: RobotShop | Robot Store | Robots | Robot Parts | Robot Kits | Robot Toys

motors & wheels: RobotShop | Robot Store | Robots | Robot Parts | Robot Kits | Robot Toys

motor controller shield: RobotShop | Robot Store | Robots | Robot Parts | Robot Kits | Robot Toys

Arduino controller: RobotShop | Robot Store | Robots | Robot Parts | Robot Kits | Robot Toys

I will settle on a battery pack and voltage regulator once I final my component choices.

The MCU is 32-bit with a 16MHz crystal, the motors' RPM is 242 and the wheels are 2.5" diameter. Are these fast, large, and good enough to balance properly? My instructor seems to think there will be problems with different torques of each motor. How much validity is in that? Any other issues I'm overlooking before I start?

It does feel like I'm not entirely sure what I'm getting myself into, but I have a few months to figure it all out. This forum will make my life much easier; I salute you [smiley=beer.gif]

Brian!

Hi BrianBot,
Welcome and good luck for your project :slight_smile:

Looking at your shopping list, I have the following comments:
motor controller shield:
This device will definitly not cope with the amperes needed to feed your motors :-?
Look here (initial post)

motors & wheels:
The wheels are wide and soft, which is good for balancing
Wheel diameter could be higher
You need encoders (at least one). How will you afix them ??

My instructor seems to think there will be problems with different torques of each motor.
How much validity is in that?

This is addressed with the second encoder

Feel free to document you project here
This thread is now very visible from Google, this is convenient for discussion with non Arduino jerks

I was denied permission to use shields, anyway, as it was declared "too easy". I suppose that's convenient.

I got my hands on a free L298N H-Bridge: http://www.st.com/stonline/books/pdf/docs/1773.pdf
Unfortunately it only can handle spikes of 3 A (repetitive 2.5 A). Did you ever get an idea of what currents are used? I plan on making the bot relatively small...

Also, have you ever run into speed problems from the microcontroller? Is 16 MHz fast enough?

My understanding was that the sensors act as the feedback to tell the motors direction and speed, so a motor encoder is not necessary. I see you made the same statement in your initial post.

Check out the "murata girl" a unicycle robot that balances forward/back and left/right. It is at the 1:35 mark on the youtube video.

I'm trying to use the Kalman code described in this thread. When the angle of my bot changes rapidly, the filter can take 10 cycles to settle. In other words, the computed angle lags the accelerometer angle by as much as 10 cycles. This is making my bot slow to respond. Is this lag normal? Is there anything I can do to speed up the response?

Thanks!
Osman