IUPUI Project #1

Dependencies:   FXOS8700 FXAS21002 kalman MotionSensor

Files at this revision

API Documentation at this revision

Comitter:
fitzpatrick
Date:
Mon Mar 15 19:27:29 2021 +0000
Parent:
0:21d86aae6b2a
Commit message:
IUPUI Embedded project #1; http://robotsforroboticists.com/kalman-filtering/; http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/; ;

Changed in this revision

FXAS21002.lib Show annotated file Show diff for this revision Revisions of this file
FXOS8700.lib Show annotated file Show diff for this revision Revisions of this file
Grove_temperature.lib Show diff for this revision Revisions of this file
MotionSensor.lib Show annotated file Show diff for this revision Revisions of this file
kalman.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXAS21002.lib	Mon Mar 15 19:27:29 2021 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/AswinSivakumar/code/FXAS21002/#c9ebfc81e8b6
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXOS8700.lib	Mon Mar 15 19:27:29 2021 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/AswinSivakumar/code/FXOS8700/#df2167370234
--- a/Grove_temperature.lib	Tue Aug 07 04:36:25 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/MACRUM/code/Grove_temperature/#aee37a51ccbb
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotionSensor.lib	Mon Mar 15 19:27:29 2021 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/components/code/MotionSensor/#4d6e28d4a18a
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.lib	Mon Mar 15 19:27:29 2021 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/cdonate/code/kalman/#8a460b0d6f09
--- 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
+        }
+
 }
 
+