Pages: 1 ... 28 29 [30] 31 32 ... 35   Go Down
Author Topic: Guide to gyro and accelerometer with Arduino including Kalman filtering  (Read 242001 times)
0 Members and 2 Guests are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi!, I'm using IMUs for my final project, and I've been studying Kalman filters implementations for a while.

Studying Lauszus code, I see that Kalman filter gain (K0 and K1) is obtained from the error covariance matrix (P00, P01, P10 and P11) multiplied by the time between readings (dt) and added some constants.

If I see how Kalman gains evolve in time, I see that K0 approachs to 1 and, if time between readings remain constant, K1 approachs to -1/dt. If this is OK, where does Kalman filter improve the Complementary Filter?

Thanks and greetings from Argentina
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 8
Posts: 282
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

@juanv
You are totally right, see this comment on my blog: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/comment-page-1/#comment-54328 down to this one: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/comment-page-1/#comment-55104.
Logged

Girona (Catalonia)
Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi, I'm a newbie, but i would like try with this IMU http://www.pololu.com/catalog/product/1269.
What's is your opinion?. Can I implement your algorithm with this IMU?

Albert
« Last Edit: May 27, 2013, 11:08:01 am by AlbertG » Logged

Albert

Denmark
Offline Offline
Sr. Member
****
Karma: 8
Posts: 282
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

@AlbertG
You can use any IMU you like smiley
Logged

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

Hello, i`m doing my little Self Balancing robot project, got 4 days left to show it to the pubblic, i`ve calculated the PID for the motors (using DC motors) and this forum was the GOD for me, better said Lazarus is God! So i wondered if this forum is thread is still opened and you still answer newbie quesitons. Anways i`ll just ask away, i`ve obtained the data from my sensor, i`ll try to attach it, all the action is on the X axis, do i need the kalman Y calculus to be done? or how does it work? From what i`ve read, i only have to pass to my motors the kalman X value through PWM, that`s analogWrite(kalmanX, pin number); ... wait... this is where i have the problem... where does my PID loop go? i should pass the kalman calculus value to the PID and then pass the PID calculus to the motors... no? I`m confused, i`ve read so many to get this thing working, now my sensors are accurate have accurate readings thanks to you, i`ve calculated the pid loop, now i have to put it all together and i`mm a little confused.
I`m`a try attaching that .txt sensor reading, i hope you`re still here and still have some time to post a reply  smiley
Thanks in advance for the answer!
Best regards,
Cata


* Data Lazarus.txt (298.46 KB - downloaded 17 times.)

* Data Lazarus.png (60.37 KB, 1318x624 - viewed 48 times.)
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 8
Posts: 282
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

@LillSlick
You should check out my robot at Kickstarter: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit.
The source code is available at Github: https://github.com/TKJElectronics/Balanduino/tree/master/Firmware/Balanduino. It will pretty much answer all your questions smiley-wink
Logged

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

WOW thanks for the reply on such a short notice! Your code is very elaborate, you`re very thorough. I don`t need complicated stuff, only want to balance the damn thing smiley no time for other stuff, i have to present my work in 4 days or else i`m screwed, it is my first robotics project, i`m an automation engineer and this project was proposed by my boss and was stupid enough to tell a deadline without even having the hardware, they gave me all the hardware 3-4 days ago and they said 1week ... so now after i`ve done the phisical part i would like to get the code done, i`ll use your data acquisition and kalman filtering and i have to get the motors to balance the robot. I had a look on your code and saw you have much control over the motors, i`m not using the encoders, i don`t care if one of the wheels will have a different caracteristic, plus i`m using two PmodHB5 h-bridges, the pins: Dir/En/Vcc/GND i`ve merged them and i thoat i`ll send the same command for them all, if dir is 1, only 1 pin is high and bolth h-bridges should behave in the same manner, and the PWM is the same for both i`ll send 1 pwm to 1 pin which runs both h-bridges at the same time... that`s what i thoat, and wanna get this sorted as soon as possible so i don`t get buttfucked by my bosses smiley). It`s kinda hard putting up with 10 hours of work and after that, concentrate on the "other project" but they don`t care. Can u have some pointers, how should i implement a simplified version of ur code so that the robot balances itsself based on the kalman calculus u`ve done in the mpu6050 file and that`s about it... is it complicated? jason`s done some cool stuff with the motor control, he has an if decision there which does two types of commands for the motors, one aggressive command and one conservative command, that`s kinda nice but i won`t have the time to experiment with it. So, i only need the basics working, after that, i`ll build on it as you built on your own project which frankly is a super-project! I`m very interested in this project, i love it, i want to take it to a whole new level, but for my own, not for some fool who thinks he`s gonna unemploy me. Sorry for misspelling i`ve done so far, hope you can give me some pointers smiley. Thanks!
Best regards,
Cata  
PS: i`ll post the code i was talking about smiley

