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

IUPUI

Dependencies:   FXOS8700 FXAS21002 kalman MotionSensor

main.cpp@1:615afe04158d, 2021-03-15 (annotated)

Committer:
fitzpatrick
Date:
Mon Mar 15 19:36:20 2021 +0000
Revision:
1:615afe04158d
Parent:
0:21d86aae6b2a 
IUPUI Project 1

Who changed what in which revision?

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