John Fitzpatrick
IUPUI Project #1
Dependencies: FXOS8700 FXAS21002 kalman MotionSensor
Revision 1:73243284f9d2, committed 2021-03-15
- 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
--- /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
+ }
+
}
+