Balancing robot for dummies

#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();

}

#aasanchez
It's because it take less than 0.5ms, so it will round down to 0ms. Try to use micros() (http://arduino.cc/en/Reference/Micros) instead as I did: BalancingRobotArduino/BalancingRobotArduino.ino at master · TKJElectronics/BalancingRobotArduino · GitHub

It will give you better resolution and thereby more accurate timing.

Regards
Lauszus

sensors aquisition smoothing and zeroing

First i have a i2c sensor SparkFun IMU Fusion Board - ADXL345 & IMU3000 - SEN-10252 - SparkFun Electronics ... This sensor is different in the values ??that can deliver
If i connect and plot they value, I get something like this.

Lectura IMU3000 #tesiandoando (This is All values)

The gyros value... range(-32768,32768); (16 bits resolutions)
The acc range(-512,512); (10bits resolution)

because these values ??are not in quids using the map function are taken to the range from 0 to 1023

   temp = buffer[2] << 8 | buffer[3];
   readIMU[0] = map(temp,-32768,32768,0,1023);  //  GYR_Y in Quids
   temp = buffer[7] << 8 | buffer[6];
   readIMU[1] = map(temp,-512,512,0,1023);       //  ACC_X in Quids
   temp = buffer[11] << 8 | buffer[10];
   readIMU[2] = map(temp,-512,512,0,1023);       //  ACC_Z in Quids*/

Values (Aprox)
506 - GYR_Y
500 - ACC_X
751 - ACC_Z
See Quids.png

After Zeroing
I cant stablish for my sensor the Acc_Z zero value... like 102...

My sensor is a 4 mg/LSB scale factor.
the device is in 10-bit mode.
And the g range +-2g
Datasheet (http://www.sparkfun.com/datasheets/Sensors/Accelerometer/ADXL345.pdf, page 17)

See backward.png and Backward.png....

#include <math.h>
#include <Wire.h>
#define ON   pinMode(11, OUTPUT);digitalWrite(11, HIGH);

#define GYRO 0x68         // gyro I2C address
#define REG_GYRO_X 0x1D   // IMU-3000 Register address for GYRO_XOUT_H
#define ACCEL 0x53        // Accel I2c Address
#define ADXL345_POWER_CTL 0x2D

int            sensorValue[3]  = {0, 0, 0};
int            sensorZero[3]   = {0, 0, 0};
// Sensors Module  ---------------------------------------------------------------------
byte           buffer[12];     // Array para almacenar los valdores del I2C
int            GYR_Y;
int            ACC_Z;
int            ACC_X;
int            readIMU[3] =  {0,0,0};
unsigned long  samplesIMU[3] = {0,0,0};
int            i;

#define STD_LOOP_TIME 10000
unsigned long  lastLoopTime = STD_LOOP_TIME;
unsigned long  lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long  loopStartTime = 0;
int gyro_x;
int gyro_y;
int gyro_z;
int accel_x;
int accel_y;
int accel_z;

// Sensors Module  ---------------------------------------------------------------------
void writeTo(int device, byte address, byte val) {
  Wire.beginTransmission(device); // Comienza la transmision
  Wire.write(address); // Envia la direccion
  Wire.write(val); // Envia el valor
  Wire.endTransmission(); // Finaliza la transmision
}

void configIMU(){
  Wire.begin();
  // Set Gyro settings
  // Sample Rate 1kHz, Filter Bandwidth 42Hz, Gyro Range 500 d/s 
  writeTo(GYRO, 0x16, 0x0B);       
  //set accel register data address
  writeTo(GYRO, 0x18, 0x32);     
  // set accel i2c slave address
  writeTo(GYRO, 0x14, ACCEL);     

  // Set passthrough mode to Accel so we can turn it on
  writeTo(GYRO, 0x3D, 0x08);     
  // set accel power control to 'measure'
  writeTo(ACCEL, ADXL345_POWER_CTL, 8);     
  //cancel pass through to accel, gyro will now read accel for us   
  writeTo(GYRO, 0x3D, 0x28);    
}

void readSensor(){
  long temp;
  // First set the register start address for X on Gyro  
  Wire.beginTransmission(GYRO);
  Wire.write(REG_GYRO_X); //Register Address GYRO_XOUT_H
  Wire.endTransmission();

  // New read the 12 data bytes
  Wire.beginTransmission(GYRO);
  Wire.requestFrom(GYRO,12); // Read 12 bytes
  i = 0;
  while(Wire.available())
  {
    buffer[i] = Wire.read();
    i++;
  }
  Wire.endTransmission();

  temp = buffer[2] << 8 | buffer[3];
  readIMU[0] = map(temp,-32768,32768,0,1023);  //  GYR_Y in Quids
  temp = buffer[7] << 8 | buffer[6];
  readIMU[1] = map(temp,-512,512,0,1023);       //  ACC_X in Quids
  temp = buffer[11] << 8 | buffer[10];
  readIMU[2] = map(temp,-512,512,0,1023);       //  ACC_Z in Quids*/
}

void calibrateSensors() {                                       // Set zero sensor values
  int samples = 50;
  for(int n=0; n<3; n++) {
    samplesIMU[n] = 0;
  }
  for(int i=0; i<samples; i++){
    readSensor();
    samplesIMU[0] += readIMU[0];
    samplesIMU[1] += readIMU[1];
    samplesIMU[2] += readIMU[2];    
  }  

  for(int n=0; n<3; n++) {
    sensorZero[n] = samplesIMU[n]/samples;
  }
}

void updateSensors() {                                         // data acquisition
  int samples = 5;
  for(int n=0; n<3; n++) {
    samplesIMU[n] = 0;
  }
  for(int i=0; i<samples; i++){
    readSensor();
    samplesIMU[0] += readIMU[0];
    samplesIMU[1] += readIMU[1];
    samplesIMU[2] += readIMU[2];    
  }  

  for(int n=0; n<3; n++) {
    sensorValue[n] = samplesIMU[n]/samples - sensorZero[n];
  }
}

/*
Zeros Values
 */
void serialOut_zero() {    
  Serial.print("GIRO_Y");
  Serial.print(sensorZero[0]);
  Serial.print("\t ACC_X:");         
  Serial.print(sensorZero[1]);
  Serial.print("\t ACC_Z:");  
  Serial.println(sensorZero[2]);
}

/*
This Function show values directly form IMU3000
 */
void serialOut_value() {    
  Serial.print(readIMU[0]);
  Serial.print(",");         
  Serial.print(readIMU[1]);
  Serial.print(",");   
  Serial.println(readIMU[2]);
}

/*
The correct value, for use..
 */
void serialOut_read() {    
  Serial.print(sensorValue[0]);
  Serial.print(",");         
  Serial.print(sensorValue[1]);
  Serial.print(",");  
  Serial.println(sensorValue[2]);
}

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);
  configIMU();
  calibrateSensors();
  ON;
  delay(1000);
}

