main.cpp

00001 #include "ITG3200.h"
00002 #include "ADXL345_I2C.h"
00003 
00004 Serial pc(USBTX, USBRX);
00005 ITG3200 gyro(p9, p10);
00006 ADXL345_I2C accel(p9, p10);
00007 
00008 int main() {
00009     gyro.setWhoAmI(0x68);
00010     pc.printf("Now starting 6-degrees-of-freedom (ITG-3200 ADXL345) test...\n");
00011     pc.printf("Accelerometer Device ID is: 0x%02x\n", accel.getDeviceID());
00012     pc.printf("Gyro Devide ID is: 0x02x\n", gyro.getWhoAmI());
00013     
00014     int accel_read[3] = {0, 0, 0};
00015     
00016     // Accel setup
00017     // These are here to test whether any of the initialization fails. It will print the failure
00018     if (accel.setPowerControl(0x00)){
00019          pc.printf("didn't intitialize power control\n"); 
00020          return 0;  }
00021     //Full resolution, +/-16g, 4mg/LSB.
00022     wait(.001);
00023      
00024     if(accel.setDataFormatControl(0x0B)){
00025         pc.printf("didn't set data format\n");
00026         return 0;  }
00027     wait(.001);
00028     
00029     //3.2kHz data rate.
00030     if(accel.setDataRate(ADXL345_3200HZ)){
00031        pc.printf("didn't set data rate\n");
00032        return 0;    }
00033     wait(.001);
00034     
00035     //Measurement mode.
00036     
00037     if(accel.setPowerControl(MeasurementMode)) {
00038        pc.printf("didn't set the power control to measurement\n"); 
00039        return 0;   
00040     }
00041      
00042     // Gyro setup   
00043     gyro.setLpBandwidth(LPFBW_42HZ);
00044 
00045     while (1) {
00046         wait(0.1);
00047         
00048         accel.getOutput(accel_read);
00049         
00050         pc.printf("Gyro[%5i, %5i, %5i]   Accel[%4i, %4i, %4i]\n", 
00051                             gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ(),
00052                             (int16_t)accel_read[0], (int16_t)accel_read[1], (int16_t)accel_read[2]);
00053     }
00054     
00055 }