Guide to gyro and accelerometer with Arduino including Kalman filtering

Using Arduino Sensors
451

hey, i made a stupid version starting from the data acquisition code you provided :slight_smile: they wanted to see my progress so ... now i have until Thursday to finish, and one of them was so impressed hes gonna send me tomorrow to do some work 210 KM away from home, just to be sure i dont finish on time. This said, ill show u my code, ill have to implement that motor control of yours, i hope i`ll succeed.
Lemme show you how it works until now :

And my stupid code is:

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

#define LPWM 3          // left motor PWM
#define RPWM 5         // right motor PWM
#define LDIR 4         // left motor direction
#define RDIR 7         // right motor direction 

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

#define W_radius 0.0035  // meters

#define minOutput 0
#define maxOutput 250


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
double velocity;
 
 double Setpoint = 0, Input, Output;
 double aggKp=8, aggKi=0.5, aggKd=2;
 double consKp=4, consKi=0.02, consKd=1;  
 PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);

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(LPWM, LOW);
  digitalWrite(RPWM, LOW);
  digitalWrite(LDIR, LOW);
  digitalWrite(RDIR, LOW);
  
  // Set all motor control pins to outputs.
  pinMode(LPWM, OUTPUT);
  pinMode(RPWM, OUTPUT);
  pinMode(LDIR, OUTPUT);
  pinMode(RDIR, OUTPUT);
  pinMode(13, OUTPUT);
  
  // PID library 
    myPID.SetOutputLimits(minOutput,maxOutput);
    myPID.SetSampleTime(1);
    myPID.SetMode(AUTOMATIC);
  
 // 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])/1000;
  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;
   
   velocity = accY/W_radius;
   Input = velocity; 

  
  double sens = Setpoint-Input;
        if(sens>1) 
        {
          myPID.SetControllerDirection(DIRECT);
        }
        else if(sens<-1)
        {
          myPID.SetControllerDirection(REVERSE);
        }
       
        double gap = abs(Setpoint-Input);
        

          if(gap<10)
          {  
            myPID.SetTunings(consKp, consKi, consKd);
          }
          else
          {
             myPID.SetTunings(aggKp, aggKi, aggKd);
          }
        
          myPID.Compute();
        //  Output = map(Output, 0, 1000, 0, 255); 
          if(Output<minOutput+20){ Output=0; }
    
     if(sens>=0)
           {
             digitalWrite(LDIR, LOW); 
             digitalWrite(RDIR, HIGH);
             analogWrite(LPWM,Output);
             analogWrite(RPWM,Output);
              
           }
           else
           {
             digitalWrite(LDIR, HIGH);
             digitalWrite(RDIR, LOW);             
             analogWrite(LPWM,Output);
             analogWrite(RPWM,Output);           
             
           }
 delay(1);
}

Its not good, but ... it does SOMETHING, as you can observe im not using the kalman angle, i loaded that library but the code does not work it always gives me error at that single line :

#include <Kalman.h> // Kalman filter library - see:...

So, im making progress, but i really need to set that speed, and controller you recommended. Ill get right on it, i have 2 hours until i have to go to sleep because i`m awfully tired. Keep in touch, what do you say about my code? stupid huh? :))