Archives for category: RobotC

Here is the code for the filter that calculates attitude from gyro and accelerometer. The functional bits of the code are well commented in the code itself. The main structure of the code will be described in this post. A practical description of the filter can be found in this article. The theory is in this bunch of articles.

The filter loops, giving frequent updates of an estimation of the robots attitude. Therefore the code is in one endless loop. The faster the filter loops the better. But the execution speed is limited due to hardware limitation and other processes that need CPU time. To preserve time for other processes the iteration speed of the program is controlled. At the start of the program the sensors and data structures need to be initialized. Hardware initialization is done in a separate function called init (not included in the code below). This concentrates all hardware depended code into one function and keeps the main function easily reusable, even if you have different hardware.
As stated before the loop contains the filter. Aside from the code that implements the filter there is code to supply information to the user. These parts of the code are all commented away. If they are useful to you, you can uncomment them. Even then you can switch these parts of the code on or of by changing a true to false in the if statement surrounding this code. There are three ways to supply information to the end user. Data can be logged to excel, displayed on the NXT screen or a full color led (BLINKM) can be triggered if certain conditions are met.

If you find the code useful or informative please press the “like” button. This might encourage me to make more posts like this.

#pragma systemfile
#pragma autostarttasks;

// this defines the iteration time of the filter in msec
#define IMU_speed 25

#include "math.c"
#include "matrixDirectives.c"
#include "I2C.c"
#include "IMU_acclMatrix.c"
#include "IMU_gyroMatrix.c"
#include "IMU_blinkM.c"
/*
// Optional code to log the filter state in an excel fie
#define NXT2excelMaxData 8
#include "nxt2excel.c"
*/

// this struct will hold attitude, position (Todo) and speed (Todo) of the robot
typedef struct
{
  matrix C;
  bool initialized;
} tPosition;

tPosition position;

void init();
void normalizeM(matrix &in);



