Go Down

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

Binu Madola

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

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 second
int 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 output
float   Kp = 40;                                           //proportional gain                   
float   Ki = 1;                                            //integral gain               
float   Kd = 1;                                            //derivative gain
float 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 unicycle
float Pitch;

//for SyRen
int SyRen_Output;                                          //final motor output value
float Int_Output;                                          //used to convert pid output to int


void 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:

http://www.youtube.com/watch?v=mEee1iI6emE




Binu Madola

hai,
     I got gyro angle and acclerometer value and calibarated using Goly filter.
Then how can I fuse together and get error values

dotrar

Theres already a massive thread about autobalancing robots in page 2 of the forums.

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

Go Up