Balancing robot for dummies

Yeah I guess that would be true because of the moment of inertia. So ill move the battery toward the top and mount the IMU close to it.

I ordered the 6dof IMU board from sparkfun tonight!

What do you guys think of this motor and encoder setup??

http://www.robotshop.com/cytron-12v-430rpm-15-57oz-gear-motor-encoder-2.html

Sounds like you are thinking building a big one :). They look pretty powerful for sure, just make sure that your battery source can handle the load. Two of these motors will draw around 2A and the stall current will be much higher.

Hi guys, i want to buy this dual driver Pololu - Dual MC33926 Motor Driver Carrier for a 29:1 pololu motor. As regard to you it's ok? Why is necessary this driver? i say this because, this driver it's ok until 9 A, but the motor requires 5 A max. Maybe this http://www.robot-italy.com/product_info.php?cPath=83_30&products_id=2386?

So I am really struggling to pick out the right motor. I really want to stick with my motor shield but it has a max of 2 amps. Most motors that I am finding have low free running current but the stall current exceeds my motor shield. Do these type of bots push the motors to their stall current? I do not see my design pushing the motor anywhere close to its stall current?

I've bought the pololu 29:1 and the relative motor control. So i've a questions: How connect the wires of motor and encoder to arduino and motor control?

Hi all,

Started a project around 2 months ago to build an autonomous robot. The wheels and motors I'm using are as follows: http://www.technobotsonline.com/banebot-wheel-124x20mm-1-2-hex-shore-50.html, Pololu - 30:1 Metal Gearmotor 37Dx52L mm 12V with 64 CPR Encoder (No End Cap).

Although I am able to balance the robot using a PID algorithm, I am finding it tough to control the position of the robot (via the encoders). I have a tilt sensor sending me the angle the robot is at and setting the motorspeed accordingly. To control the position of the robot I have been varying the reference angle to try and move the robot towards a reference point(eg: x = 0) to keep the robot in the same position but I have not had great results.

State space control would be best to control multiple inputs/outputs but I am not sure how this can be done on the Arduino Uno, any ideas?
If state space isn't used how is the best way to approach the problem I am encountering with the position?

The code I am using is as follows:

#include "DualVNH5019MotorShield.h"
#include "math.h"

enum PinAssignments {
  encoderPinA = 2,   // right
  encoderPinB = 3,   // left
  clearButton = 12    // another two pins
};


DualVNH5019MotorShield md;
int sensorPin = A2;
float setpointang = 501.0;
int oldangle, oldangle_1, oldangle_2, angle, newangle, current_1, current_2, errorpos, a, b, c = 0;
double sumangle, output, oldpos, oldpos_1, oldpos_2, derivpos, integpos = 0.0;
int Kp = 20; 
int ZoneA = 2000;
int ZoneB = 1000;
float motorspeed, deriv, error, errorposctrl;
double Kd = 0.3;   
float freq = 66.67;
float T = ((1/freq)*1000)-0.5;
float Kp_1 = 1.00 ;
float posScaleA = 500.0;
float posScaleB = 1000.0;
float posScaleC = 2000.0;


volatile unsigned int encoderPos = 0;  // a counter for the dial
unsigned int lastReportedPos = 1;   // change management
static boolean rotating=false;      // debounce management

// interrupt service routine vars
boolean A_set = false;              
boolean B_set = false;


void setup()
{
  
  Serial.begin(115200);
  md.init();

  TCCR1B = TCCR1B & 0b11111000 | 0x01;

  pinMode(encoderPinA, INPUT); 
  pinMode(encoderPinB, INPUT); 
  pinMode(clearButton, INPUT);
 
  // turn on pullup resistors
  digitalWrite(encoderPinA, HIGH);
  digitalWrite(encoderPinB, HIGH);
  digitalWrite(clearButton, HIGH);

  attachInterrupt(0, doEncoderB, RISING);  //CHANGE
}


