hige dura
main.cpp@0:b89a41f096ae, 2012-05-15 (annotated)
- Committer:
- higedura
- Date:
- Tue May 15 08:31:45 2012 +0000
- Revision:
- 0:b89a41f096ae
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| higedura | 0:b89a41f096ae | 1 | #include "ITG3200.h" |
| higedura | 0:b89a41f096ae | 2 | |
| higedura | 0:b89a41f096ae | 3 | Serial pc(USBTX, USBRX); |
| higedura | 0:b89a41f096ae | 4 | ITG3200 gyro(p9, p10); |
| higedura | 0:b89a41f096ae | 5 | |
| higedura | 0:b89a41f096ae | 6 | int main() { |
| higedura | 0:b89a41f096ae | 7 | |
| higedura | 0:b89a41f096ae | 8 | float t = 0; |
| higedura | 0:b89a41f096ae | 9 | float dt = 0.1; |
| higedura | 0:b89a41f096ae | 10 | double Gyro0 [3] = {0, 0, 0}; |
| higedura | 0:b89a41f096ae | 11 | double Gyro1 [3] = {0, 0, 0}; |
| higedura | 0:b89a41f096ae | 12 | double Ang0 [3] = {0, 0, 0}; |
| higedura | 0:b89a41f096ae | 13 | double Ang1 [3] = {0, 0, 0}; |
| higedura | 0:b89a41f096ae | 14 | |
| higedura | 0:b89a41f096ae | 15 | pc.printf("Now starting ITG-3200 test\n"); |
| higedura | 0:b89a41f096ae | 16 | pc.printf(" Unit: Gyro[deg/s] Angle[deg]\n"); |
| higedura | 0:b89a41f096ae | 17 | pc.printf(" Time GyroX GyroY GyroZ AngX AngY AngZ\n"); |
| higedura | 0:b89a41f096ae | 18 | |
| higedura | 0:b89a41f096ae | 19 | //Set highest bandwidth. |
| higedura | 0:b89a41f096ae | 20 | gyro.setLpBandwidth(LPFBW_42HZ); |
| higedura | 0:b89a41f096ae | 21 | |
| higedura | 0:b89a41f096ae | 22 | //Arbitrary wait for printf clarity. |
| higedura | 0:b89a41f096ae | 23 | wait(0.1); |
| higedura | 0:b89a41f096ae | 24 | |
| higedura | 0:b89a41f096ae | 25 | while (1) { |
| higedura | 0:b89a41f096ae | 26 | //pc.printf("%x\n",gyro.getWhoAmI()); |
| higedura | 0:b89a41f096ae | 27 | // YAL 9DOF: x+16, y-39, z+40 |
| higedura | 0:b89a41f096ae | 28 | // My 9DOF: x, y, z |
| higedura | 0:b89a41f096ae | 29 | Gyro1[0] = (gyro.getGyroX()+16)/14.375; Gyro1[1] = (gyro.getGyroY()-39)/14.375; Gyro1[2] = (gyro.getGyroZ()+40)/14.375; |
| higedura | 0:b89a41f096ae | 30 | |
| higedura | 0:b89a41f096ae | 31 | // Low pass filter for gyro |
| higedura | 0:b89a41f096ae | 32 | if( -1.0<Gyro1[0] && Gyro1[0]<1.0 ){ Gyro1[0]=0; } |
| higedura | 0:b89a41f096ae | 33 | if( -1.0<Gyro1[1] && Gyro1[1]<1.0 ){ Gyro1[1]=0; } |
| higedura | 0:b89a41f096ae | 34 | if( -1.0<Gyro1[2] && Gyro1[2]<1.0 ){ Gyro1[2]=0; } |
| higedura | 0:b89a41f096ae | 35 | |
| higedura | 0:b89a41f096ae | 36 | // Trapezoidal integration |
| higedura | 0:b89a41f096ae | 37 | Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2; |
| higedura | 0:b89a41f096ae | 38 | Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2; |
| higedura | 0:b89a41f096ae | 39 | Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2; |
| higedura | 0:b89a41f096ae | 40 | |
| higedura | 0:b89a41f096ae | 41 | pc.printf("%6.1f, %5.0f, %5.0f, %5.0f, %6.1f, %6.1f, %6.1f\n\r", |
| higedura | 0:b89a41f096ae | 42 | t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]); |
| higedura | 0:b89a41f096ae | 43 | |
| higedura | 0:b89a41f096ae | 44 | Gyro0[0] = Gyro1[0]; Gyro0[1] = Gyro1[1]; Gyro0[2] = Gyro1[2]; |
| higedura | 0:b89a41f096ae | 45 | Ang0[0] = Ang1[0]; Ang0[1] = Ang1[1]; Ang0[2] = Ang1[2]; |
| higedura | 0:b89a41f096ae | 46 | t = t+dt; |
| higedura | 0:b89a41f096ae | 47 | wait(0.1); |
| higedura | 0:b89a41f096ae | 48 | |
| higedura | 0:b89a41f096ae | 49 | } |
| higedura | 0:b89a41f096ae | 50 | |
| higedura | 0:b89a41f096ae | 51 | } |