Diff: main.cpp

Revision:

1:73243284f9d2

Parent:

0:21d86aae6b2a

--- a/main.cpp	Tue Aug 07 04:36:25 2018 +0000
+++ b/main.cpp	Mon Mar 15 19:27:29 2021 +0000
@@ -1,15 +1,158 @@
 #include "mbed.h"
-#include "Grove_temperature.h"
+#include "FXOS8700.h"
+#include "FXAS21002.h"
+#include "kalman.c"
+
+#define ROLL_SERVO_CENTER 1570
+#define ROLL_SERVO_MAX 2000
+#define ROLL_SERVO_MIN 1000
+
+#define PITCH_SERVO_CENTER 1570
+#define PITCH_SERVO_MAX 2000
+#define PITCH_SERVO_MIN 1000
+
+#define PI             3.14159265
+#define Rad2Degree       57.2957795
+
+
+Serial pc(USBTX, USBRX); // Serial Port 115200
+
+// Sensor GPIO Definitions, I2C
+FXOS8700 accel(D14,D15);
+FXOS8700 mag(D14,D15);
+FXAS21002 gyro(D14,D15);
+
+//Timer used in Kalman filter
+Timer ProgramTimer;
 
-Grove_temperature temp(A4);
-Serial pc(USBTX, USBRX);
+// Servo GPIO Definitions
+PwmOut Pitch_M(D11);
+PwmOut Roll_M(D10);
+
+//Define Kalman filter Parameters
+kalman filter_pitch; 
+kalman filter_roll;
+
+float R;
+float Roll_P_C = 1.0f, Roll_I_C = 0.01f, Roll_D_C = 0.01f; //Roll PID coefficient Values
+float Roll_P = 0, Roll_I = 0, Roll_D =0, Roll_Correct = 0, Roll_Error = 0;  //Roll PID Control Values
+float Pitch_P_C = 1.0f, Pitch_I_C = 0.01f, Pitch_D_C = 0.01f; //Pitch PID coefficient Values
+float Pitch_P = 0, Pitch_I = 0, Pitch_D = 0, Pitch_Correct = 0, Pitch_Error = 0; //Pitch PID control Values
+int Roll_Servo_Val, Pitch_Servo_Val;  // Servo pulse widths
+double angle[3];      // Kalman Pitch and Roll angles
+unsigned long timer;  // absolute time stamp
+int val, Loop_Count;  // Loop_Count used to settel kalman filter
+
+int main() {
+
+    pc.baud(115200); // Serial Port 115200
+    
+    // Configure Accelerometer FXOS8700, Magnetometer FXOS8700 & Gyroscope FXAS21002
+    accel.accel_config();
+    mag.mag_config();
+    gyro.gyro_config();
+    
+    // Initialize Kalman Filter
+    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
+    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
 
-// main() runs in its own thread in the OS
-int main() {
-    pc.baud(115200);
+    // Accelerometer, Magnomometer and Gyro Variables
+    float adata[3];       //Accelerometer x, y, x values
+    float mag_data[3];    //Magnomometer x, y, x values
+    float gyro_data[3];   //Gyro pitch, roll, yaw values
+    
+    // Roll Servo Setup
+    int Roll_pulse = ROLL_SERVO_CENTER;
+    Roll_M.period_us(20000);          // servo requires a 20ms period
+    Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms
+    // Pitch Servo Setup  
+    int Pitch_pulse = PITCH_SERVO_CENTER;
+    Pitch_M.period_us(20000);          // servo requires a 20ms period
+    Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms
+    wait(1);  // Allow servos to settel 
+    
+    //Absolute Time Needed to Calculate Delta Time
+    ProgramTimer.start();
+    timer = ProgramTimer.read_us();
+    
+    Loop_Count = 15;   // Allows Kalman filter to sync.
+
     while (true) {
-        pc.printf("temperature = %2.2f\n", temp.getTemperature());
-        wait(1);
-    }
+
+        //scanf("%d", &val);                       // Used for debug
+        //printf("\r\n Value is %d \r\n", val);    // Used for debug
+
+        // Acquire Accelerometer values
+        accel.acquire_accel_data_g(adata);  // Acquire Acceleration Vectors
+        printf("\r\n Accelerometer Values\r\n");
+        printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", adata[0],adata[1],adata[2]);
+                
+        // Acquire Magnomometer Vectors
+        mag.acquire_mag_data_uT(mag_data);        // Acquire Magnomometer Vectors
+        printf("\r\n Magnomometer Values\r\n");
+        printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", mag_data[0],mag_data[1],mag_data[2]);    
+                
+        // Acquire Gyro values
+        gyro.acquire_gyro_data_dps(gyro_data);    // Acquire Gyro values
+        printf("\r\n Gyro Values\r\n");
+        printf(" Pitch: %4.2f,\t Roll: %4.2f,\t Yaw: %4.2f\r\n", gyro_data[0],gyro_data[1],gyro_data[2]);
+        
+        // RMS Value of Accelerometer 
+        R = sqrt(std::pow(adata[0], 2) + std::pow(adata[1], 2) + std::pow(adata[2], 2));
+        
+        // Kalman Filter
+        kalman_predict(&filter_pitch, gyro_data[0],  (ProgramTimer.read_us() - timer)); // Predict uses Pitch portion of gyro and delta time
+        kalman_update(&filter_pitch, acos(adata[0]/R));   //Update uses normalized Pitch portion of accelerometer
+        kalman_predict(&filter_roll, gyro_data[1],  (ProgramTimer.read_us() - timer)); // Predict uses Roll portion of gyro and delta time
+        kalman_update(&filter_roll, acos(adata[1]/R));    //Update uses normalized Roll portion of accelerometer
+        
+        angle[0] = kalman_get_angle(&filter_pitch);   // Kalman Pitch
+        angle[1] = kalman_get_angle(&filter_roll);    // Kalman Roll
+        
+        timer = ProgramTimer.read_us();     // Update Timer
+    
+        printf("\r\n Roll Angle X: %.6f   Pitch Angle Y: %.6f \r\n", Rad2Degree * angle[1], Rad2Degree * angle[0]);
+
+        if (Loop_Count > 0) 
+        {
+            // Loop Iterations to Initialize Kalman Filter to sync.
+            Loop_Count--;
+            printf("\n\rLoop_Count = %d\n\r", Loop_Count);
+        }
+        else
+        {
+            // Loop Iterations Complete, Apply Results of Kalman Filter
+            // PID Controller (Mostly Proportional)
+            Roll_Error = (Rad2Degree * angle[1]) - 90;  // Calculate Roll Error
+            Roll_D = Roll_Error - Roll_P;               // Differentiation of Roll Error
+            Roll_I = Roll_I + Roll_Error;               // Intigration of Roll Error
+            Roll_P = Roll_Error;                        // Proportional Roll Error
+            Roll_Correct = Roll_P * Roll_P_C + Roll_I * Roll_I_C + Roll_D * Roll_D_C;  // Sum product of PID coefficients and values
+            printf("\n\rRoll_Correct = %f\n\r",Roll_Correct);       
+            
+            Roll_pulse =  Roll_pulse + Roll_Correct;    // Update Roll Servo PWM Value
+            if (Roll_pulse <= ROLL_SERVO_MIN) Roll_pulse = ROLL_SERVO_MIN;
+            if (Roll_pulse >= ROLL_SERVO_MAX) Roll_pulse = ROLL_SERVO_MAX;
+            Roll_M.pulsewidth_us(Roll_pulse);           // servo position determined by a pulsewidth between 1-2ms  
+            printf("\n\rRoll_pulse = %d\n\r",Roll_pulse);
+            
+            Pitch_Error = (Rad2Degree * angle[0]) - 90; // Calculate Pitch Error
+            Pitch_D = Pitch_Error - Pitch_P;            // Differentiation of Pitch Error
+            Pitch_I = Pitch_I + Pitch_Error;            // Intigration of Pitch Error
+            Pitch_P = Pitch_Error;                      // Proportional Pitch Error
+            Pitch_Correct = Pitch_P * Pitch_P_C + Pitch_I * Pitch_I_C + Pitch_D * Pitch_D_C; // Sum product of PID coefficients and values
+            printf("\n\rPitch_Correct = %f\n\r",Pitch_Correct);       
+
+            Pitch_pulse =  Pitch_pulse + Pitch_Correct; // Update Roll Servo PWM Value
+            if (Pitch_pulse <= PITCH_SERVO_MIN) Pitch_pulse = PITCH_SERVO_MIN;
+            if (Pitch_pulse >= PITCH_SERVO_MAX) Pitch_pulse = PITCH_SERVO_MAX;
+            Pitch_M.pulsewidth_us(Pitch_pulse);         // servo position determined by a pulsewidth between 1-2ms  
+            printf("\n\rPitch_pulse = %d\n\r",Pitch_pulse);               
+        }         
+        
+        wait(0.01);  // Loop timer
+        }
+
 }
 
+