main.cpp

00001 #include "ITG3200.h"
00002 
00003 Serial pc(USBTX, USBRX);
00004 ITG3200 gyro(p9, p10);
00005       
00006 int main() {
00007     
00008     float  t    =   0;
00009     float dt    =   0.1;
00010     double Gyro0 [3] = {0, 0, 0};
00011     double Gyro1 [3] = {0, 0, 0};
00012     double  Ang0 [3] = {0, 0, 0};
00013     double  Ang1 [3] = {0, 0, 0};
00014     
00015     pc.printf("Now starting ITG-3200 test\n");
00016     pc.printf(" Unit: Gyro[deg/s] Angle[deg]\n");
00017     pc.printf("  Time  GyroX  GyroY  GyroZ    AngX    AngY    AngZ\n");
00018     
00019     //Set highest bandwidth.
00020     gyro.setLpBandwidth(LPFBW_42HZ);
00021     
00022     //Arbitrary wait for printf clarity.
00023     wait(0.1);
00024     
00025     while (1) {
00026         //pc.printf("%x\n",gyro.getWhoAmI());
00027         // YAL 9DOF: x+16, y-39, z+40
00028         //  My 9DOF:  x, y, z
00029         Gyro1[0] = (gyro.getGyroX()+16)/14.375;      Gyro1[1] = (gyro.getGyroY()-39)/14.375;      Gyro1[2] = (gyro.getGyroZ()+40)/14.375;
00030         
00031         // Low pass filter for gyro
00032         if( -1.0<Gyro1[0] && Gyro1[0]<1.0 ){    Gyro1[0]=0;  }
00033         if( -1.0<Gyro1[1] && Gyro1[1]<1.0 ){    Gyro1[1]=0;  }
00034         if( -1.0<Gyro1[2] && Gyro1[2]<1.0 ){    Gyro1[2]=0;  }
00035         
00036         // Trapezoidal integration
00037         Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2;
00038         Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2;
00039         Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2;
00040         
00041         pc.printf("%6.1f, %5.0f, %5.0f, %5.0f, %6.1f, %6.1f, %6.1f\n\r", 
00042                   t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]);
00043         
00044         Gyro0[0] = Gyro1[0];  Gyro0[1] = Gyro1[1];  Gyro0[2] = Gyro1[2];
00045         Ang0[0]  = Ang1[0];   Ang0[1]  = Ang1[1];   Ang0[2]  = Ang1[2];
00046         t   =   t+dt;
00047         wait(0.1);
00048 
00049     }
00050 
00051 }