Go Down

### Topic: SELF BALANCING ROBO (Read 4445 times)previous topic - next topic

##### Apr 12, 2013, 08:45 am
Hai,
Iam doing a project on Self balancing robo using accelerometer ,gyroscope,ATMega328,and servo motor.
Has anyone try this before? Give me some help with this project.

#### tgfeminella

#1
##### Apr 12, 2013, 01:41 pm
I just built a ride-on, self-balancing unicycle, it works the same way as a self-balancing robot. I used an Arduino Uno with a Sparkfun 6dof digital IMU for orientation sensing. A library that does fusion filtering for this IMU, and lots of info, is available here: http://bildr.org/2012/03/stable-orientation-digital-imu-6dof-arduino/. If you do go this route for your IMU, you need to change two things he used in his example sketch: "Angles[3]" to "ypr[1]", and "GetEuler (angles)" to "YawPitchRoll (ypr)". This must be done, or else the library will factor in the yaw measurement (I don't know why) and invert the pitch angles when the IMU is turned around. Thanks to this library, getting angle measurements from this IMU couldn't be easier.

I used a SyRen 25 motor driver which can put out 25 amps continuous at 24v to run my electric scooter motor. It uses packet serial communication to send PWM commands to the driver. If your robot, and motors, are small enough, you might be able to get away with using an Arduino motor shield which can supply 2a to two dc motors at up to 12v. This would probably be the easiest way to do it. I only needed to use one motor, but unless you need your bot to turn, it works the exact same way.

You'll need a Proportional Integral Derivative (PID) controller to calculate power output from the amount of difference between current angle measurement and the set balancing point (error). There is a library available on the Arduino site, but I used a different one.

This was my first Arduino project and I was able to build this and get it balancing with no prior programming or soldering experience. I learned everything I needed to know from some internet research.

I found this thread to be very helpful: http://arduino.cc/forum/index.php/topic,8871.0.html. It included a step by step guide to building a balancing robot. This is where I got my PID controller from, be sure to read the whole thing. There is also a newer thread that continues this discussion in the robotics forum. You should try posting in there to get more help.

Here is my code, you could easily modify this to use another type of motor driver:

Code: [Select]
`//code for homemade self balancing unicycle://sparkfun 6dof IMU//PID output control//Dimension Engineering SyRen 25 motor driver//Arduino IDE 1.0#include <FreeSixIMU.h>                                    //fusion filtering library#include <FIMU_ADXL345.h>                                  //accelerometer library#include <FIMU_ITG3200.h>                                  //gyro library#include <Wire.h>//timing: sets timing at roughly 100 loops per secondint STD_LOOP_TIME = 9;             int lastLoopTime = STD_LOOP_TIME;int lastLoopUsefulTime = STD_LOOP_TIME;unsigned long loopStartTime = 0;//for IMU:float ypr[3];                                              //yaw pitch roll (only pitch is necessary for one-axis balance in this application)FreeSixIMU sixDOF = FreeSixIMU();                          //set the FreeSixIMU object//for PID controller: float K = 0.2;                                             //multiple of PID outputfloat   Kp = 40;                                           //proportional gain                    float   Ki = 1;                                            //integral gain               float   Kd = 1;                                            //derivative gainfloat last_error = 0;float integrated_error = 0;float pTerm = 0, iTerm = 0, dTerm = 0;float error;float BalancePoint = 0;                                    //the angle at which the PID controller will attempt to balance the unicyclefloat Pitch;//for SyRenint SyRen_Output;                                          //final motor output value float Int_Output;                                          //used to convert pid output to intvoid setup(){  Serial.begin(9600);                                      //set baud rate  Wire.begin();    delay(5);  sixDOF.init();                                           //begin the IMU  delay(5);          Serial.write(170);                                       //initializes SyRen controller and lets it determine baud rate}int updatePid(float BalancePoint, float Pitch)   {         //PID control: gets angles and calculates output from -127 to 127  sixDOF.getYawPitchRoll(ypr);  Pitch = ypr[1];  error = BalancePoint - Pitch;   pTerm = Kp * error;  integrated_error += error;  if (integrated_error > 20) { integrated_error = 20; }  if (integrated_error < -20) { integrated_error = -20; }    iTerm = Ki * integrated_error;  dTerm = Kd * (error - last_error);                              last_error = error;  return (K*(pTerm + iTerm + dTerm));}void loop(){    Int_Output = updatePid(BalancePoint, Pitch);             //runs PID controller and returns output     if (Int_Output > 127) { Int_Output = 127; }              //constrains PID controller output to usable values  if (Int_Output < -127) {Int_Output = -127; }        SyRen_Output = abs(Int_Output);                        // number output will always be positive 0 to 127 (motor conrtoller only accepts positive integers in this range)    if (Int_Output > 0) {                                  //if output from PID is positive (unicycle is leaned back), motor will drive in reverse    Serial.write(128);                                     //serial address of SyRen motor driver  Serial.write(1);                                         //this determines direction of output  Serial.write(SyRen_Output);                              //power output  Serial.write((128+1+SyRen_Output) & 0b01111111);         //checksum to confirm correct commands (SyRen will not act on command packet if these values are different)    }    if (Int_Output < 0) {                                  //if output from PID is negative (unicyle is leaned forward), motor will drive in forward     Serial.write(128);  Serial.write((byte)0);  Serial.write(SyRen_Output);  Serial.write((128+(byte)0+SyRen_Output) & 0b01111111);    }                     //loop timing control  lastLoopUsefulTime = millis()-loopStartTime;  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);  lastLoopTime = millis() - loopStartTime;  loopStartTime = millis();  `

Here is a video of my first attempt to ride the Unicycle:

#2
##### Apr 14, 2013, 09:51 am
hai,
I got gyro angle and acclerometer value and calibarated using Goly filter.
Then how can I fuse together and get error values

#### dotrar

#3
##### Apr 15, 2013, 09:14 am

http://arduino.cc/forum/index.php/topic,60170.0.html

Go Up