void loop()
{

angle = analogRead(sensorPin); 

if ((angle < 300) || (angle > 700))
  {
    md.setM1Speed(0);
    md.setM2Speed(0);
  }
else if ((current_1 < 6000) || (current_2 < 6000) )
  {

      //PID
      oldangle_2 = oldangle_1;
      oldangle_1 = oldangle;
      oldangle = newangle;
      newangle = (setpointang - angle); 
      deriv = ((newangle + (3*oldangle) -(3*oldangle_1) - oldangle_2)*(freq/6)); 
      
      error = (Kp*(newangle) + Kd*(deriv));
      
      //Translating PID output value to a value within the range of the motor driver
      motorspeed = ((error/511)*400); 

      //Driving the motors
      md.setM1Speed(motorspeed);
      md.setM2Speed(motorspeed);
      
//---------------------------------------------------------------------------------------------------------------------------------------------------------------Angle Control End
//Serial.println(analogRead(A3));   //to plot

  if (lastReportedPos != encoderPos) {   
    if (motorspeed < 0)
    {
      errorpos = errorpos - (lastReportedPos - encoderPos);
    }
    else if (motorspeed > 0)
    {
      errorpos = errorpos + (lastReportedPos - encoderPos);
    }   
    lastReportedPos = encoderPos;
  }



errorposctrl = errorpos; 
Serial.print("   errorposctrl:  ");
Serial.print(errorposctrl);

float a = (errorposctrl/posScaleA);
float b = (errorposctrl/posScaleB);
float c = (errorposctrl/posScaleC);


if (abs(errorposctrl) > ZoneA)
{
   setpointang = setpointang - (a);
}
else if (abs(errorposctrl) > ZoneB)
{
   setpointang = setpointang - (b);
}
else 
{
   setpointang = setpointang - (c);
}


Serial.print("     a:  ");
Serial.print(a);
Serial.print("    b:   ");
Serial.print(b);
Serial.print("     c:   ");
Serial.println(c);
//






current_1 = (analogRead(A0)) * 34;
current_2 = (analogRead(A1)) * 34;


//Serial.print("    errorpos:   ");
//Serial.print(errorpos);
//Serial.print("     encoderPos:   ");
//Serial.println(encoderPos);

//delay((int)T);

  }
}





//// Interrupt on A changing state
//void doEncoderA(){
//      encoderPos += 1;
//}

// Interrupt on B changing state, same as A above
void doEncoderB()
{
      encoderPos += 1;
}

Thanks in advance,
MAAhelp

MAAhelp:
Hi all,

Started a project around 2 months ago to build an autonomous robot. The wheels and motors I'm using are as follows: http://www.technobotsonline.com/banebot-wheel-124x20mm-1-2-hex-shore-50.html, Pololu - 30:1 Metal Gearmotor 37Dx52L mm 12V with 64 CPR Encoder (No End Cap).

Although I am able to balance the robot using a PID algorithm, I am finding it tough to control the position of the robot (via the encoders). I have a tilt sensor sending me the angle the robot is at and setting the motorspeed accordingly. To control the position of the robot I have been varying the reference angle to try and move the robot towards a reference point(eg: x = 0) to keep the robot in the same position but I have not had great results.

State space control would be best to control multiple inputs/outputs but I am not sure how this can be done on the Arduino Uno, any ideas?
If state space isn't used how is the best way to approach the problem I am encountering with the position?

The code I am using is as follows:

#include "DualVNH5019MotorShield.h"

#include "math.h"

enum PinAssignments {
  encoderPinA = 2,   // right
  encoderPinB = 3,   // left
  clearButton = 12    // another two pins
};

