Balancing robot for dummies

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:

@Lauszus, thank you very much. I've a new questions. I've an Arduino 1, and i would to buy a usb shield to add on this shield a BT dongle to use this with a PS3 controller. I noticed that apart from the shield you can mount imu, motors and encoders on arduino1, in case the shield would I need a new arduino? thanks

#batista1987
I'm not totally sure what you mean?
But yes use the USB Host shield (http://www.circuitsathome.com/products-page/arduino-shields/usb-host-shield-2-0-for-arduino) for Arduino Uno. All official Arduino are supported by the version 2 of the shield.

But the shield have to use two pins despite MISO, MOSI and SCK - 11,12, and 13 on a small Arduino (Uno, Duemilanove etc.). These are SS and INT. Normally they are located at pin 9 and 10, but can be rerouted to any I/O, see http://www.circuitsathome.com/usb-host-shield-hardware-manual at "Interface modifications".

Regards
Lauszus

@Lauszus, ok so USBSHIED use pin 9 and 10 of digital out (is true?).
Excuse me but is the first approach to arduino, and i've a problem to understand, so i've a new questions. The pin thah you use (apart IMU) on (Arduino1 for example) are:
PDO=pin0 leftA (is INA left out of driver motor?)
PD1=pin1 (is INB left out of driver motor?)
PB1= A4 (is INA right out of driver motor?)
PC5=A5 (is INB right out of driver motor?)
pin2= leftEcoder1
pin4=leftEncoder2
pin3=rightEncoder1
pin5=rightEncoder2

So apart the imu are these, right? Why you connect the right motor to a analog input?
I use a different imu (SparkFun IMU Fusion Board - ADXL345 & IMU3000 - SEN-10252 - SparkFun Electronics) that use a i2c protocol, and this imu is connected to a4,a5. Can i change

#define rightA PINC4 // PC4 - pin A4
#define rightB PINC5 // PC5 - pin A5

to

#define rightA PINC4 // PC3 - pin A3
#define rightB PINC5 // PC2 - pin A2

Regards
Batista1987

#batista1987
Yes the shield uses pin 9 and 10 normall. But I had to use pin 9 and 10 for 20kHz pwm output, so I rerouted them to pin 7 and 8.
Here is a list of the pins I used:

pin 0 (PD0) = left motor logic A - output
pin 1 (PD1) = left motor logic B - output
pin 2 (PD2) = fist encoder input A - input
pin 3 (PD3) = fist encoder input B - input
pin 4 (PD4) = second encoder input A - input
pin 5 (PD5) = second encoder input B - input
pin 6 (PD6) = buffer (used to indicated when finished calibration) - output
pin 7 (PD7) = INT of USB Host Shield - input
pin 8 (PB0) = SS of USB Host Shield - output
pin 9 (PB1) = left motor pwm (OC1A) - output
pin 10 (PB2) = right motor pwm (OC1B) - output
pin 11 (PB3) = MOSI for USB Host Shield - output
pin 12 (PB4) = MISO for USB Host Shield - input
pin 13 (PB5) = SCK for USB Host Shield - output
pin 14 - A0 (PC0) = gyro y-axis - input
pin 15 - A1 (PC1) = accelerometer x-axis - input
pin 16 - A2 (PC2) = accelerometer y-axis - input
pin 17 - A3 (PC3) = accelerometer z-axis - input
pin 18 - A4 (PC4) = right motor logic A - output
pin 19 - A5 (PC5) = right motor logic B - output

As you can see the reason why I the analog pin for the motor is because I had used all other pins - remember that an analog pin can also be used as a general I/O.

You could actually save two pin, as your IMU uses I2C while mine is analog. If I had to order a new one, I would also go for a digital IMU to save more pins.

There is no difference in the below:

#define rightA PINC4 // PC4 - pin A4
#define rightB PINC5 // PC5 - pin A5
to
Code:
#define rightA PINC4 // PC3 - pin A3
#define rightB PINC5 // PC2 - pin A2

You have only edited the comment NOT the pins definition. The right approach would be:

#define rightA PINC3 // PC3 - pin A3
#define rightB PINC2 // PC2 - pin A2

Regards
Lauszus

I'm trying to test the code of how to calculate the arctang that aparcere here http://dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization and is used in this project, I'm testing with a calculator but I can not give a result not even like ... can someone please show me some input values ??and assume something that should out ..

GYROSCOPE DRIFT...???

Hi guys! I finally got my robot balancing... Far from perfectly but it stands up pretty well... Thanks to all for your help!

Just a query about the dreaded Gyroscope Drift!
Is it still an issue with modern gyroscopes?
I've mapped the output of the rate pin from my IMU on the serial monitor of the arduino program for over a 30 minute duration and I have failed to see any meaningful drift... The signal is noisy alright but there is no indication of a drifting from the set point while motionless!?

The IMU that I am using is a 3dof from Sparkfun and is now retired "IMU Combo Board - 3 Degrees of Freedom - ADXL203/ADXRS613"

the link is: IMU Combo Board - 3 Degrees of Freedom - ADXL203/ADXRS613 - SEN-09127 - SparkFun Electronics

Any thoughts on this...?

#aasanchez
It's already supported by the Arduino IDE, just type atan2(y,x). See http://www.arduino.cc/en/Math/H

#rob4white
Yes drift is still a issue in a modern gyroscope. You will see it, if you rotate the gyroscope. I don't think it will drift when motionless - or at least not very much!

I decided in the next 2 day i gonna make mi balancing bot (have a little vacations)

This is my part list...
Arduino Duemilanove http://arduino.cc/es/Main/arduinoBoardDuemilanove
Motors: http://www.robotshop.com/world/solutions-cubed-ezr-5.html (They do not have encoder, I think this caused me problems in the PID)
Sensor SparkFun IMU Fusion Board - ADXL345 & IMU3000 - SEN-10252 - SparkFun Electronics (This is i2c sensor)
Battery: Polymer Lithium Ion Battery - 1500mAh 11.1v - PRT-10470 - SparkFun Electronics
Motor Drive: http://www.sparkfun.com/products/9670

Now i test this.. Prevas Parekring
At the final they say:

You should read “2,10?

I read:

0,10
0,9
0,9
0,9
0,10
0,9
0,8
0,9
0,10
0,9
0,9
0,10
0,9
0,8
0,9
0,10
0,9

I read 0 always in the first number... i wait put more code to see this change...

My code..

#define ON   pinMode(11, OUTPUT);digitalWrite(11, HIGH);

// Variables de Tiempo
int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;

void serialOut_timing() {
  static int skip=0;
  if(skip++==5) { // display every 500 ms (at 100 Hz)
    skip = 0;
    Serial.print(lastLoopUsefulTime); 
    Serial.print(",");
    Serial.println(lastLoopTime); 
  }
}

void setup(){
  Serial.begin(9600);
  ON; 
}

void loop(){
  serialOut_timing();
  // *********************** loop timing control **************************
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();

}