DC motor control with PID

Forum 2005-2010 (read only) General Exhibition
15

I know you are going to get swarmed for your code for the Kalman, but I am really struggling getting this to work. Can you detail how you went about this? Was the programming intensive? ...

Hi Gibby623, sorry, I just got your PM's
If your robot rolls about the x axis, you only need GYR_X, ACC_Y and ACC_Z
I notice that signal quality is much better when the sensor is upward, apparently, the IMU does not appreciate to be up side down
I will publish the full code pretty soon

here is the Kalman module:

// Kalman filter module

 float Q_angle  =  0.001;
 float Q_gyro   =  0.003;
 float R_angle  =  0.03; 

 float x_angle = 0;
 float x_bias = 0;
 float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;      
 float dt, y, S;
 float K_0, K_1;


  float kalmanCalculate(float newAngle, float newRate,int looptime) {
    dt = float(looptime)/1000;                                  
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;
    
    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;
    
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
    
    return x_angle;
  }

The function parameters:

  • newAngle = ACC_angle (Accelerometers angle)
  • newRate = GYRO_rate (Gyro angle rate)
  • looptime = lastLoopTime (time between sensors measurement)

More info this week end, lets keep it public