DualVNH5019MotorShield md;
int sensorPin = A2;
float setpointang = 501.0;
int oldangle, oldangle_1, oldangle_2, angle, newangle, current_1, current_2, errorpos, a, b, c = 0;
double sumangle, output, oldpos, oldpos_1, oldpos_2, derivpos, integpos = 0.0;
int Kp = 20;
int ZoneA = 2000;
int ZoneB = 1000;
float motorspeed, deriv, error, errorposctrl;
double Kd = 0.3;   
float freq = 66.67;
float T = ((1/freq)*1000)-0.5;
float Kp_1 = 1.00 ;
float posScaleA = 500.0;
float posScaleB = 1000.0;
float posScaleC = 2000.0;

volatile unsigned int encoderPos = 0;  // a counter for the dial
unsigned int lastReportedPos = 1;   // change management
static boolean rotating=false;      // debounce management

// interrupt service routine vars
boolean A_set = false;             
boolean B_set = false;

void setup()
{
 
  Serial.begin(115200);
  md.init();

TCCR1B = TCCR1B & 0b11111000 | 0x01;

pinMode(encoderPinA, INPUT);
  pinMode(encoderPinB, INPUT);
  pinMode(clearButton, INPUT);

// turn on pullup resistors
  digitalWrite(encoderPinA, HIGH);
  digitalWrite(encoderPinB, HIGH);
  digitalWrite(clearButton, HIGH);

attachInterrupt(0, doEncoderB, RISING);  //CHANGE
}

void loop()
{

angle = analogRead(sensorPin);

if ((angle < 300) || (angle > 700))
  {
    md.setM1Speed(0);
    md.setM2Speed(0);
  }
else if ((current_1 < 6000) || (current_2 < 6000) )
  {

//PID
      oldangle_2 = oldangle_1;
      oldangle_1 = oldangle;
      oldangle = newangle;
      newangle = (setpointang - angle);
      deriv = ((newangle + (3oldangle) -(3oldangle_1) - oldangle_2)(freq/6));
     
      error = (Kp
(newangle) + Kd*(deriv));
     
      //Translating PID output value to a value within the range of the motor driver
      motorspeed = ((error/511)*400);

//Driving the motors
      md.setM1Speed(motorspeed);
      md.setM2Speed(motorspeed);
     
//---------------------------------------------------------------------------------------------------------------------------------------------------------------Angle Control End
//Serial.println(analogRead(A3));   //to plot

if (lastReportedPos != encoderPos) {   
    if (motorspeed < 0)
    {
      errorpos = errorpos - (lastReportedPos - encoderPos);
    }
    else if (motorspeed > 0)
    {
      errorpos = errorpos + (lastReportedPos - encoderPos);
    }   
    lastReportedPos = encoderPos;
  }

errorposctrl = errorpos;
Serial.print("   errorposctrl:  ");
Serial.print(errorposctrl);

float a = (errorposctrl/posScaleA);
float b = (errorposctrl/posScaleB);
float c = (errorposctrl/posScaleC);

if (abs(errorposctrl) > ZoneA)
{
   setpointang = setpointang - (a);
}
else if (abs(errorposctrl) > ZoneB)
{
   setpointang = setpointang - (b);
}
else
{
   setpointang = setpointang - (c);
}

Serial.print("     a:  ");
Serial.print(a);
Serial.print("    b:   ");
Serial.print(b);
Serial.print("     c:   ");
Serial.println(c);
//

current_1 = (analogRead(A0)) * 34;
current_2 = (analogRead(A1)) * 34;

//Serial.print("    errorpos:   ");
//Serial.print(errorpos);
//Serial.print("     encoderPos:   ");
//Serial.println(encoderPos);

//delay((int)T);

}
}

//// Interrupt on A changing state
//void doEncoderA(){
//      encoderPos += 1;
//}

// Interrupt on B changing state, same as A above
void doEncoderB()
{
      encoderPos += 1;
}




Thanks in advance,
MAAhelp

It seems that the problem I am currently encountering could be easily solved by just adding the integral term with regards to the PID control I am currently trying. Does this make sense?

