Diff: main.cpp

Revision:

0:40b56bdec1d2

Child:

1:2ae94169eee6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jul 14 10:45:54 2015 +0000
@@ -0,0 +1,98 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "ledControl.h"
+#include "brushlessController_L293D.h"
+
+/* Defined in the MPU6050.cpp file  */
+// I2C i2c(p9,p10);         // setup i2c (SDA,SCL)  
+
+Serial ftdi(p13,p14);    // default baud rate: 9600
+MPU6050 mpu6050;         // class: MPU6050, object: mpu6050 
+Ticker toggler1;
+Ticker filter;           
+
+void toggle_led1();
+void toggle_led2();
+void complementaryFilter(float* pitch, float* roll);
+void compFilter();
+
+float pitchAngle = 0;
+float rollAngle = 0;
+int prevStep = 0;               // previous step for the brushless motor
+int errorMargin = 6;   // error margin in degrees for stabilizing the gimbal system
+int setPoint = 0;               // set point in degrees for the gimbal system 
+
+int main() 
+{
+    ftdi.baud(9600);                            // baud rate: 9600
+    i2c.frequency(400000);                      // fast i2c: 400 kHz
+    mpu6050.whoAmI();                           // Communication test: WHO_AM_I register reading 
+ //   wait(1);
+    mpu6050.calibrate(accelBias,gyroBias);      // Calibrate MPU6050 and load biases into bias registers
+    ftdi.printf("Calibration is completed. \r\n");
+ //   wait(0.5);
+    mpu6050.init();                             // Initialize the sensor
+ //   wait(1);
+    ftdi.printf("MPU6050 is initialized for operation.. \r\n\r\n");
+  //  wait_ms(500);
+    
+    while(1) 
+    {
+        filter.attach(&compFilter, 0.005);    // Call the complementaryFilter func. every 5 ms (200 Hz sampling period)
+         
+        if (abs(rollAngle - setPoint) < errorMargin)
+        {
+            // Do not move if above statement is true
+            led4 = 1;
+        }          
+        else if(rollAngle > setPoint)
+        {
+            oneStep(0,30, &prevStep);
+            led4 = 0;
+        }
+        else 
+        {   
+            oneStep(1,30, &prevStep);
+            led4 = 0;
+        }
+        wait_ms(5);   // wait for new rollAngle data to arrive
+    }
+}
+
+void toggle_led1() {ledToggle(1);}
+void toggle_led2() {ledToggle(2);}
+
+/* This function is created to avoid address error that caused from Ticker.attach func */ 
+void compFilter() {complementaryFilter(&pitchAngle, &rollAngle);}
+
+void complementaryFilter(float* pitch, float* roll)
+{
+    /* Get actual acc value */
+    mpu6050.readAccelData(accelData);
+    mpu6050.getAres();
+    ax = accelData[0]*aRes - accelBias[0];
+    ay = accelData[1]*aRes - accelBias[1];
+    az = accelData[2]*aRes - accelBias[2]; 
+
+    /* Get actual gyro value */
+    mpu6050.readGyroData(gyroData);
+    mpu6050.getGres();     
+    gx = gyroData[0]*gRes;  // - gyroBias[0];      // Results are better without extracting gyroBias[i]
+    gy = gyroData[1]*gRes;  // - gyroBias[1]; 
+    gz = gyroData[2]*gRes;  // - gyroBias[2]; 
+
+    float pitchAcc, rollAcc;
+
+    /* Integrate the gyro data(deg/s) over time to get angle */
+    *pitch += gx * dt;  // Angle around the X-axis
+    *roll -=  gy * dt;  // Angle around the Y-axis
+    
+    /* Turning around the X-axis results in a vector on the Y-axis
+    whereas turning around the Y-axis results in a vector on the X-axis. */
+    pitchAcc = atan2f((float)accelData[1], (float)accelData[2])*180/PI;
+    rollAcc  = atan2f((float)accelData[0], (float)accelData[2])*180/PI;
+    
+    /* Apply Complementary Filter */
+    *pitch = *pitch * 0.98 + pitchAcc * 0.02;
+    *roll  = *roll  * 0.98 + rollAcc  * 0.02;   
+}
\ No newline at end of file