IUPUI Project #1
Dependencies: FXOS8700 FXAS21002 kalman MotionSensor
main.cpp@1:73243284f9d2, 2021-03-15 (annotated)
- Committer:
- fitzpatrick
- Date:
- Mon Mar 15 19:27:29 2021 +0000
- Revision:
- 1:73243284f9d2
- Parent:
- 0:21d86aae6b2a
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/; ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MACRUM | 0:21d86aae6b2a | 1 | #include "mbed.h" |
fitzpatrick | 1:73243284f9d2 | 2 | #include "FXOS8700.h" |
fitzpatrick | 1:73243284f9d2 | 3 | #include "FXAS21002.h" |
fitzpatrick | 1:73243284f9d2 | 4 | #include "kalman.c" |
fitzpatrick | 1:73243284f9d2 | 5 | |
fitzpatrick | 1:73243284f9d2 | 6 | #define ROLL_SERVO_CENTER 1570 |
fitzpatrick | 1:73243284f9d2 | 7 | #define ROLL_SERVO_MAX 2000 |
fitzpatrick | 1:73243284f9d2 | 8 | #define ROLL_SERVO_MIN 1000 |
fitzpatrick | 1:73243284f9d2 | 9 | |
fitzpatrick | 1:73243284f9d2 | 10 | #define PITCH_SERVO_CENTER 1570 |
fitzpatrick | 1:73243284f9d2 | 11 | #define PITCH_SERVO_MAX 2000 |
fitzpatrick | 1:73243284f9d2 | 12 | #define PITCH_SERVO_MIN 1000 |
fitzpatrick | 1:73243284f9d2 | 13 | |
fitzpatrick | 1:73243284f9d2 | 14 | #define PI 3.14159265 |
fitzpatrick | 1:73243284f9d2 | 15 | #define Rad2Degree 57.2957795 |
fitzpatrick | 1:73243284f9d2 | 16 | |
fitzpatrick | 1:73243284f9d2 | 17 | |
fitzpatrick | 1:73243284f9d2 | 18 | Serial pc(USBTX, USBRX); // Serial Port 115200 |
fitzpatrick | 1:73243284f9d2 | 19 | |
fitzpatrick | 1:73243284f9d2 | 20 | // Sensor GPIO Definitions, I2C |
fitzpatrick | 1:73243284f9d2 | 21 | FXOS8700 accel(D14,D15); |
fitzpatrick | 1:73243284f9d2 | 22 | FXOS8700 mag(D14,D15); |
fitzpatrick | 1:73243284f9d2 | 23 | FXAS21002 gyro(D14,D15); |
fitzpatrick | 1:73243284f9d2 | 24 | |
fitzpatrick | 1:73243284f9d2 | 25 | //Timer used in Kalman filter |
fitzpatrick | 1:73243284f9d2 | 26 | Timer ProgramTimer; |
MACRUM | 0:21d86aae6b2a | 27 | |
fitzpatrick | 1:73243284f9d2 | 28 | // Servo GPIO Definitions |
fitzpatrick | 1:73243284f9d2 | 29 | PwmOut Pitch_M(D11); |
fitzpatrick | 1:73243284f9d2 | 30 | PwmOut Roll_M(D10); |
fitzpatrick | 1:73243284f9d2 | 31 | |
fitzpatrick | 1:73243284f9d2 | 32 | //Define Kalman filter Parameters |
fitzpatrick | 1:73243284f9d2 | 33 | kalman filter_pitch; |
fitzpatrick | 1:73243284f9d2 | 34 | kalman filter_roll; |
fitzpatrick | 1:73243284f9d2 | 35 | |
fitzpatrick | 1:73243284f9d2 | 36 | float R; |
fitzpatrick | 1:73243284f9d2 | 37 | float Roll_P_C = 1.0f, Roll_I_C = 0.01f, Roll_D_C = 0.01f; //Roll PID coefficient Values |
fitzpatrick | 1:73243284f9d2 | 38 | float Roll_P = 0, Roll_I = 0, Roll_D =0, Roll_Correct = 0, Roll_Error = 0; //Roll PID Control Values |
fitzpatrick | 1:73243284f9d2 | 39 | float Pitch_P_C = 1.0f, Pitch_I_C = 0.01f, Pitch_D_C = 0.01f; //Pitch PID coefficient Values |
fitzpatrick | 1:73243284f9d2 | 40 | float Pitch_P = 0, Pitch_I = 0, Pitch_D = 0, Pitch_Correct = 0, Pitch_Error = 0; //Pitch PID control Values |
fitzpatrick | 1:73243284f9d2 | 41 | int Roll_Servo_Val, Pitch_Servo_Val; // Servo pulse widths |
fitzpatrick | 1:73243284f9d2 | 42 | double angle[3]; // Kalman Pitch and Roll angles |
fitzpatrick | 1:73243284f9d2 | 43 | unsigned long timer; // absolute time stamp |
fitzpatrick | 1:73243284f9d2 | 44 | int val, Loop_Count; // Loop_Count used to settel kalman filter |
fitzpatrick | 1:73243284f9d2 | 45 | |
fitzpatrick | 1:73243284f9d2 | 46 | int main() { |
fitzpatrick | 1:73243284f9d2 | 47 | |
fitzpatrick | 1:73243284f9d2 | 48 | pc.baud(115200); // Serial Port 115200 |
fitzpatrick | 1:73243284f9d2 | 49 | |
fitzpatrick | 1:73243284f9d2 | 50 | // Configure Accelerometer FXOS8700, Magnetometer FXOS8700 & Gyroscope FXAS21002 |
fitzpatrick | 1:73243284f9d2 | 51 | accel.accel_config(); |
fitzpatrick | 1:73243284f9d2 | 52 | mag.mag_config(); |
fitzpatrick | 1:73243284f9d2 | 53 | gyro.gyro_config(); |
fitzpatrick | 1:73243284f9d2 | 54 | |
fitzpatrick | 1:73243284f9d2 | 55 | // Initialize Kalman Filter |
fitzpatrick | 1:73243284f9d2 | 56 | kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
fitzpatrick | 1:73243284f9d2 | 57 | kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); |
MACRUM | 0:21d86aae6b2a | 58 | |
fitzpatrick | 1:73243284f9d2 | 59 | // Accelerometer, Magnomometer and Gyro Variables |
fitzpatrick | 1:73243284f9d2 | 60 | float adata[3]; //Accelerometer x, y, x values |
fitzpatrick | 1:73243284f9d2 | 61 | float mag_data[3]; //Magnomometer x, y, x values |
fitzpatrick | 1:73243284f9d2 | 62 | float gyro_data[3]; //Gyro pitch, roll, yaw values |
fitzpatrick | 1:73243284f9d2 | 63 | |
fitzpatrick | 1:73243284f9d2 | 64 | // Roll Servo Setup |
fitzpatrick | 1:73243284f9d2 | 65 | int Roll_pulse = ROLL_SERVO_CENTER; |
fitzpatrick | 1:73243284f9d2 | 66 | Roll_M.period_us(20000); // servo requires a 20ms period |
fitzpatrick | 1:73243284f9d2 | 67 | Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms |
fitzpatrick | 1:73243284f9d2 | 68 | // Pitch Servo Setup |
fitzpatrick | 1:73243284f9d2 | 69 | int Pitch_pulse = PITCH_SERVO_CENTER; |
fitzpatrick | 1:73243284f9d2 | 70 | Pitch_M.period_us(20000); // servo requires a 20ms period |
fitzpatrick | 1:73243284f9d2 | 71 | Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms |
fitzpatrick | 1:73243284f9d2 | 72 | wait(1); // Allow servos to settel |
fitzpatrick | 1:73243284f9d2 | 73 | |
fitzpatrick | 1:73243284f9d2 | 74 | //Absolute Time Needed to Calculate Delta Time |
fitzpatrick | 1:73243284f9d2 | 75 | ProgramTimer.start(); |
fitzpatrick | 1:73243284f9d2 | 76 | timer = ProgramTimer.read_us(); |
fitzpatrick | 1:73243284f9d2 | 77 | |
fitzpatrick | 1:73243284f9d2 | 78 | Loop_Count = 15; // Allows Kalman filter to sync. |
fitzpatrick | 1:73243284f9d2 | 79 | |
MACRUM | 0:21d86aae6b2a | 80 | while (true) { |
fitzpatrick | 1:73243284f9d2 | 81 | |
fitzpatrick | 1:73243284f9d2 | 82 | //scanf("%d", &val); // Used for debug |
fitzpatrick | 1:73243284f9d2 | 83 | //printf("\r\n Value is %d \r\n", val); // Used for debug |
fitzpatrick | 1:73243284f9d2 | 84 | |
fitzpatrick | 1:73243284f9d2 | 85 | // Acquire Accelerometer values |
fitzpatrick | 1:73243284f9d2 | 86 | accel.acquire_accel_data_g(adata); // Acquire Acceleration Vectors |
fitzpatrick | 1:73243284f9d2 | 87 | printf("\r\n Accelerometer Values\r\n"); |
fitzpatrick | 1:73243284f9d2 | 88 | printf(" X:%6.1f,\t Y:%6.1f,\t Z:%6.1f\r\n\r\n", adata[0],adata[1],adata[2]); |
fitzpatrick | 1:73243284f9d2 | 89 | |
fitzpatrick | 1:73243284f9d2 | 90 | // Acquire Magnomometer Vectors |
fitzpatrick | 1:73243284f9d2 | 91 | mag.acquire_mag_data_uT(mag_data); // Acquire Magnomometer Vectors |
fitzpatrick | 1:73243284f9d2 | 92 | printf("\r\n Magnomometer Values\r\n"); |
fitzpatrick | 1:73243284f9d2 | 93 | 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]); |
fitzpatrick | 1:73243284f9d2 | 94 | |
fitzpatrick | 1:73243284f9d2 | 95 | // Acquire Gyro values |
fitzpatrick | 1:73243284f9d2 | 96 | gyro.acquire_gyro_data_dps(gyro_data); // Acquire Gyro values |
fitzpatrick | 1:73243284f9d2 | 97 | printf("\r\n Gyro Values\r\n"); |
fitzpatrick | 1:73243284f9d2 | 98 | printf(" Pitch: %4.2f,\t Roll: %4.2f,\t Yaw: %4.2f\r\n", gyro_data[0],gyro_data[1],gyro_data[2]); |
fitzpatrick | 1:73243284f9d2 | 99 | |
fitzpatrick | 1:73243284f9d2 | 100 | // RMS Value of Accelerometer |
fitzpatrick | 1:73243284f9d2 | 101 | R = sqrt(std::pow(adata[0], 2) + std::pow(adata[1], 2) + std::pow(adata[2], 2)); |
fitzpatrick | 1:73243284f9d2 | 102 | |
fitzpatrick | 1:73243284f9d2 | 103 | // Kalman Filter |
fitzpatrick | 1:73243284f9d2 | 104 | kalman_predict(&filter_pitch, gyro_data[0], (ProgramTimer.read_us() - timer)); // Predict uses Pitch portion of gyro and delta time |
fitzpatrick | 1:73243284f9d2 | 105 | kalman_update(&filter_pitch, acos(adata[0]/R)); //Update uses normalized Pitch portion of accelerometer |
fitzpatrick | 1:73243284f9d2 | 106 | kalman_predict(&filter_roll, gyro_data[1], (ProgramTimer.read_us() - timer)); // Predict uses Roll portion of gyro and delta time |
fitzpatrick | 1:73243284f9d2 | 107 | kalman_update(&filter_roll, acos(adata[1]/R)); //Update uses normalized Roll portion of accelerometer |
fitzpatrick | 1:73243284f9d2 | 108 | |
fitzpatrick | 1:73243284f9d2 | 109 | angle[0] = kalman_get_angle(&filter_pitch); // Kalman Pitch |
fitzpatrick | 1:73243284f9d2 | 110 | angle[1] = kalman_get_angle(&filter_roll); // Kalman Roll |
fitzpatrick | 1:73243284f9d2 | 111 | |
fitzpatrick | 1:73243284f9d2 | 112 | timer = ProgramTimer.read_us(); // Update Timer |
fitzpatrick | 1:73243284f9d2 | 113 | |
fitzpatrick | 1:73243284f9d2 | 114 | printf("\r\n Roll Angle X: %.6f Pitch Angle Y: %.6f \r\n", Rad2Degree * angle[1], Rad2Degree * angle[0]); |
fitzpatrick | 1:73243284f9d2 | 115 | |
fitzpatrick | 1:73243284f9d2 | 116 | if (Loop_Count > 0) |
fitzpatrick | 1:73243284f9d2 | 117 | { |
fitzpatrick | 1:73243284f9d2 | 118 | // Loop Iterations to Initialize Kalman Filter to sync. |
fitzpatrick | 1:73243284f9d2 | 119 | Loop_Count--; |
fitzpatrick | 1:73243284f9d2 | 120 | printf("\n\rLoop_Count = %d\n\r", Loop_Count); |
fitzpatrick | 1:73243284f9d2 | 121 | } |
fitzpatrick | 1:73243284f9d2 | 122 | else |
fitzpatrick | 1:73243284f9d2 | 123 | { |
fitzpatrick | 1:73243284f9d2 | 124 | // Loop Iterations Complete, Apply Results of Kalman Filter |
fitzpatrick | 1:73243284f9d2 | 125 | // PID Controller (Mostly Proportional) |
fitzpatrick | 1:73243284f9d2 | 126 | Roll_Error = (Rad2Degree * angle[1]) - 90; // Calculate Roll Error |
fitzpatrick | 1:73243284f9d2 | 127 | Roll_D = Roll_Error - Roll_P; // Differentiation of Roll Error |
fitzpatrick | 1:73243284f9d2 | 128 | Roll_I = Roll_I + Roll_Error; // Intigration of Roll Error |
fitzpatrick | 1:73243284f9d2 | 129 | Roll_P = Roll_Error; // Proportional Roll Error |
fitzpatrick | 1:73243284f9d2 | 130 | Roll_Correct = Roll_P * Roll_P_C + Roll_I * Roll_I_C + Roll_D * Roll_D_C; // Sum product of PID coefficients and values |
fitzpatrick | 1:73243284f9d2 | 131 | printf("\n\rRoll_Correct = %f\n\r",Roll_Correct); |
fitzpatrick | 1:73243284f9d2 | 132 | |
fitzpatrick | 1:73243284f9d2 | 133 | Roll_pulse = Roll_pulse + Roll_Correct; // Update Roll Servo PWM Value |
fitzpatrick | 1:73243284f9d2 | 134 | if (Roll_pulse <= ROLL_SERVO_MIN) Roll_pulse = ROLL_SERVO_MIN; |
fitzpatrick | 1:73243284f9d2 | 135 | if (Roll_pulse >= ROLL_SERVO_MAX) Roll_pulse = ROLL_SERVO_MAX; |
fitzpatrick | 1:73243284f9d2 | 136 | Roll_M.pulsewidth_us(Roll_pulse); // servo position determined by a pulsewidth between 1-2ms |
fitzpatrick | 1:73243284f9d2 | 137 | printf("\n\rRoll_pulse = %d\n\r",Roll_pulse); |
fitzpatrick | 1:73243284f9d2 | 138 | |
fitzpatrick | 1:73243284f9d2 | 139 | Pitch_Error = (Rad2Degree * angle[0]) - 90; // Calculate Pitch Error |
fitzpatrick | 1:73243284f9d2 | 140 | Pitch_D = Pitch_Error - Pitch_P; // Differentiation of Pitch Error |
fitzpatrick | 1:73243284f9d2 | 141 | Pitch_I = Pitch_I + Pitch_Error; // Intigration of Pitch Error |
fitzpatrick | 1:73243284f9d2 | 142 | Pitch_P = Pitch_Error; // Proportional Pitch Error |
fitzpatrick | 1:73243284f9d2 | 143 | Pitch_Correct = Pitch_P * Pitch_P_C + Pitch_I * Pitch_I_C + Pitch_D * Pitch_D_C; // Sum product of PID coefficients and values |
fitzpatrick | 1:73243284f9d2 | 144 | printf("\n\rPitch_Correct = %f\n\r",Pitch_Correct); |
fitzpatrick | 1:73243284f9d2 | 145 | |
fitzpatrick | 1:73243284f9d2 | 146 | Pitch_pulse = Pitch_pulse + Pitch_Correct; // Update Roll Servo PWM Value |
fitzpatrick | 1:73243284f9d2 | 147 | if (Pitch_pulse <= PITCH_SERVO_MIN) Pitch_pulse = PITCH_SERVO_MIN; |
fitzpatrick | 1:73243284f9d2 | 148 | if (Pitch_pulse >= PITCH_SERVO_MAX) Pitch_pulse = PITCH_SERVO_MAX; |
fitzpatrick | 1:73243284f9d2 | 149 | Pitch_M.pulsewidth_us(Pitch_pulse); // servo position determined by a pulsewidth between 1-2ms |
fitzpatrick | 1:73243284f9d2 | 150 | printf("\n\rPitch_pulse = %d\n\r",Pitch_pulse); |
fitzpatrick | 1:73243284f9d2 | 151 | } |
fitzpatrick | 1:73243284f9d2 | 152 | |
fitzpatrick | 1:73243284f9d2 | 153 | wait(0.01); // Loop timer |
fitzpatrick | 1:73243284f9d2 | 154 | } |
fitzpatrick | 1:73243284f9d2 | 155 | |
MACRUM | 0:21d86aae6b2a | 156 | } |
MACRUM | 0:21d86aae6b2a | 157 | |
fitzpatrick | 1:73243284f9d2 | 158 |