I have this motors:
http://www.robotshop.com/world/solutions-cubed-ezr-5.html

And this Motor board controller:
http://www.sparkfun.com/products/9670

I would like to know if you think this hardware is possible to build a robot of this type?????

#aasanchez
Yes I think is possible, but the motors are not that powerfull (only 3.6 kg-cm torque), so start out by making a small version first.
For instance I used these motors from Pololu: Pololu - 30:1 Metal Gearmotor 37Dx52L mm 12V with 64 CPR Encoder (No End Cap) which has a torque of 8 kg-cm.
Also I think it is weird, that the peak current is not listed on the website or in the pdf (http://www.robotshop.com/world/PDF/rbsol03_manual.pdf), but if think the motor controller will be okay - if it get's very how, then add a heatsink.

Regards
Lauszus

Hi guys. i've a new questions.1) when the wheel complete a fully rotation of 360 degree what is the value of count 486 or 1856? So what is the conversion between count (kas code) and degree position?
2) is the conversion between quid (kas code) and degree angle?

#batista1987
I depends on if you set up an interrupt on every edge.
We will use these motors as an example: Pololu - 30:1 Metal Gearmotor 37Dx52L mm 12V with 64 CPR Encoder (No End Cap)
If you only have one interrupt, lets say on a raising edge, then you will get 464 counts per resolution if you trigger the interrupt on the same pin with every change you will get 4642=928 counts per resolution.
If you also make an interrupt on a rising edge on the other input, you will get 464
3=1392 and if you do an interrupt on change on this input to you will get 464*4=1856 counts per resolution.

For my balancing robot I only created one interrupt on a rising edge (BalancingRobot/Encoder.h at master · TKJElectronics/BalancingRobot · GitHub) equal to to 464 counts per resolution, as I decided that I didn't need any more resolution than that.

If you got 464 counts per resolution one counts is equal 360/464=0.775862069 degrees

See wiki for more details: Rotary encoder - Wikipedia

The conversion between quids and angle depends on the sensitivity of your IMU. See my other post for more details: http://arduino.cc/forum/index.php/topic,58048.0.html

Regards
Lauszus

@Lauszus, thanks :slight_smile: you are very kind. So considering your thought, this code gives me 464 (i use dc motor 29:1 of pololu) pulses per revolution, or am I making a mistake?

void setup(){
....
 attachInterrupt(1, rencoder, FALLING);
....
}

void rencoder()  { 
 if (PIND & 0b01000000)    count--;                     
  else                      count++;        
}

So i know that we talks about interrupt, but why we write this( if (PIND & 0b01000000) )?

@Lauszus: I saw your robot (on youtube), I would like to congratulate you on the awesome robot. I have also read the thread: Guide to gyro and accelerometer with Arduino including Kalman filtering, although not identical I though it could help me tackle any problems I might encounter. I'm using an Arduino Uno board and a tilt sensor. Although the robot balances well, it drifts (does not stay in a set position). May you elaborate how you managed to make the robot stay in the same position? As I have tried various methods but none of them seem to be working as I intended them to...
FYI: I am balancing the robot with a basic PD controller.

Thanks in advance for the help :slight_smile:

I got the same problem before I implemented the encoders. Have you any encoders? As I think they make it stay in the same position.

BTW:
I have now ported the code to Arduino.
The code can be found at github: The Arduino version, can now be found at github: GitHub - TKJElectronics/BalancingRobotArduino: This is the Arduino version of the code for my balancing robot/segway

Regards
Lauszus

Lauszus:
I got the same problem before I implemented the encoders. Have you any encoders? As I think they make it stay in the same position.

BTW:
I have now ported the code to Arduino.
The code can be found at github: The Arduino version, can now be found at github: GitHub - TKJElectronics/BalancingRobotArduino: This is the Arduino version of the code for my balancing robot/segway

Regards
Lauszus