* BalancingRobot.rar (4.11 KB - downloaded 13 times.)
« Last Edit: July 05, 2013, 01:31:32 pm by LillSlick » Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 8
Posts: 282
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

@LillSlick
You should take a look at the old version of the code here: https://github.com/TKJElectronics/BalancingRobotArduino, but basically you can just comment out these lines: https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Balanduino/Balanduino.ino#L13-L17 and then remove all the code related to the encoders. I would recommend that.

But you really need to work hard all weekend to make it in 4 days! It should be possible to make it balance if your hardware is alright.
This was my first attempt to make a balancing robot: http://blog.tkjelectronics.dk/2011/12/sneak-peak-segway-guide-code/. It didn't use the encoders - you might want to take a look at the source code below the video.

I just looked at Jason's robot: http://www.transistor.io/balancing-robot.html, but he using a servo. I'm using a brushed DC motor.

How long are you? Have you build the entire hardware and checked that all hardware is working as intended? It's important that you get the basics like the IMU, motor and all the other stuff working before you start writing the software to make it balance.
« Last Edit: July 05, 2013, 03:56:22 pm by Lauszus » Logged

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

I got all the hardware done, the H-Bridges and motors are working correctly, the mechanical part is balanced(almost standing on its own) wiring is done, the only thing i need to do is code. i`ll attach some pictures i took while building it, they`re not the final product as it looks now, but it should give you an idea smiley. Thanks for the pointers, i`ll get right on it!

The pictures are a little to big for the forum so i`ll share them like this:
https://dl.dropboxusercontent.com/u/40769383/20130623_190515.png
https://dl.dropboxusercontent.com/u/40769383/20130627_005027.png
https://dl.dropboxusercontent.com/u/40769383/20130702_120216.jpg
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 8
Posts: 282
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Looks alright. Maybe a little to wide and what's up with that big cable box on the middle level? Is there anything inside it? smiley

Any progress with the code?
I also noticed that you actually got encoders on those motors. If you have time I would recommend implementing them as well smiley
Logged

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