task main()
{
  float duration;
  matrix  U, T;
  vector I, P, Rate;
  long lastTime, start, wait;
  bool home, oldhome;
  // PI controller parameters, these are used to tune the filter
  float kp=0.0005,ki=0.005, id=0.99;

  // Initialize the sensors
  init();
  

  lastTime=nPgmTime;
  eraseDisplay();

  // initialize cosine matrix, the cosine matrix holds the attitude of the robot
  position.C[0][0]=1;
  position.C[0][1]=0;
  position.C[0][2]=0;
  position.C[1][0]=0;
  position.C[1][1]=1;
  position.C[1][2]=0;
  position.C[2][0]=0;
  position.C[2][1]=0;
  position.C[2][2]=1;

  // iterate forever, but halt if one of the sensorsreturns an error
  while (I2C_gyro.error==NO_ERR && I2C_accl.error==NO_ERR)
  {
    start=nPgmTime;
    alive();

    // get sensor readings, 
    //   gyro readings (in rad/sec) are stored in a vector (array of 3 elements) called gyro.rate
    //   acceleration readings (in g) are stored in a vector called accl.g
    Gyro_getGyroBiased();
    Accl_getAcclBiased();

    // for better filter output the exact time for each iteration is measured
    duration=(nPgmTime-lastTime)/1000.0;
    lastTime=nPgmTime;
    // nPgmTime wraps around when reaching maxint, correct for this
    if (duration<0) duration=IMU_speed/1000.0; 
    
    /*
    // Optional code to log the filter state in an excel fie
    if (false)
    {
	    for (int Rcol=0;Rcol<3;Rcol++)
	    {
		    NXT2excelAddValue(1,gyro.rate[Rcol]*1000);
		    NXT2excelAddValue(2,accl.g[Rcol]*1000);
		    NXT2excelAddValue(3,duration*1000.0);
		    NXT2excelAddValue(4,I[Rcol]*1000);
		    NXT2excelAddValue(5,P[Rcol]*1000);
		      for (int ii=0;ii<3;ii++)
		        NXT2excelAddValue(6+ii,position.C[Rcol][ii]*1000.0);
		    NXT2excelSendMessage();
	    }
    }
    */


    // scale the turning rate (rad/sec) to a change in attitude (rad)
    VscaleVS(gyro.rate,duration,Rate);

    // corract the gyro signal for offset (I) and accumulated errors (P), but do it slowly (ki, kp)
    // at the same time, convert it to a skew symmetric matrix
    // for speed not matrix functions are used here
    U[2][1]=gyro.rate[0]*duration-I[0]*ki-P[0]*kp;
    U[1][2]=-U[2][1];
    U[0][2]=gyro.rate[1]*duration-I[1]*ki-P[1]*kp;
    U[2][0]=-U[0][2];
    U[1][0]=gyro.rate[2]*duration-I[2]*ki-P[2]*kp;
    U[0][1]=-U[1][0];

    
    // transform the change in attitude in respect to robot axis into change in attitude in respect to global axis
    // for speed not matrix functions are used here
    // matrix equivalent: MproductMM(position.C,U,T);
    T[0][0]=position.C[0][1]*U[1][0]+position.C[0][2]*U[2][0];
    T[0][1]=position.C[0][0]*U[0][1]+position.C[0][2]*U[2][1];
    T[0][2]=position.C[0][0]*U[0][2]+position.C[0][1]*U[1][2];
    T[1][0]=position.C[1][1]*U[1][0]+position.C[1][2]*U[2][0];
    T[1][1]=position.C[1][0]*U[0][1]+position.C[1][2]*U[2][1];
    T[1][2]=position.C[1][0]*U[0][2]+position.C[1][1]*U[1][2];
    T[2][0]=position.C[2][1]*U[1][0]+position.C[2][2]*U[2][0];
    T[2][1]=position.C[2][0]*U[0][1]+position.C[2][2]*U[2][1];
    T[2][2]=position.C[2][0]*U[0][2]+position.C[2][1]*U[1][2];

    // add the cange in attitude to the attitude to get an updated attitude
    MsumMM(position.C,T,position.C);


    // Renormalize matrix
    // due to small errors the matrix gets corrupted over time (its axis are no longer perpendicular
    // this function corrects this
    normalizeM(position.C);

    // correct the attitude using tilt info from the accelerometer
    // but only if there is no or little acceleration, otherwise titl info can not be trusted
    if (abs(sqrt(accl.g[0]*accl.g[0]+accl.g[1]*accl.g[1]+accl.g[2]*accl.g[2])-1.0)<.2)
    {
      // Calculate difference (error) between culculated attitude and accelerometer
      // and use this in a proportional feedback (P)
	    P[0] = (position.C[2][1]*accl.g[2]) - (position.C[2][2]*accl.g[1]);
	    P[1] = (position.C[2][2]*accl.g[0]) - (position.C[2][0]*accl.g[2]);
	    P[2] = (position.C[2][0]*accl.g[1]) - (position.C[2][1]*accl.g[0]);
	    // Add the error to the accumulated error and use this in a integral feedback (I)
	    I[0]=I[0]*id+P[0]*duration;
	    I[1]=I[1]*id+P[1]*duration;
	    I[2]=I[2]*id+P[2]*duration;
	    }
    else
    {
    P[0]=0;
    P[1]=0;
    P[2]=0;
    }

    /*
    // optional code to drive the LED in the sensor
    if (true)
    {
      // test if the robot is in it's starting attitude
      if ( abs(position.C[2][0])<.009 && abs(position.C[2][1])<.009)
        home=true;
      else
        home=false;
      // change the color of the LED if needed
      if (home != oldhome)
      {
        if (home)
          blinkM_setColour(0 , 0, 128);
        else
          blinkM_setColour(64 , 0, 0);
      }
      oldhome=home;
    }
    */
    
    
    // optional code to display sensor and filter data
    if (true && time1[T1]>200)
    {
      eraseDisplay();
      nxtDisplayTextLine(7, "%5d",wait);
      //displayM(position.C);
      //displayV(accl.g,1);
      //displayV(gyro.rate,0);
      ClearTimer(T1);
    }
    //while (nNxtButtonPressed==-1);
    //while (nNxtButtonPressed!=-1);


    // wait for next iteration
    // the program does not assume a constant execution speed, 
    // instead it measures every iteration, and waits the exact amount of time to guarantue constant iteration speed
    wait=IMU_speed-(nPgmTime-start);

    if (wait<0)
    {
      wait=0;
      //PlaySound(soundException);
    }
    wait1Msec(wait);
  }

}


