Julio Fajardo / Mbed 2 deprecated Galileo_HoverBoardRider

Roll & Pitch Angles (Kalman Filter)

Dependencies:   L3GD20H LSM303DLHC kalman mbed-dsp mbed-rtos mbed

main.cpp@2:f3043132a959, 2016-04-09 (annotated)

Committer:
julioefajardo
Date:
Sat Apr 09 19:17:26 2016 +0000
Revision:
2:f3043132a959
Parent:
0:7fd305c81a8e
Child:
3:874424bbe577
CMSIS-DSP PID Implementation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
julioefajardo 0:7fd305c81a8e 1 #include "mbed.h"
julioefajardo 0:7fd305c81a8e 2 #include "rtos.h"
julioefajardo 0:7fd305c81a8e 3 #include "arm_math.h"
julioefajardo 0:7fd305c81a8e 4 #include "kalman.c"
julioefajardo 0:7fd305c81a8e 5 #include "LSM303DLHC.h"
julioefajardo 0:7fd305c81a8e 6 #include "L3GD20H.h"
julioefajardo 0:7fd305c81a8e 7
julioefajardo 0:7fd305c81a8e 8 #define Rad2Dree 57.295779513082320876798154814105f
julioefajardo 0:7fd305c81a8e 9
julioefajardo 2:f3043132a959 10 #define PID_L_KP 0.0275f /* Proporcional */ //0.015f
julioefajardo 2:f3043132a959 11 #define PID_L_KI 0.000125f /* Integral */
julioefajardo 2:f3043132a959 12 #define PID_L_KD 0.075f /* Derivative */
julioefajardo 2:f3043132a959 13
julioefajardo 2:f3043132a959 14 #define PID_R_KP 0.0275f /* Proporcional */ //0.015f
julioefajardo 2:f3043132a959 15 #define PID_R_KI 0.000125f /* Integral */
julioefajardo 2:f3043132a959 16 #define PID_R_KD 0.075f /* Derivative */
julioefajardo 2:f3043132a959 17
julioefajardo 2:f3043132a959 18 #define L_SP PI/2
julioefajardo 2:f3043132a959 19 #define R_SP PI/2
julioefajardo 2:f3043132a959 20
julioefajardo 0:7fd305c81a8e 21 DigitalOut led_red(LED_RED);
julioefajardo 0:7fd305c81a8e 22 DigitalOut led_green(LED_GREEN);
julioefajardo 0:7fd305c81a8e 23 DigitalIn sw2(SW2);
julioefajardo 0:7fd305c81a8e 24 DigitalIn sw3(SW3);
julioefajardo 0:7fd305c81a8e 25 Serial pc(USBTX, USBRX);
julioefajardo 0:7fd305c81a8e 26
julioefajardo 2:f3043132a959 27 LSM303DLHC imuL(D14,D15);
julioefajardo 2:f3043132a959 28 L3GD20H gyroL(D14,D15);
julioefajardo 0:7fd305c81a8e 29
julioefajardo 2:f3043132a959 30 LSM303DLHC imuR(PTC11,PTC10);
julioefajardo 2:f3043132a959 31 L3GD20H gyroR(PTC11,PTC10);
julioefajardo 0:7fd305c81a8e 32
julioefajardo 0:7fd305c81a8e 33 Timer GlobalTime;
julioefajardo 0:7fd305c81a8e 34 Timer ProgramTimer;
julioefajardo 0:7fd305c81a8e 35
julioefajardo 2:f3043132a959 36 kalman filter_pitch_L;
julioefajardo 2:f3043132a959 37 kalman filter_roll_L;
julioefajardo 0:7fd305c81a8e 38
julioefajardo 2:f3043132a959 39 kalman filter_pitch_R;
julioefajardo 2:f3043132a959 40 kalman filter_roll_R;
julioefajardo 0:7fd305c81a8e 41
julioefajardo 2:f3043132a959 42 int accL[3];
julioefajardo 2:f3043132a959 43 int magL[3];
julioefajardo 2:f3043132a959 44 short gyrL[3];
julioefajardo 0:7fd305c81a8e 45
julioefajardo 2:f3043132a959 46 int accR[3];
julioefajardo 2:f3043132a959 47 int magR[3];
julioefajardo 2:f3043132a959 48 short gyrR[3];
julioefajardo 0:7fd305c81a8e 49
julioefajardo 0:7fd305c81a8e 50 struct vector {
julioefajardo 0:7fd305c81a8e 51 float x;
julioefajardo 0:7fd305c81a8e 52 float y;
julioefajardo 0:7fd305c81a8e 53 float z;
julioefajardo 2:f3043132a959 54 } AccL, GyrL, AccR, GyrR;
julioefajardo 0:7fd305c81a8e 55
julioefajardo 2:f3043132a959 56 float RL, RR;
julioefajardo 2:f3043132a959 57 double angleL[3];
julioefajardo 2:f3043132a959 58 double angleR[3];
julioefajardo 2:f3043132a959 59
julioefajardo 2:f3043132a959 60 float L_error;
julioefajardo 2:f3043132a959 61 float R_error;
julioefajardo 2:f3043132a959 62
julioefajardo 2:f3043132a959 63 float L;
julioefajardo 0:7fd305c81a8e 64 float R;
julioefajardo 0:7fd305c81a8e 65
julioefajardo 0:7fd305c81a8e 66 unsigned long timer;
julioefajardo 0:7fd305c81a8e 67 long loopStartTime;
julioefajardo 0:7fd305c81a8e 68
julioefajardo 2:f3043132a959 69 void AccRaw2GL(int acc[3]);
julioefajardo 2:f3043132a959 70 void GyrRaw2DL(short gyr[3]);
julioefajardo 2:f3043132a959 71 void AccRaw2GR(int acc[3]);
julioefajardo 2:f3043132a959 72 void GyrRaw2DR(short gyr[3]);
julioefajardo 0:7fd305c81a8e 73
julioefajardo 0:7fd305c81a8e 74 int main() {
julioefajardo 2:f3043132a959 75 arm_pid_instance_f32 L_PID;
julioefajardo 2:f3043132a959 76 arm_pid_instance_f32 R_PID;
julioefajardo 2:f3043132a959 77
julioefajardo 2:f3043132a959 78 //Left
julioefajardo 2:f3043132a959 79 L_PID.Kp = PID_L_KP; /* Proporcional */
julioefajardo 2:f3043132a959 80 L_PID.Ki = PID_L_KI; /* Integral */
julioefajardo 2:f3043132a959 81 L_PID.Kd = PID_L_KD; /* Derivative */
julioefajardo 2:f3043132a959 82
julioefajardo 2:f3043132a959 83 //Right
julioefajardo 2:f3043132a959 84 R_PID.Kp = PID_R_KP; /* Proporcional */
julioefajardo 2:f3043132a959 85 R_PID.Ki = PID_R_KI; /* Integral */
julioefajardo 2:f3043132a959 86 R_PID.Kd = PID_R_KD; /* Derivative */
julioefajardo 2:f3043132a959 87
julioefajardo 2:f3043132a959 88 arm_pid_init_f32(&L_PID, 1);
julioefajardo 2:f3043132a959 89 arm_pid_init_f32(&R_PID, 1);
julioefajardo 2:f3043132a959 90
julioefajardo 0:7fd305c81a8e 91 GlobalTime.start();
julioefajardo 2:f3043132a959 92 imuL.init();
julioefajardo 2:f3043132a959 93 imuR.init();
julioefajardo 0:7fd305c81a8e 94 led_green = 1;
julioefajardo 0:7fd305c81a8e 95 led_red = 1;
julioefajardo 0:7fd305c81a8e 96 pc.baud(115200);
julioefajardo 0:7fd305c81a8e 97 pc.printf("Hello World from FRDM-K64F board.\r\n");
julioefajardo 2:f3043132a959 98 kalman_init(&filter_pitch_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 2:f3043132a959 99 kalman_init(&filter_roll_L, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 2:f3043132a959 100 kalman_init(&filter_pitch_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 2:f3043132a959 101 kalman_init(&filter_roll_R, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
julioefajardo 0:7fd305c81a8e 102
julioefajardo 0:7fd305c81a8e 103 ProgramTimer.start();
julioefajardo 0:7fd305c81a8e 104 loopStartTime = ProgramTimer.read_us();
julioefajardo 0:7fd305c81a8e 105 timer = loopStartTime;
julioefajardo 0:7fd305c81a8e 106
julioefajardo 0:7fd305c81a8e 107 while (true) {
julioefajardo 2:f3043132a959 108 imuL.readAcc(accL);
julioefajardo 2:f3043132a959 109 AccRaw2GL(accL);
julioefajardo 2:f3043132a959 110 gyroL.read(gyrL);
julioefajardo 2:f3043132a959 111 GyrRaw2DL(gyrL);
julioefajardo 0:7fd305c81a8e 112
julioefajardo 2:f3043132a959 113 imuR.readAcc(accR);
julioefajardo 2:f3043132a959 114 AccRaw2GR(accR);
julioefajardo 2:f3043132a959 115 gyroR.read(gyrR);
julioefajardo 2:f3043132a959 116 GyrRaw2DR(gyrR);
julioefajardo 0:7fd305c81a8e 117
julioefajardo 2:f3043132a959 118 RL = sqrt(std::pow(AccL.x, 2) + std::pow(AccL.y, 2) + std::pow(AccL.z, 2));
julioefajardo 2:f3043132a959 119 RR = sqrt(std::pow(AccR.x, 2) + std::pow(AccR.y, 2) + std::pow(AccR.z, 2));
julioefajardo 0:7fd305c81a8e 120
julioefajardo 2:f3043132a959 121 kalman_predict(&filter_pitch_L, GyrL.x, (ProgramTimer.read_us() - timer));
julioefajardo 2:f3043132a959 122 kalman_update(&filter_pitch_L, acos(AccL.x/RL));
julioefajardo 2:f3043132a959 123 kalman_predict(&filter_roll_L, GyrL.y, (ProgramTimer.read_us() - timer));
julioefajardo 2:f3043132a959 124 kalman_update(&filter_roll_L, acos(AccL.y/RL));
julioefajardo 2:f3043132a959 125
julioefajardo 2:f3043132a959 126 kalman_predict(&filter_pitch_R, GyrR.x, (ProgramTimer.read_us() - timer));
julioefajardo 2:f3043132a959 127 kalman_update(&filter_pitch_R, acos(AccR.x/RR));
julioefajardo 2:f3043132a959 128 kalman_predict(&filter_roll_R, GyrR.y, (ProgramTimer.read_us() - timer));
julioefajardo 2:f3043132a959 129 kalman_update(&filter_roll_R, acos(AccR.y/RR));
julioefajardo 0:7fd305c81a8e 130
julioefajardo 2:f3043132a959 131 angleL[0] = kalman_get_angle(&filter_pitch_L);
julioefajardo 2:f3043132a959 132 angleL[1] = kalman_get_angle(&filter_roll_L);
julioefajardo 2:f3043132a959 133
julioefajardo 2:f3043132a959 134 angleR[0] = kalman_get_angle(&filter_pitch_R);
julioefajardo 2:f3043132a959 135 angleR[1] = kalman_get_angle(&filter_roll_R);
julioefajardo 0:7fd305c81a8e 136
julioefajardo 2:f3043132a959 137 L_error = angleL[0] - L_SP;
julioefajardo 2:f3043132a959 138 R_error = angleR[0] - R_SP;
julioefajardo 0:7fd305c81a8e 139
julioefajardo 2:f3043132a959 140 L = arm_pid_f32(&L_PID, L_error);
julioefajardo 2:f3043132a959 141 R = arm_pid_f32(&R_PID, R_error);
julioefajardo 0:7fd305c81a8e 142
julioefajardo 0:7fd305c81a8e 143 timer = ProgramTimer.read_us();
julioefajardo 0:7fd305c81a8e 144
julioefajardo 2:f3043132a959 145 printf("IMUL\tPitch=%6.2f\tRoll=%6.2f\r\n",Rad2Dree*angleL[0],Rad2Dree*angleL[1]);
julioefajardo 2:f3043132a959 146 printf("IMIR\tPitch=%6.2f\tRoll=%6.2f\r\n",Rad2Dree*angleR[0],Rad2Dree*angleR[1]);
julioefajardo 0:7fd305c81a8e 147 wait(0.2);
julioefajardo 0:7fd305c81a8e 148 }
julioefajardo 0:7fd305c81a8e 149 }
julioefajardo 0:7fd305c81a8e 150
julioefajardo 2:f3043132a959 151 void AccRaw2GL(int acc[3]){
julioefajardo 2:f3043132a959 152 AccL.x = acc[0]/1024.0f;
julioefajardo 2:f3043132a959 153 AccL.y = acc[1]/1024.0f;
julioefajardo 2:f3043132a959 154 AccL.z = acc[2]/1024.0f;
julioefajardo 0:7fd305c81a8e 155 }
julioefajardo 0:7fd305c81a8e 156
julioefajardo 2:f3043132a959 157 void GyrRaw2DL(short gyr[3]){
julioefajardo 2:f3043132a959 158 GyrL.x = gyr[0]/225.0f;
julioefajardo 2:f3043132a959 159 GyrL.y = gyr[1]/225.0f;
julioefajardo 2:f3043132a959 160 GyrL.z = gyr[2]/225.0f;
julioefajardo 0:7fd305c81a8e 161 }
julioefajardo 0:7fd305c81a8e 162
julioefajardo 2:f3043132a959 163 void AccRaw2GR(int acc[3]){
julioefajardo 2:f3043132a959 164 AccR.x = acc[0]/1024.0f;
julioefajardo 2:f3043132a959 165 AccR.y = acc[1]/1024.0f;
julioefajardo 2:f3043132a959 166 AccR.z = acc[2]/1024.0f;
julioefajardo 0:7fd305c81a8e 167 }
julioefajardo 0:7fd305c81a8e 168
julioefajardo 2:f3043132a959 169 void GyrRaw2DR(short gyr[3]){
julioefajardo 2:f3043132a959 170 GyrR.x = gyr[0]/225.0f;
julioefajardo 2:f3043132a959 171 GyrR.y = gyr[1]/225.0f;
julioefajardo 2:f3043132a959 172 GyrR.z = gyr[2]/225.0f;
julioefajardo 0:7fd305c81a8e 173 }