yes that`s my power box, i got a battery in there 9V one, i don`t know if it`s going to be enough... i hope so!
with the code, i`ve tried some stuff, but i think i messed it up when I choose to command both Enable pins and Direction pins through the same pin, for the Enable the pin number 3 and for the Direction - pin number 1 on the arduino UNO. I`m using PmodHB5 drivers, up to 2A. I was using this code, your example for collecting and filtering data and my stupid coding smiley) :
Code:
Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#define APWM 3          // left motor PWM
//#define RPWM 10         // right motor PWM
#define ADIR 1         // left motor direction
//#define RDIR 12         // right motor direction

#define KP 0.5          // proportional controller gain [LSB/deg/loop]
#define KD 0.5          // derivative controller gain [LSB/deg/loop]

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
float output = 0.0;

void setup() { 
    // Make sure all motor controller pins start low.
  digitalWrite(APWM, LOW);
 // digitalWrite(RPWM, LOW);
  digitalWrite(ADIR, LOW);
 // digitalWrite(RDIR, LOW);
 
  // Set all motor control pins to outputs.
  pinMode(APWM, OUTPUT);
 // pinMode(RPWM, OUTPUT);
  pinMode(ADIR, OUTPUT);
 // pinMode(RDIR, OUTPUT);
  pinMode(13, OUTPUT);
 
  Serial.begin(115200);
  Wire.begin();
  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
 
  while(i2cRead(0x75,i2cData,1));
  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while(1);
  }
 
  delay(100); // Wait for sensor to stabilize
 
  /* Set kalman and gyro starting angle */
  while(i2cRead(0x3B,i2cData,6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2π and then from radians to degrees
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
 
  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;
 
  timer = micros();
}

void loop() {
  /* Update all the values */ 
  while(i2cRead(0x3B,i2cData,14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]); 
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
 
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2π and then from radians to degrees
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
 
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter 
 // gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
 
  //compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  //compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
 
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  //kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
   
   //temp = ((double)tempRaw + 12412.0) / 340.0;
   
    // PD controller.
  output += kalAngleX * KP + gyroXrate * KD;
   
   // Clip as float (to prevent wind-up).
  if(output < -255.0) { output = -255.0; }
  if(output > 255.0) { output = 255.0; }
  if(kalAngleX > 3) {
  digitalWrite(ADIR, HIGH);
  analogWrite(APWM,output);
  }
  if(kalAngleX > 300) {
    digitalWrite(ADIR, LOW);
    analogWrite(APWM,output);
  }
 
 
 
  /* Print Data */
  /*
  Serial.print(accX);Serial.print("\t");
  Serial.print(accY);Serial.print("\t");
  Serial.print(accZ);Serial.print("\t");
 
  Serial.print(gyroX);Serial.print("\t");
  Serial.print(gyroY); Serial.print("\t");
  Serial.print(gyroZ);Serial.print("\t");
  */
  Serial.print(accXangle);Serial.print("\t");
  Serial.print(gyroXangle);Serial.print("\t");
  //Serial.print(compAngleX);Serial.print("\t");
  Serial.print(kalAngleX);Serial.print("\t");
  Serial.print(output);Serial.print("\t"); 
  Serial.print("\t");
 
 // Serial.print(accYangle);Serial.print("\t");
 // Serial.print(gyroYangle);Serial.print("\t");
 // Serial.print(compAngleY); Serial.print("\t");
 // Serial.print(kalAngleY);Serial.print("\t");
 
  //Serial.print(temp);Serial.print("\t");
   
  Serial.print("\r\n");
  delay(1);
}

Based on your mpu6050 example for data filtering and i added some lines to make something move smiley
The wheels go in separate directions... it doesn`t balance its self.
I`m going through your code now, i`m commenting the parts with the encoders and EEPROM readings and all the stuff that stands for the BT, PS3, WII so on...
In the baladuino library i`ve modified the ports, i`m using pin 3 for the pwm and pin 1 for the enable signal that means portD,
Code:
/* Left motor */
#define leftPort PORTD // a fost #define leftPort PORTC
#define leftPortDirection DDRD

#define leftA PINC6 // PC6 - M1A - pin 23
#define leftB PINC7 // PC7 - M1B - pin 24
#define leftPWM PIND2  // a fost #define leftPWM PIND5 // PD5 - PWM1A (OC1A) - pin 18

/* Right motor */
#define rightPort PORTD
#define rightPortDirection DDRD

#define rightA PINB0 // PB0 - M2A - pin 25
#define rightB PINB1 // PB1 - M2B - pin 26
#define rightPWM PIND4 // same as my pin 5 --> PD4 - PWM1B (OC1B) - pin 17

/* Pins connected to the motor drivers enable pins */
const uint8_t leftEnable = 1; // was  const uint8_t leftEnable = 21;
//const uint8_t leftEnable = 4; // was const uint8_t rightEnable = 22;


One question until now : targetAngle - you said " the resting angle of the robot"  <=== where do i find that? gyro reading? and when the robot is balancing or?

