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.

Cool :)

I thought robots on two wheels only balance. How does it move forward and backward?

  I guess most do just balance but I wanted to try and add some movement and I thought trying to make it autonomous would be a bit too challenging.. Anyway I am using a really crappy remote control from a small car.. The problem with my remote is that I don't have analog sticks to make it move I only have 4 buttons(forward,back,right,left) so that is why it is kind of jerky when changing direction from forward to backward or turning.. I might try and sacrafice a better remote controll car in the future.. hehehe..

Very nice! I'm impressed! I want to build one sometime... I am building a (small) butler robot and I would like to make it balance like that...

I have a few questions:

What kind of resolution the encoders need to be? I saw initialization for only one encoder? Do you use both?

What is the max robot speed when trying to recover from a big angle fall? 

I saw you added a weight on top of the robot, what happens if you add a tray in the front of the robot and add the weight there, like the robot would have an object in it's arms? How would the robot calculate the balance point then?

My long term purpose is to build a robot somewhat like this one (a "bit" simpler): 

     Thanks for the comment.. and to answer your questions.. The encoders are capable of running at a resolution of 400 but I am only running them at 100 with a gear reduction of 50:1 on the motor... Yes you are correct that I am only using 1encoder.. This was the first robot that I setup that uses encoders at all so initially they aren't required to get it to balance .. They are required to get it to balance in 1 spot or on uneven ground.. I am not sure what the max speed when recovering from a big fall would be as I don't have any failsafe built in to compensate for a fall, at least not yet.. The weight on the top is the battery and it is there to make balancing much easier.. The higher the weight and taller the robot the easier it is to balance.. I am sure with the proper encoders and coding you could easily compensate for different weights being placed in the "arms" of one of these robots.. I have tried adding weight offset at the top of mine and with the encoders compensating it doesn't seem to have much effect.. Anyway if you have any more questions feel free to ask..

like dallaby said, putting weight at the top of a balanced pole type thing actually makes it easier to balance. However, if you're going to have it carry uneven weight, you might want a force sensor in the hands to compensate.

Thanks for your answers guys.

I have build a Simple Balancing Robot using servos and a Ping sensor to measure the distance to the ground. It had problems balancing and I never succeeded to make it stable for more than a minute or so. I mounted the batteries on top of the robot, so I know about the weight at the top thing. I think there were 2 problems with my setup, the Ping sensor values would offset if the tilt angle would be more than a couple of degrees (probably because of the way the sound is reflected by the ground surface at different angles) and the second problem was servos are not fast enough to recover from a tilt angle of more than 5 degrees or so. This is why I was asking those questions. Oh, about the weight carried by the robot's arms, I think the robot will just tilt back a bit so the new COG is placed above the wheels again for a perfect balance. Correct me if I'm wrong, please. I have a gyro and a accelerometer, but I am still debating what motors I should buy, I want them to be silent and have built in encoders and of course, not cost more than $30 a piece. I know drill motors would do, but they are awfully noisy, and a balancing robot uses the motors a lot. I think the total weight of the robot will be around 15kg, less if I use LiPo batteries and about one meter tall.

   I am glad to hear that you have tackled this in the past.. I know when I first looked at them I was going to try the ping/ir sensor route but opted out... I do think servos would be fast enough for a small balancer if you have the PID loop tight enough as one of the small ones I build just had servo like motors, and I have seen servo motor ones before on the net.. You are kind of right about readjusting the COG when something is placed in its arms, this is where the encoders will come in really handy as you can code them to help you determine the correct COG.. I know the motor delima as I battled that one myself ALOT..Trying to find motors with enough power and speed and with encoders.. NOT EASY...The motors on this balancer are not the best for this type of application as the rapid forward and reverse can strip the gears inside(I found out the hard way), but trying to get encoders with motors is not easy either.. If you ever find the perfect motor please share, if not the only thing I can recommend is trying to get one with a great gear box and building your own encoder but try and make it as high resolution as possible.. Also below is a few more links to previous balancers I made so feel free to use that as motivation.. enjoy..

http://www.youtube.com/watch?v=_2fzz_gO27U - same as above but different motors..

http://www.youtube.com/watch?v=xQcq8j3kGTE - older pvc model with drill motors and a really loud motor controller

http://www.youtube.com/watch?v=9bXmjumSo3c - first one with servo type motors (solarbotics)

 

Do you think these motors will work for a bigger balancing robot? Probably the encoders will need a higher resolution, but I am more concerned if the motors will actually work.

Thanks.

  I think you can pretty much make any work but those ones sure are expensive, especially if you have to rework the encoders.. You would probably be better off buying wipermotors for a car at a fraction of the cost, and building your own encoders.. Just a thought..

Awesome bot! I'm starting to collect useful parts for general use; would you mind listing the parts you used for this bot and where you got them? Thanks!