John Fitzpatrick / Mbed OS Coffee-Cup-Stabilizer-01

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?

UserRevisionLine numberNew 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