So now i`m going through your code and making something much more simpler, only to balance the robot.
I agree with you on the Encoders, I will implement them, but not now... first i want to show them a simple thing, then i can work on it to get the complicated stuff smiley make a REAL balancing robot.

I still need to figure out how this works
Code:
  /* Set PWM frequency to 20kHz - see the datasheet http://www.atmel.com/Images/doc8272.pdf page 128-135 */
  // Set up PWM, Phase and Frequency Correct on pin 18 (OC1A) & pin 17 (OC1B) with ICR1 as TOP using Timer1
  TCCR1B = _BV(WGM13) | _BV(CS10); // Set PWM Phase and Frequency Correct with ICR1 as TOP and no prescaling
  ICR1H = (PWMVALUE >> 8); // ICR1 is the TOP value - this is set so the frequency is equal to 20kHz
  ICR1L = (PWMVALUE & 0xFF);

  /* Enable PWM on pin 18 (OC1A) & pin 17 (OC1B) */
  // Clear OC1A/OC1B on compare match when up-counting
  // Set OC1A/OC1B on compare match when downcounting
  TCCR1A = _BV(COM1A1) | _BV(COM1B1);
  setPWM(leftPWM,0); // Turn off pwm on both pins
  setPWM(rightPWM,0);

and how to do it for arduino uno and for my pins smiley
« Last Edit: July 07, 2013, 12:08:26 pm by LillSlick » Logged

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

getting this strange error.. dunno wtf to do about it smiley-sad
Code:
In file included from Schita001.ino:2:
Balanduino.h:59: error: redefinition of 'const uint8_t leftEnable'
Balanduino.h:58: error: 'const uint8_t leftEnable' previously defined here
Schita001:11: error: 'Kalman' does not name a type
Schita001.ino: In function 'void setup()':
Schita001:18: error: 'rightEnable' was not declared in this scope
Schita001:75: error: 'kalman' was not declared in this scope
Schita001.ino: In function 'void loop()':
Schita001:118: error: 'kalman' was not declared in this scope
Schita001:125: error: 'kalman' was not declared in this scope
Motor.ino: In function 'void moveMotor(Command, Command, double)':
Motor:9: error: 'PINC7' was not declared in this scope
Motor:13: error: 'PINC7' was not declared in this scope
Motor.ino: In function 'void stopMotor(Command)':
Motor:32: error: 'PINC7' was not declared in this scope

with the code based on the data acquisition and something to get the motors moving i get some results but the Output of the PD controller is not very good... i`ll attach a file so you can take a look.

* Sensors.txt (74.2 KB - downloaded 15 times.)
« Last Edit: July 08, 2013, 03:54:47 am by LillSlick » Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 8
Posts: 282
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

You are using an Uno (ATmega328P), right?

Then pin 3 is located on PIND3 and not PIND2.
See this page for more information: http://arduino.cc/en/Hacking/PinMapping168.

Target angle is the balancing point of the robot - this is usually 180, but you might have to change it if you CG is offset to either side. I used that when I mounted a GoPro on mine: http://www.kickstarter.com/projects/tkjelectronics/balanduino-balancing-robot-kit/posts/450808.

Regarding the PWM registers. You should read the datasheet: http://www.atmel.com/Images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet.pdf at page 128 to 138.

This error is caused because you are defining the same varaible twice:
Code:
Balanduino.h:59: error: redefinition of 'const uint8_t leftEnable'
Balanduino.h:58: error: 'const uint8_t leftEnable' previously defined here

Try renaming one of them two "rightEnable":
Code:
Schita001:18: error: 'rightEnable' was not declared in this scope

You need to download the Kalman library I wrote: https://github.com/TKJElectronics/KalmanFilter.
Code:
Schita001:11: error: 'Kalman' does not name a type

PINC7 is not available is on the ATmega328P:
Code:
Motor:9: error: 'PINC7' was not declared in this scope
Motor:13: error: 'PINC7' was not declared in this scope
Motor:32: error: 'PINC7' was not declared in this scope
Simply uncomment it for now.

Regarding the "Sensors.txt". Please tell me what is the output from each column or I will not be able to help you smiley
Logged

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

Yes 328 it is, my CG is as much as possible on the middle part, it's pretty well balanced, i wrote the columns but as i recon they didnt save...  so its like this: accX/gyroX/KalmanXangle/PD output

i'll try the mods u've suggested, the pwm values are the same for mine to, right?
and one more thing the PmodHB5 doesn't give information on the max frequency, how do i find it? i saw ur's is 20kHz... i dunno my max...
Logged

Denmark
Offline Offline
Sr. Member
****
Karma: 8
Posts: 282
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Hmm that doesn't look right. Are you using the example code I wrote?

The PWM value is the same, it will automatically scale the value from 0-100 to 0-PWMVALUE: https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Balanduino/Motor.ino#L4.

The PWMVALUE is set automatically here: https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Balanduino/Balanduino.h#L16-L17.

You will need to look in the datasheet. After that change this value: https://github.com/TKJElectronics/Balanduino/blob/master/Firmware/Balanduino/Balanduino.h#L16 to the desired frequency.
Logged

Pages: 1 ... 28 29 [30] 31 32 ... 35   Go Up
Jump to: