Self balancing robot in 60 lines

Forum 2005-2010 (read only) General Exhibition
1

Hi guys. I am working on IMU's and various stabilization algorithms with many other friends here in Milan. We are working on a little production of robot kits and components. Here it is a demo of PICO kit used to build a very simple self balancing robot:


robot with auto get up funciton

here it is the original article:
http://www.gioblu.com/index.php?option=com_content&view=article&id=100:self-balancing-robot-per-noobs-p&catid=36:robotica&Itemid=34

(click on your language's flag to get Google Translate traduciton, beware, often is quite silly)

Servo left;
Servo right;

#define servoLeftPin     9
#define servoRightPin   10
#define servoZero       81
#define IRFront         0
#define IRBack          1
#define ledPin          13

int frontSense = 0;
int backSense = 0;
int orientation = 0;


void setup() {
pinMode(ledPin, OUTPUT);
left.attach(servoLeftPin);
right.attach(servoRightPin);
left.write(servoZero);
right.write(servoZero);
Serial.begin(9600);
}


void getOrientation() {
  frontSense = 0;
  backSense = 0;
  orientation = 0;
 for (int i = 0; i  90) delta = 90;
if(delta < -90) delta = -90;

left.write(81 - delta);
right.write(81 + delta);

Serial.print(orientation);
Serial.print("   ");
Serial.println(delta);
}

What do you think about it?
Any advice? :smiley:

2

Haha, cool! Have you made use of the sonic ranger? Maybe make it roam the room as the next step?

3

Yes that is the idea :stuck_out_tongue: , I am considering how to regulate throttle in both directions and how to steer.

4

Is that all of the code?

5

Yes, all in 60 lines. If you need auto get up function you can find it here:

6

Hey, gbm, something appears wrong with the code as posted above in this thread (I'm not saying your code is wrong, just that what you posted appears "mangled"). Can you repost the code (or correct your first post)?

:slight_smile:

7 
Servo left;
Servo right;

#define servoLeftPin     9
#define servoRightPin   10
#define servoZero       81
#define IRFront         0
#define IRBack          1
#define ledPin          13

int frontSense = 0;
int backSense = 0;
int orientation = 0;

void setup() {
pinMode(ledPin, OUTPUT);
left.attach(servoLeftPin);
right.attach(servoRightPin);
left.write(servoZero);
right.write(servoZero);
Serial.begin(9600);
}


void getOrientation() {
  frontSense = 0;
  backSense = 0;
  orientation = 0;
 for (int i = 0; i < 10; i++) {
  frontSense = analogRead(IRFront) + frontSense;
  backSense = analogRead(IRBack) + backSense;
  if (i == 9) {
   frontSense = frontSense / 10;
   backSense = backSense / 10;
   orientation = frontSense - backSense;
  }
 }
}


void loop() {
getOrientation();

float delta = orientation / 8;
if(delta > 90) delta = 90;
if(delta < -90) delta = -90;
left.write(81 - delta);
right.write(81 + delta);
Serial.println(orientation);
}

Ok sorry, something mangled my code ;), this is the rightone, only 50 lines :smiley:

sorry for double posting

8

Can it travel up an incline?

-transfinite

9

thats really cool using the sensors to measure the distance to the floor and alter it accordingly.

10

Hi guys. No, inclination is a deadzone for PICO, without a gyro/acc/IMU it can't know where is down, he only know the distance from his front and ground, and his back and ground. For this reason the only way to let it go on a inclinated plan (with only ir sensors) would be to move the battery according to reactions. But it is a littlebit complex.

11

Time for a youtube video!

12

Here it is:
PICO self balancing robot - YouTube (first video only delta)
PICO self balancing robot - Auto get up - YouTube (autogetup function)

do you like it :D?

now i am triyng to write in the simplest way possible a sort of inertial calculation, based on time passed inclinated in one direction
example:

if(delta > 0) inertia = inertia + 0.1;
f(delta < 0) inertia = inertia - 0.1;

this is a first try but is very effective. Not powerful in micro-corrections but very nice when the robot is drifting. If you try a project like this you surely find big problems to stabilize robot if it get speed in one direction. The key is to follow science laws. I get a sort of inertia data and I add it to servo correction. For this reason if the robot get speed in one direction inertia goes up and add power to servos to undo the inertia momentum.

13

Hi guys, I worked a lot and I implemented PID algoritm. IT IS INCREDIBLE!!! It Workssss!!! ;D ;D ;D ;D ;D ;D ;D ;D
I am the happiest man on the planet now!!!

Look @ the video:

I write an article based on PID, a sort of tutorial:
http://www.gioblu.com/index.php?option=com_content&view=article&id=102:che-cose-lalgoritmo-pid&catid=38:programmazione&Itemid=7

Here it is the code:

//Giovanni Blu Mitolo - Gioblu Robotics - released under Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
//use and enjoy free :-) - gioscarab[att]gmail.com
//Giovanni Blu Mitolo - Gioblu Robotics - materiale rilasciato sotto licenza Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
//usa pure e divertiti  :-) - gioscarab[att]gmail.com

#include  //Library used to interface servos - ibreria utilizzata per pilotare servi 
                    
Servo left; 
Servo right;

float time = 0;

#define servoLeftPin     9
#define servoRightPin   10
#define servoZero       81 //calibration 4 servos - calibrazione per servi
#define IRFront          2 
#define IRBack           3
#define ledPin          13 
#define leftIr           2 //proximity ir emitters - emettitori ir x sensori prossimità
#define rightIr          4


int frontSense = 0; //forward ir sensor - sensore ir anteriore
int backSense = 0;  //backward ir sensor - sensore ir posteriore
int orientation = 0;
int previousOrientation = 0;

float P = 0;
float I = 0;  //Il mesurement (empirism) - misurazione inerziale
float D = 0;
int fall = 0;       //value used to determine if the robot is fall
                    //variabile utilizzata per determinare se il robot è caduto
float kP = 12;      //used to tune
float kI = 430;
float kD = 20;

void setup() {
pinMode(ledPin, OUTPUT);
left.attach(servoLeftPin);
right.attach(servoRightPin);
left.write(servoZero);
right.write(servoZero);
Serial.begin(9600);
}


void getOrientation() {
  frontSense = 0;
  backSense = 0;
  orientation = 0;
 for (int i = 0; i < 10; i++) {
  frontSense = analogRead(IRFront) + frontSense;
  backSense = analogRead(IRBack) + backSense;
  if (i == 9) {
   frontSense = frontSense / 10;
   backSense = backSense / 10;
   orientation = frontSense - backSense;
  }
 }
}

/*Simplest thing ever
Stop wheels for 1/4 of a second
to get momentum to get up*/

/*La cosa piu' semplice del mondo
Stop alle ruote per 1/4 di secondo
per ottenere abbastanza momento per 
rialzarsi*/

void autoGetUp() {
if (fall == 25 || fall == - 25) {
 left.write(servoZero);
 right.write(servoZero);
 delay(250);
 fall = 0;
 I = 0;
}
}


void loop() {
autoGetUp();
getOrientation();
float previousTime = time;
time = millis();
float interval = time - previousTime;
      P = orientation / kP;
      I = I + (P * interval) / kI;
      D = (orientation - previousOrientation) / interval / kD;
      float PID = P + I + D;
if(P > 90) P = 90; //stop increase or decrease of the value
if(P < -90) P = -90; //costrizione del valore tra 90 e -90
if(orientation > 250) fall = fall + 1; 
if(orientation < -250) fall = fall - 1;
if(PID <= 1 && PID > 0) PID = 0; //cut off micro-corrections
if(PID >= -1 && PID < 0) PID = 0;
left.write(81 - PID);
right.write(81 + PID); 
previousOrientation = orientation;
Serial.print(P);
Serial.print("  ");
Serial.print(I);
Serial.print("  ");
Serial.print(D);
Serial.print("  ");
Serial.print(interval);
Serial.print("  ");
Serial.println(orientation);
}
14

Hi guys. I have a brainfight with friends about PID. I use (you can see it in the code or article) a "variable time PID". It is simple, I take the loop duration value (time - previousTime = interval) and I multiply that with I and D.
My engineer friends says that thing should not work and the pid MUST be used in fixed time. Their programs do thing and then if the loop duration is less then the fixed duration spleeps until get fixed duration.
I think is stupid to loose time in that way. They say that is important to have the same "weight" per acquisition. Simply every acquisition MUST have the same duration to be proportial. I use a variable (interval) that is multiplied to I and D that rise and lowdown porportional to acquisition duration. If a loop is very long interval is a big value, for this reason the weight of this value is more then others with smaller interval. My sistem works take a look:

Is there some coder/skilled that can clarify this question? :wink:

15

Your engineer friends are correct.

However if you have something you cobbled/modified that you got to work and are happy with it's performance then let it go.

Lefty

16

Hi retrolefty! Thank you for the answer.
This is not a cobbled/modified code. I wrote it down from the first raw.
I think this is only a new approach to PID.

void loop() {
float previousTime = time;
time = millis();
float interval = time - previousTime;
      P = orientation / kP;
      I = I + (P * interval) / kI;
      D = (orientation - previousOrientation) / interval / kD;
      float PID = P + I + D;
if(P > 90) P = 90; //stop increase or decrease of the value
if(P < -90) P = -90;
if(PID <= 1 && PID > 0) PID = 0; //cut off micro-corrections
if(PID >= -1 && PID < 0) PID = 0;
left.write(81 - PID);
right.write(81 + PID);
previousOrientation = orientation;
}

If I have a value that is proportional to interval duration and I use it as time is used in PID i get a value that gives a weight, proportional to time duration, to I and D values. If I(value) is adding and get a value that is very low, but has a long interval, that has more influence to trend then a value with a low interval. I think this is logically demostrated by my last video and this 3 raws of text ;).

Think about extending your code with the standard version. This tunes itself automatically. The other has problems and must be regulated every time you add routines or functions. Another bad thing is the sleep time loss in every loop.

17

There's a whole article here that suggests you can have a variable delta T (time step), and I don't see why it wouldn't work - that's how integration works.

http://thecodebender.com/journal/2009/3/30/designing-a-pid-making-your-robot-get-from-point-a-to-point.html

Andrew

18

Very nice bot. Nice design.