Pages: 1 ... 4 5 [6] 7 8   Go Down
Author Topic: Balancing robot for dummies  (Read 38693 times)
0 Members and 1 Guest are viewing this topic.
Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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


Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 16
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

}
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Newbie
*
Karma: 0
Posts: 16
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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



* IMU3000-simple.png (40.51 KB, 940x516 - viewed 26 times.)

* quids.png (32.44 KB, 954x530 - viewed 25 times.)

* foward.png (30.32 KB, 954x530 - viewed 27 times.)

* backward.png (32.81 KB, 954x530 - viewed 19 times.)
« Last Edit: April 23, 2012, 08:31:56 pm by aasanchez » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

@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
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi lauszus,
i've changed this code
Code:
#define rightA PINC4 // PC4 - pin A4
#define rightB PINC5 // PC5 - pin A5
#define rightPWM PINB2 // PB2 - pin 10 (OC1B)
to
Code:
#define rightA PIND4 // PD4 - pin 4
#define rightB PIND5 // PD5 - pin 5
#define rightPWM PINB2 // PB2 - pin 10 (OC1B)

and this
Code:
#define leftA PIND0 // PD0 - pin 0
#define leftB PIND1 // PD1 - pin 1
#define leftPWM PINB1 // PB1 - pin 9 (OC1A)
to
Code:
#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:
/* Left motor */
#define leftPort PORTD
#define leftPortDirection DDRD
#define leftPwmPortDirection DDRB
and this
Code:
/* Right motor */
#define rightPort PORTC
#define rightPortDirection DDRC
#define rightPwmPortDirection DDRB
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

@batista1987
Here is how it should look:
Code:
/* 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
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

@Lauszus

Hi Lauszus
i've used your code, so

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

Code:
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:
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
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

The following code is wrong:
Code:
void readEncoder(){
      if((bool)(PIND & _BV(3)) == (bool)(PIND & _BV(6)))  //my pin encoder are 3 and 6
    else
        countRight--;
}

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

Offline Offline
Newbie
*
Karma: 0
Posts: 44
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

excuse me but I was wrong to copy my code.

my code was this

Code:
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--;
}
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 10
Posts: 287
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

@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 smiley
Logged

Pages: 1 ... 4 5 [6] 7 8   Go Up
Jump to: