Let's Make Robots!

YBot - Balancing Arduino Robot

Balances on 2 wheels, r/c controlled

      Well I am not sure I understand the phrase 'time to build' but that would be the time it took me to cut the parts and mount the motors, getting it to work was much much longer.. I learned alot and anyone that wants to take on this project will hopefully read through this and pick up some important tips.. Well a few quick tips, you don't need to spend tons of money I have built 4 of these now with different motors and probably the best balancer was a really small one with solarbotics motors(5.00/ea) and a homemade motor controller.. The only thing you need that is expensive is a IMU, I have tried a couple so my findings are a 75 degree/s gyro and a 1.5g accelerometer... I am sure you could get others to work(I tried a 150 degree/s gyro successfully) but the most important would be the gyro..  Anyway 2 another things that always comes up in conversation when talking about balancing robots are 'zero backlash' motors and the infamous 'kalman filter'.. Ok for the wheels yes it would be great but you can get almost anything to balance so it depends on your application, and pretty much the exact same thing for the kalman filter, the robot in this post is using a simple complimentary filter as you can see by the code below.. One last tip when starting out is to try and find very soft tires as these will increase the friction which will make a HUGE difference when trying to get it to balance.. Most people tell you to start on carpet which is also a HUGE help.. But seriously soft rubber tires that can be deflated would be best.. I think that is about it so I am sharing the code below for use with an Arduino microcontroller.. If you have any questions please feel free to ask..

This code is not completely cleaned up, my appologies..

**Code for Arduino**


#define NUMREADINGS 5

//**********************Setup Variables for the IR Sensor*********************
//****************************************************************************
int IRpin = 5;
unsigned long IRDT = 0;
int ledpin = 13;
float distance = 0.00;
//****************************************************************************
//****************************************************************************


//**********************Setup Variables for Gyro and Accel********************
//****************************************************************************
int readings[NUMREADINGS];                // the readings from the analog input
int index = 0;                            // the index of the current reading
int total = 0;                            // the running total
double average = 0.00;                          // the average
int rVal = 0;
int refPin = 1;
int rPin = 0;
int yPin = 2;
//*****************************************************************************
//*****************************************************************************


//********************Setup other Balancing Variables**************************
//*****************************************************************************
double vTorque = 0.0;
int m1Dir1 = 8;
int m1Dir2 = 9;
int m1PWM = 11;
int m2Dir1 = 7;
int m2Dir2 = 6;
int m2PWM = 10;


double vAngle = 0.0;
double vHighPass = 0.0;
double vLowPass = 0.0;
double vSetpoint = 4.00;
double vRate = 0.0;
double last_angle = 0.0;
double Pterm = 0.0;
double Iterm = 0.0;
double Istate = 0.0;
double Dterm = 0.0;
double Pgain = 1.9;
double Igain = 0.12;
double Dgain = 2.7;

int mydt = 5;
unsigned long last_PID = 0;
unsigned long lastread = 0;
double vYaxis = 0.00;

//*****************************************************************************
//*****************************************************************************

//***********************Variables for driving*********************************
//*****************************************************************************
#define encoder0PinA  3
#define encoder0PinB  4
int vrDist = 0;
int vlDist = 0;
int last_vrDist = 0;
int last_vlDist = 0;
int vrencodeb = 0;
double vrSpeed = 0;
double last_vrSpeed = 0;
int vlSpeed = 0;
int vTurn = 0;
int vTurnR = 0;
int vTurnL = 0;
int vDrive = 0;
int vAdjust = 0;
int vBalAdjust = 0;
int vrDirection = 0;
int last_vrDirection = 0;
int myBaldt = 50;
unsigned long lastBalupdate = 0;
double sPterm = 0.0;
double sIterm = 0.0;
double sIstate = 0.0;
double sDterm = 0.0;
double sPgain = 3.5;
double sIgain = 1.0;
double sDgain = 4.5;
double vrTarget_Speed = 0;


void setup()
{
  Serial.begin(9600);                     // initialize serial communication with computer
 
  //Setup Motor pins******************* 
  pinMode(m1Dir1,OUTPUT);
  pinMode(m1Dir1,OUTPUT);
  pinMode(m2Dir1,OUTPUT);
  pinMode(m2Dir2,OUTPUT);
  pinMode(m1PWM,OUTPUT);
  pinMode(m2PWM,OUTPUT);
  //***********************************
 
  //*Calibrate balance point************
  delay(100);
  /*
  for (int a = 0; a < 20; a ++)
  {
  readings[a] = analogRead(yPin); // read from the sensor
  if (a != 5)
  total += readings[a];               // add the reading to the total
  delay(40);
  Serial.println(readings[a]);
  }
  */
 
  pinMode(encoder0PinA, INPUT);
  pinMode(encoder0PinB, INPUT);
  digitalWrite(encoder0PinA, HIGH);       // turn on pullup resistor
  digitalWrite(encoder0PinB, HIGH);
  attachInterrupt(1, rencoder, FALLING);
 
 
  rVal = analogRead(refPin);
  average = analogRead(yPin);          // calculate the average
  vSetpoint = average - rVal;
 
 //***********************************
 
}

