hige dura
/
ITG3200_I2C
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 20 2022 02:11:10 by 1.7.2