Thanks for the quick reply and for the code in an arduino version! My main problem is understanding how you are implementing the encoders. Till now I have managed to read the encoder value via interrupts as you have done, what I am not sure is how I am going to use this information to keep the robot in the same position :S

Thanks in advance,
MAAhelp

#MAAhelp

I read the encoders in every 10th loop: BalancingRobotArduino/BalancingRobotArduino.ino at master · TKJElectronics/BalancingRobotArduino · GitHub this is equal to every 100ms - I then update the encoder position and velocity (velocity is just the difference between the last and current wheel position).

I then adjust the resting angle according to the position error and wheel velocity: BalancingRobotArduino/BalancingRobotArduino.ino at master · TKJElectronics/BalancingRobotArduino · GitHub

I only look at the wheel velocity while it's moving, see BalancingRobotArduino/BalancingRobotArduino.ino at master · TKJElectronics/BalancingRobotArduino · GitHub and BalancingRobotArduino/BalancingRobotArduino.ino at master · TKJElectronics/BalancingRobotArduino · GitHub

Also take a look at my reply to Kas at my blog (TKJ Electronics » The Balancing Robot):

"The restAngle is a constant (90 degrees), I only adjusted it before I implemented the encoders, as they keep adjusting the angle, so it remains in the same position.
The forward and backward commands including the targetOffset, are sent from the Arduino via serial. See this line: BalancingRobot/BalancingRobot.cpp at master · TKJElectronics/BalancingRobot · GitHub.
But if you just set the offset to for instance 5 degrees, it will eventually fall over, so to avoid this, I adjust the offset depending on the wheel velocity. This can also so act as an break, as if the wheels are going forward and you set the backward command, the target offset is actually getting bigger than the values being sent.
The stop function is a bit different. At first there is three zones – this will slow it down again, when reaching the targetPosition. It adjust the restAngle, so it moves to the right position. At last I have also implemented the wheel velocity. It’s main purpose is to keep it in the same spot. In short the positionError force it to go to a certain position and the wheelVelocity keeps it there.
At last I limit the restAngle angle, so it doesn’t overreact when the error is great. Think of it as the constrain function for the Arduino.
The next step is just a normal PID controller. The pTerm looks at the error, if it’s for instance 5 degrees it will multiply it by a constant, to make the error less. The problem is that this will make the robot start to oscillate – this is what dTerm prevents: it looks at the difference between the last error and the error. So if the last error were 5 degrees, but the new error is 3 degrees, the diference is -2, so it will actually slow the robot down, when it’s reaching the correct angle. At last the iTerm will force it not to be satisfied until the error is zero – this will also helps the robot from drifting, as if the error is for instance just 0.5 degrees, it would start to drift, if the iTerm values wasn’t there.
At last I split up the PIDValue, and set and offset to it, if the Arduino sends an turn or rotate command. It will keeps balancing if you just set the exact same offset to the difference motors, as I explained in the video."

Regards
Lauszus

#batista1987
Yes that is correct.

The reason why you write: PIND & 0b01000000 is beacuse you want to read port D at bit 6 if you look at the port mapping (http://arduino.cc/en/Hacking/PinMapping168) you can see that PD6 is equal to pin 6 - but this is not always the case, if you look at the pin mapping for the ATmega2560 (http://arduino.cc/en/Hacking/PinMapping2560) then you can see that PD6 not connected to anything (NC).

I prefer it to do it like this, as it's much more readable: BalancingRobotArduino/BalancingRobotArduino.ino at master · TKJElectronics/BalancingRobotArduino · GitHub

Where _BV() is a standard AVR definition:
#define _BV(bit) (1 << (bit))

See: avr-libc: <avr/sfr_defs.h>: Special function registers

For more information about the reading and writing to the registers, have a look at the port manipulation page:

Regards
Lauszus

@Lauszus

Thanks for the detailed post! I will let you know how I get along. Really appreciate it hopefully solves the problem I was encountering :slight_smile: