main.cpp

00001 #include "mbed.h"
00002 #include "ST7735_TFT.h"
00003 #include "Arial24x23i.h"
00004 #include "Arial11x11.h"
00005 #include "Arial9x9.h"
00006 #include "MPU6050.h"
00007 #include "MMA8451Q.h"
00008 
00009 #define MMA8451_I2C_ADDRESS (0x1d<<1)
00010 
00011 Serial pc(USBTX, USBRX); // tx, rx default baud rate: 9600
00012  
00013 void compFilter();
00014 void preparePeriferals();
00015 
00016 MPU6050 mpu6050;           // class: MPU6050, object: mpu6050 
00017 PinName const SDA = PTE25;
00018 PinName const SCL = PTE24;
00019 MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS); //on-board accelerometer
00020 //ST7735_TFT lcd(PTD6, NC, PTD5, PTA13, PTD2, PTD4, "TFT"); // TFT; sda, miso (not connected), sck, cs, AO(rs), reset
00021 Ticker systick; 
00022 
00023 float pitchAngle = 0;
00024 float rollAngle = 0;
00025 float rollX = 0;
00026 float pitchY = 0;
00027 int main()
00028 {   
00029     
00030     pc.baud(9600);                              // baud rate: 9600
00031     mpu6050.whoAmI();                           // Communication test: WHO_AM_I register reading 
00032     wait(1);
00033     mpu6050.calibrate(accelBias,gyroBias);      // Calibrate MPU6050 and load biases into bias registers
00034     pc.printf("Calibration is completed. \r\n");
00035     wait(0.5);
00036     mpu6050.init();                             // Initialize the sensor
00037     wait(1);
00038     pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
00039     wait_ms(500);
00040     systick.attach(&compFilter, 0.005);    // calls the complementaryFilter func. every 5 ms (200 Hz sampling period)             
00041     while (true) 
00042     {
00043         atan (ax);
00044         //pc.printf("Accelerometer (onboard)   X = %1.2f, Y = %1.2f, Z = %1.2f\r\n", acc.getAccX(), acc.getAccY(), acc.getAccZ());
00045         pc.printf("Accelerometer MPU6050(g)  X = %.3f,  Y = %.3f,  Z = %.3f\r\n", ax, ay, az);
00046         pc.printf("Gyroscope MPU6050(deg/s) gx = %.3f, gy = %.3f,  gz = %.3f\r\n", gx, gy, gz);
00047         pc.printf("Gyroscope MPU6050(deg/s) roll = %.3f, pitch = %.3f\r\n",rollAngle, pitchAngle);
00048         wait(1.0f);     
00049                                                                
00050     }    
00051 }
00052 void compFilter() {
00053     mpu6050.complementaryFilter(&pitchAngle, &rollAngle);
00054 }