void loop()
{

  //**********************Main Code loop*******************************************
  //*******************************************************************************
  //*******************************************************************************

  Complimentaryfilter();            //run complimentary filter function


  Calculate_Torque();        //run torque calculations through PID
 
 
  Calculate_Speed();
 
 
  Read_IR();

  //*****************End Main Code Loop********************************************
  //*******************************************************************************
  //*******************************************************************************
}

//*******************************************************************************
//*******************************************************************************
//********************Setup PID loop to determine torque*************************

void Calculate_Torque()
{
  if((millis()-last_PID) >= mydt)          // sample every dt ms -> 1000/dt hz.
  {
    last_PID = millis();


    Pterm = vAngle * Pgain;
    Istate = (Istate + vAngle);
    Iterm = Istate * Igain;
    Dterm = (vAngle - last_angle) * Dgain;

    vTorque = Pterm + Dterm + Iterm;

    last_angle = vAngle;

    vTorque = constrain(vTorque,-255,255);
   
    Drive_Motor(vTorque);     //Send torque to motor drive code
  }
  //*************End Setup PID Loop************************************************
  //*******************************************************************************
  //*******************************************************************************
}

//*********************Drive Motors**********************************************
//*******************************************************************************
//*******************************************************************************

int Drive_Motor(int vTorque)
{
  if (vTorque >= 0)                                  //indicates drive motors forward
  {
   if (vTurn >= 0)                                 //indicates turn to the right
     {
      vTurnR = vTorque + vTurn;
      vTurnL = vTorque - vTurn;
      digitalWrite(m1Dir1, LOW);                       //Signals H-Bridge to move power forward
      digitalWrite(m1Dir2, HIGH);
     
      if (vTurnL < 0)
        {
    digitalWrite(m2Dir1, HIGH);                       //Signals H-Bridge to move power forward
        digitalWrite(m2Dir2, LOW);
        }
      else
        {
        digitalWrite(m2Dir1, LOW);                       //Signals H-Bridge to move power forward
        digitalWrite(m2Dir2, HIGH);
        }
      vTurnL = abs(vTurnL);
      vTurnR = abs(vTurnR);
      analogWrite(m1PWM,vTurnR);
      analogWrite(m2PWM,vTurnL);
     }
   
   if (vTurn < 0)                                 //indicates turn to the left
     {
      vTurnR = vTorque + vTurn;
      vTurnL = vTorque - vTurn;
      digitalWrite(m1Dir1, LOW);                       //Signals H-Bridge to move power forward
      digitalWrite(m1Dir2, HIGH);
     
      if (vTurnL < 0)
        {
    digitalWrite(m2Dir1, HIGH);                       //Signals H-Bridge to move power forward
        digitalWrite(m2Dir2, LOW);
        }
      else
        {
        digitalWrite(m2Dir1, LOW);                       //Signals H-Bridge to move power forward
        digitalWrite(m2Dir2, HIGH);
        }
      vTurnR = abs(vTurnR);
      vTurnL = abs(vTurnL);
      analogWrite(m1PWM,vTurnR);
      analogWrite(m2PWM,vTurnL);
     }
  }

else if (vTorque < 0)
  {
   vTorque = abs(vTorque);
   if (vTurn >= 0)                                 //indicates turn to the right
     {
      vTurnR = vTorque - vTurn;
      vTurnL = vTorque + vTurn;
      digitalWrite(m2Dir1, HIGH);                       //Signals H-Bridge to move power forward
      digitalWrite(m2Dir2, LOW);
     
      if (vTurnR < 0)
        {
    digitalWrite(m1Dir1, LOW);                       //Signals H-Bridge to move power forward
        digitalWrite(m1Dir2, HIGH);
        }
      else
        {
        digitalWrite(m1Dir1, HIGH);                       //Signals H-Bridge to move power forward
        digitalWrite(m1Dir2, LOW);
        }
      vTurnL = abs(vTurnL);
      vTurnR = abs(vTurnR);
      analogWrite(m1PWM,vTurnR);
      analogWrite(m2PWM,vTurnL);
     }
   
   if (vTurn < 0)                                 //indicates turn to the left
     {
      vTurnR = vTorque - vTurn;
      vTurnL = vTorque + vTurn;
      digitalWrite(m2Dir1, HIGH);                       //Signals H-Bridge to move power forward
      digitalWrite(m2Dir2, LOW);
     
      if (vTurnR < 0)
        {
    digitalWrite(m1Dir1, LOW);                       //Signals H-Bridge to move power forward
        digitalWrite(m1Dir2, HIGH);
        }
      else
        {
        digitalWrite(m1Dir1, HIGH);                       //Signals H-Bridge to move power forward
        digitalWrite(m1Dir2, LOW);
        }
      vTurnR = abs(vTurnR);
      vTurnL = abs(vTurnL);
      analogWrite(m1PWM,vTurnR);
      analogWrite(m2PWM,vTurnL);
     } 

  }
 
 


  //******** End of Motor Drive Function*******************************************
  //*******************************************************************************
  //*******************************************************************************
}