void init()
{
    // configure port;
  SensorType[S3]=sensorI2CCustom;
  nI2CRetries=3;
  position.initialized=false;
  accl.initialized=false;
  gyro.initialized=false;

  // configure blinkM;
  I2C_blinkM.port=S3;
  I2C_blinkM.address=0x12;
  blinkM_setColour(0 , 4, 0);


  // configure accl;
  I2C_accl.port=S3;
  I2C_accl.address=0xa6;
  Accl_init();
  Accl_getBias();
  accl.initialized=true;


  // configure gyro;
  I2C_gyro.port=S3;
  I2C_gyro.address=0xD0;
  Gyro_init();
  Gyro_getBias();
  gyro.initialized=true;


  if (I2C_gyro.error || I2C_accl.error)
  {
    nxtScrollText("IMU: %3d, %3d",I2C_gyro.error , I2C_accl.error);
    blinkM_setColour(63 , 0, 0);
    PlaySound(soundException);
    wait1Msec(2000);
  }
  blinkM_setColour(0 , 0 , 0);
  position.initialized=true;

}

//normalizes a matrix (takes 2.87 msec)
void normalizeM(matrix &in)
{
  float error, renorm;
  vector vx,vy, xor, yor, zor;


  VgetRowMS(in, 0,vx);
  VgetRowMS(in, 1,vy);
  dotProductVVS(vx, vy,error); // eq.18
  if (abs(error)>0.05)
  {
    VscaleVS(vy,error/-2.0, xor);
    VsumVV(vx, xor, xor); //eq.19

    VscaleVS(vx,error/-2.0, yor);
    VsumVV(vy, yor, yor); //eq.19

    VcrossProductVV(xor,yor, zor); //eq.20

    dotProductVVS(xor, xor,error);
    renorm = .5 *(3 - error); //eq.21
    for ( int Rcol=0; Rcol<Msize;Rcol++)
      in[0][Rcol]=renorm*xor[Rcol];
    dotProductVVS(yor, yor,error);
    renorm = .5 *(3 - error); //eq.21
    for ( int Rcol=0; Rcol<Msize;Rcol++)
      in[1][Rcol]=renorm*yor[Rcol];
    dotProductVVS(zor, zor,error);
    renorm = .5 *(3 - error); //eq.21
    for ( int Rcol=0; Rcol<Msize;Rcol++)
      in[2][Rcol]=renorm*zor[Rcol];
  }
}
Advertisements

“I know why you’re here, Neo. I know what you’ve been doing… why you hardly sleep, why you live alone, and why night after night, you sit by your computer.” from The Matrix.

As you might know from my previous posts there is quite a bit of math involved in computing speed, position and attitude from IMU sensor data. Thus far I have dealt with translating robot related measurements to world related measurements and offset correction. But there is more to come, correction for sensor position on the robot and, most important, sensor fusion. No matter what technique I will use for this, all documentation is in matrix math. This makes sense as matrix math is very compact. Therefore I decided to rewrite my IMU code to matrix math. I did this before when I built my Kalman filter, but then I abandoned in the end. But then I was using 2*2 matrices, now I am working in 3 dimensions and my matrices are 3*3. The bigger the matrices the more compact matrix math is compared to component math.

So, I’m drawn back into the matrix. Last night I have converted my library of matrix functions to work with 3*3 matrices. Next few nights I will convert the functionality I already have build for the IMU. Only then I can continue with sensor fusion. It really is two steps forward, one step back.