void loop(){
  updateSensors();
  serialOut_read();
  // *********************** loop timing control **************************
  lastLoopUsefulTime = micros()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delayMicroseconds(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = micros() - loopStartTime;
  loopStartTime = micros();
}

@Lauszus Why do u use Acc_Y?

#Lauszus, thanks for your reply :slight_smile: I would to use every 1850 impulse of encoder, so my questions are:
1)How can i use the second encoder with other pin? because the first encoder are connected to a pin2 and pin3, that are the pins that i use with the interupt 0, and 1 respectively
2)using 1850 pulses per revolution, the speed of calculation in each period according to you would be slowed down with so many interruptions? In other words, can in a sampling step to have the time to read the speed, perform the calculations to obtain the PWM value to send the signal to the motor? thanks

@aasanchez
I use the y-axis because it's how my IMU is orientated. If I rotated my IMU 90 degrees, I would have to use the x-axis.

@batista1987

  1. You have to use an AVR with more external interrupts than the ATmega328. For instance the ATmega2560 have 6 external interrupt - see http://arduino.cc/it/Reference/AttachInterrupt

  2. I'm not sure, but I can't see any reason to get that amount of resolution and it might slow your microprocessor down. You might have to experiment and try it out.

Regards
Lauszus

Hi lauszus,
i've changed this code

#define rightA PINC4 // PC4 - pin A4
#define rightB PINC5 // PC5 - pin A5
#define rightPWM PINB2 // PB2 - pin 10 (OC1B)

to

#define rightA PIND4 // PD4 - pin 4
#define rightB PIND5 // PD5 - pin 5
#define rightPWM PINB2 // PB2 - pin 10 (OC1B)

and this

#define leftA PIND0 // PD0 - pin 0
#define leftB PIND1 // PD1 - pin 1
#define leftPWM PINB1 // PB1 - pin 9 (OC1A)

to

#define leftA PINB0 // PB0 - pin 8
#define leftB PINB3 // PB3 - pin 11
#define leftPWM PINB1 // PB1 - pin 9 (OC1A)

So now how can i modify this code?

/* Left motor */
#define leftPort PORTD
#define leftPortDirection DDRD
#define leftPwmPortDirection DDRB

and this

/* Right motor */
#define rightPort PORTC
#define rightPortDirection DDRC
#define rightPwmPortDirection DDRB

@batista1987
Here is how it should look:

/* Left motor */
#define leftPort PORTB
#define leftPortDirection DDRB
#define leftPwmPortDirection DDRB
and this 
Code:
/* Right motor */
#define rightPort PORTD
#define rightPortDirection DDRD
#define rightPwmPortDirection DDRB

Just to make sure, you are not using the USB Host Shield, right?

Regards
Lauszus

@Lauszus, thanks :slight_smile:
Yes I don't use USB host shield. Now i would make only a self balancing without external controller

Hi guys when i try to read the angular (rad/s) speed of my pololu Dc motor (19:1) in the output value there are many noise

float getSpeed()  {
static long countRightAnt = 0;
  lin_speed =(countRight - countRightAnt)*2*pi/(16*2*19)*1000/STD_LOOP_TIME 
 countRightAnt = countRight; 
return lin_speed;  
}

//encoder (  attachInterrupt(1, rencoder, CHANGE);)
void rencoder()  {                                       // pulse and direction, direct port reading to save cycles
  if (PIND & 0b00000001)    countRight--;                     // if(digitalRead(encodPinB1)==HIGH)   count++   (on DI #2)
  else                      countRight++;        
}

As regards to you my function are true?

this is my output with a costant duty cicle
http://dl.dropbox.com/u/15269862/out.txt

You can't do like that if you attach a interrupt on CHANGE, change it to either FALLING or RISING.
If you need the full resolution (interrupt on CHANGE), you have to check the state of the pin that caused the interrupt. See the following line in my NXT Shield library: NXTShield/NXTShield.cpp at master · TKJElectronics/NXTShield · GitHub on how to do so.

Regards
Lauszus