//**************************Complimentary Filtering*************************************
//*******************************************************************************
void Complimentaryfilter()
{

  if((millis()-lastread) >= mydt)          // sample every dt ms -> 1000/dt hz.
  {
    lastread = millis();
   
    rVal = analogRead(refPin);
   
    vRate = (analogRead(rPin))- (rVal + 14.00);               // read from the gyro sensor
    vYaxis = (analogRead(yPin))-(rVal - vSetpoint);
   
    //Calculate angle
   
    vAngle += vRate * 0.1;
    vHighPass = 0.97 * vAngle;
    vLowPass = 0.03 * vYaxis;
    vAngle = vHighPass + vLowPass;
 

  }
  //***********************************************************************
  //*******************End Kalman Filtering********************************
}

//********************Calculate encoder************************************
//*************************************************************************

void rencoder()
{
  if (digitalRead(encoder0PinB) == HIGH) vrDist ++;
  if (digitalRead(encoder0PinB) == LOW) vrDist --;
}

//********************Calculate speed**************************************
//*************************************************************************

void Calculate_Speed()
{
   if((millis()-lastBalupdate) >= myBaldt) 
  {
    lastBalupdate = millis();
   
    vrSpeed = vrDist - last_vrDist;
    vrSpeed = (vrSpeed / myBaldt);
    vrSpeed = vrSpeed * 15.5;
   
    last_vrDist = 0;
    vrDist = 0;
   
    sPterm = vrSpeed * sPgain;
    sIstate = (sIstate + vrSpeed);
    sIterm = sIstate * sIgain;
    sDterm = (vrSpeed - last_vrSpeed) * sDgain;
   
    last_vrSpeed = vrSpeed;
   
    vrSpeed = 0.0;
   
    vrSpeed = sPterm + sDterm;
   
    vrSpeed = map(vrSpeed,-400,400,-30,30);
  
    if (distance < 50.0){
      vrTarget_Speed = 5;
      digitalWrite(ledpin, HIGH);
    }
    else{
      vrTarget_Speed = 0;
      digitalWrite(ledpin, LOW);
    }
   
    vrSpeed = vrSpeed + vrTarget_Speed;
    //Serial.println(vrSpeed);
  
    vAngle = vAngle - vrSpeed;
  }
}

//********************Read IR**********************************************
//*************************************************************************

void Read_IR()
{
  if((millis()-IRDT) >= 100) 
  {
    IRDT = millis();
    float volts = analogRead(IRpin)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
    distance = 65*pow(volts, -1.10);
    //Serial.println(distance);
  }
}

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

Wow!!

 

Hi, it's been a while


It's now balancing, see it there
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1282384853/0


I wouldn't have succeded without coming here first

Thanks!!!

   I am really glad to see you were able to get it balancing so well.. And the design looks good and solid.. Kind of a great feeling when they finally stand on their own eh!.. hehehehe

Hi dallaby

Thanks for the goog words, I understand the feeling you described
Thanks again for your help

Yves

Awesome! Congratulations!

Hi Dallaby,

I am very impressed by the compactness of your core code, especially Complimentaryfilter()
 and Calculate_Torque()
Can't wait to give it a try
I ordered from Pololu:
   2 x #1443 29:1 Gearmotor with 64 CPR Encoder (350RPM @ 12V)
   1 x #708 Dual VNH2SP30 Motor Driver Carrier MD03A
   1 x #1083 Pololu Aluminum Mounting Hub for 6mm Shaft pair
   1 x #1084 Pololu metal Bracket pair
   1 x #1435 Pololu Wheel 90x10mm pair
   1 x #1430 Pololu Wheel 80x10mm pair

Thanks for your help in reentering this "fight for Balance"

Kas, a friend of mine designed an Arduino compatible board that mates with the VNH2SP30 motor driver, he gives away boards if you pay the shipment. Take a look here: http://www.billporter.info/?p=286

Hehe, I'm famous!

 

Thanks for the link, amazing site with lot of Arduino goodies

Do you happen to know what is the "thing" on the right of photo #5 (from top)
Batteries ??

I just received the Pololu motor, they look nice and sturdy 
I will report my findings after I test them

If you mean the small thing that on most pictures is on the right of the motor controller, that is a switching voltage regulator for the electronics (5v) so no power is wasted through heat (from 12V...). It's from Dimension Engineering. You can replace it with a regular 7805, but have a heat radiator mounted on it.

About the motors, I watched a few balancing robot videos on youtube and one of them was clicking really annoying. I hope it's not gonna be the case with the pololu motors...

How are the wheels? I've been "balancing" between buying a pair or cutting them on my cnc.