Archives for category: blinkM

Here is a short update of my works.

I’m doing two projects at the same time.
First, there is Sidbot. It waits till I master Java threads.
Second, I’m building a holonomic or Killough pilot in Lejos. This project hangs on me not knowing how far the Robot has travelled. This info is needed for the pilot to work with a navigator.
Actually, my main project now is learning java. This goes well but there us a lot to it.

Few weeks ago I ordered two more BlinkM’s. These are I2C driven all color led’s. I wrote a driver to address these led’s. The led’s are now mounted on each corner of Sidbot. So Sidbot can shine all colors to all Sides of it’s triangular body. Each led is driven individually so each one can have it’s own color. I made a nice flashlight show to test the driver.
I finished drivers for my IMU too. The filter to fuse the signals is also finished but it can’t run in a separate thread yet.

I also have been on holidays and busy with work. But these matters don’t concern this blog.

Advertisements

I totally forgot to post the video I made of the IMU sensor.

This clips shows how accurate the IMU sensor can be. This IMU sensor combines a 3 axis gyro and a 3 axis accelerometer. The signals from both sensors are combined using a nonlinear complementary filter to get an output signal that has low noise and is drift free. (Well actually it is only drift free over 2 axis. It still drifts over the Z-axis.) This home made sensor will show a blue light when it is horizontal using a 0.5 degrees margin. Otherwise the light will turn red. The full color LED is a BlinkM.
You’ll find more about this sensor in my previous posts.

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];
  }
}