Go Down

Topic: Balancing robot for dummies (Read 41038 times) previous topic - next topic

Lauszus

#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!



aasanchez

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 http://www.sparkfun.com/products/10252 (This is i2c sensor)
Battery: http://www.sparkfun.com/products/10470
Motor Drive: http://www.sparkfun.com/products/9670

Now i test  this.. http://www.x-firm.com/?page_id=187
At the final they say:
Quote
You should read "2,10?


I read:

Quote

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..
Code: [Select]

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

}

Lauszus

#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: https://github.com/TKJElectronics/BalancingRobotArduino/blob/master/BalancingRobotArduino.ino#L100

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

Regards
Lauszus

aasanchez

#78
Apr 23, 2012, 10:57 pm Last Edit: Apr 24, 2012, 03:31 am by aasanchez Reason: 1
sensors aquisition smoothing and zeroing

First i have a i2c sensor http://www.sparkfun.com/products/10252 ... This sensor is different in the values ??that can deliver
If i connect and plot they value, I get something like this.

http://twitpic.com/9d84yg (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

Code: [Select]

  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....


Code: [Select]

#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?


batista1987

#Lauszus, thanks for your reply :-) 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

Lauszus

@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

batista1987

Hi lauszus,
i've changed this code
Code: [Select]

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

to
Code: [Select]

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


and this
Code: [Select]

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

to
Code: [Select]

#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?
Code: [Select]

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

and this
Code: [Select]

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


Lauszus

@batista1987
Here is how it should look:
Code: [Select]

/* 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

batista1987

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

batista1987

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
Code: [Select]

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


Lauszus

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: https://github.com/TKJElectronics/NXTShield/blob/master/NXTShield.cpp#L116 on how to do so.

Regards
Lauszus


batista1987

@Lauszus

Hi Lauszus
i've used your code, so

Code: [Select]

void setup(){
..
attachInterrupt(1,readEncoder,CHANGE);
..
}


Code: [Select]

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



Code: [Select]

void readEncoder(){
      if((bool)(PIND & _BV(3)) == (bool)(PIND & _BV(6)))  //my pin encoder are 3 and 6
    else
        countRight--;
}


but the outout is this
http://dl.dropbox.com/u/15269862/out_1.txt

Lauszus

The following code is wrong:
Code: [Select]

void readEncoder(){
      if((bool)(PIND & _BV(3)) == (bool)(PIND & _BV(6)))  //my pin encoder are 3 and 6
    else
        countRight--;
}


It should be:
Code: [Select]

void readEncoder(){
    if((bool)(PIND & _BV(3)) == (bool)(PIND & _BV(6)))  //my pin encoder are 3 and 6
       countRight++;
    else
       countRight--;
}

batista1987

excuse me but I was wrong to copy my code.

my code was this

Code: [Select]

void readEncoder(){
      if((bool)(PIND & _BV(3)) == (bool)(PIND & _BV(6))) // pin 2 == pin 5 - See http://arduino.cc/en/Hacking/PinMapping168
        countRight++;
    else
        countRight--;
}

Lauszus

@batista1987
Okay, it seems alright then.
But I really see no point of converting it to deg/s, unless you want a speedometer or something like that?
Because it really doesn't matter if just need it to make it balance.
But of course it's always nice to know what your robot is capable of :)

Go Up