Khoa Bui Anh / Mbed 2 deprecated drum_axis

drum_axis

Dependencies:   invisdrum kalman mbed

main.cpp@1:4a15cf53ca28, 2016-10-26 (annotated)

Committer:
fxanhkhoa
Date:
Wed Oct 26 11:09:20 2016 +0000
Revision:
1:4a15cf53ca28
Parent:
0:d75dd19f5e59 
ok

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fxanhkhoa 0:d75dd19f5e59 1 #include "mbed.h"
fxanhkhoa 0:d75dd19f5e59 2 #include "kalman.c"
fxanhkhoa 0:d75dd19f5e59 3 #include "MPU6050.h"
fxanhkhoa 0:d75dd19f5e59 4 #include <math.h>
fxanhkhoa 0:d75dd19f5e59 5
fxanhkhoa 0:d75dd19f5e59 6 #define PI 3.1415926535897932384626433832795
fxanhkhoa 0:d75dd19f5e59 7 #define Rad2Dree 57.295779513082320876798154814105
fxanhkhoa 0:d75dd19f5e59 8
fxanhkhoa 0:d75dd19f5e59 9 #define dir 20
fxanhkhoa 1:4a15cf53ca28 10 #define ratio 10
fxanhkhoa 0:d75dd19f5e59 11
fxanhkhoa 0:d75dd19f5e59 12 Serial pc(USBTX, USBRX);
fxanhkhoa 1:4a15cf53ca28 13
fxanhkhoa 1:4a15cf53ca28 14 void get();
fxanhkhoa 1:4a15cf53ca28 15 void radius(float, float, float, int);
fxanhkhoa 1:4a15cf53ca28 16
fxanhkhoa 0:d75dd19f5e59 17 Timer GlobalTime;
fxanhkhoa 0:d75dd19f5e59 18 Timer ProgramTimer;
fxanhkhoa 0:d75dd19f5e59 19 InterruptIn mybutton(USER_BUTTON);
fxanhkhoa 1:4a15cf53ca28 20
fxanhkhoa 0:d75dd19f5e59 21 float R;
fxanhkhoa 0:d75dd19f5e59 22 double angle[3];
fxanhkhoa 0:d75dd19f5e59 23 unsigned long timer;
fxanhkhoa 0:d75dd19f5e59 24 long loopStartTime;
fxanhkhoa 0:d75dd19f5e59 25
fxanhkhoa 0:d75dd19f5e59 26 MPU6050 ark(PB_9,PB_8);
fxanhkhoa 1:4a15cf53ca28 27 kalman filter_pitch;
fxanhkhoa 0:d75dd19f5e59 28 kalman filter_roll;
fxanhkhoa 0:d75dd19f5e59 29 kalman filter_yaw;
fxanhkhoa 0:d75dd19f5e59 30
fxanhkhoa 0:d75dd19f5e59 31 volatile float central[3] = {0,0,0},drum[3] = {0,0,0}, delta[3] = {0,0,0};
fxanhkhoa 0:d75dd19f5e59 32 volatile float delta_1_x[2] ;
fxanhkhoa 1:4a15cf53ca28 33 volatile float delta_1_y[2], delta_2_y[2] = {17,18}, delta_3_y[2] = {16,17}, delta_4_y[2] = {16,17}, delta_5_y[2] = {23,24};
fxanhkhoa 0:d75dd19f5e59 34 volatile float delta_1_z[2];
fxanhkhoa 0:d75dd19f5e59 35 volatile int drum_stt = 0, drum_1_stt = 0, drum_2_stt = 0, drum_3_stt = 0, drum_4_stt = 0, drum_5_stt = 0;
fxanhkhoa 0:d75dd19f5e59 36
fxanhkhoa 0:d75dd19f5e59 37 int main()
fxanhkhoa 0:d75dd19f5e59 38 {
fxanhkhoa 0:d75dd19f5e59 39 mybutton.fall(get);
fxanhkhoa 0:d75dd19f5e59 40 //int count = 0;
fxanhkhoa 0:d75dd19f5e59 41 GlobalTime.start();
fxanhkhoa 1:4a15cf53ca28 42
fxanhkhoa 0:d75dd19f5e59 43 float Acc[3]= {0, 0, 0};
fxanhkhoa 0:d75dd19f5e59 44 ark.getAccelero(Acc);
fxanhkhoa 1:4a15cf53ca28 45
fxanhkhoa 0:d75dd19f5e59 46 float Gyro[3]= {0,0,0};
fxanhkhoa 0:d75dd19f5e59 47 ark.getGyro(Gyro);
fxanhkhoa 1:4a15cf53ca28 48
fxanhkhoa 1:4a15cf53ca28 49
fxanhkhoa 1:4a15cf53ca28 50 // Parameters ( R_angle, Q_gyro, Q_angle )
fxanhkhoa 1:4a15cf53ca28 51 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
fxanhkhoa 1:4a15cf53ca28 52 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
fxanhkhoa 0:d75dd19f5e59 53 kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
fxanhkhoa 1:4a15cf53ca28 54
fxanhkhoa 0:d75dd19f5e59 55 ProgramTimer.start();
fxanhkhoa 0:d75dd19f5e59 56 loopStartTime = ProgramTimer.read_us();
fxanhkhoa 0:d75dd19f5e59 57 timer = loopStartTime;
fxanhkhoa 0:d75dd19f5e59 58 int i;
fxanhkhoa 1:4a15cf53ca28 59 for (i = 0; i< 1000; i++) {
fxanhkhoa 0:d75dd19f5e59 60 //Aquire new values for the Gyro and Acc
fxanhkhoa 0:d75dd19f5e59 61 ark.getAccelero(Acc);
fxanhkhoa 0:d75dd19f5e59 62 ark.getGyro(Gyro);
fxanhkhoa 1:4a15cf53ca28 63
fxanhkhoa 0:d75dd19f5e59 64 //Calcuate the resulting vector R from the 3 acc axes
fxanhkhoa 0:d75dd19f5e59 65 R = sqrt(std::pow(Acc[0] , 2) + std::pow(Acc[1] , 2) + std::pow(Acc[2] , 2));
fxanhkhoa 1:4a15cf53ca28 66
fxanhkhoa 1:4a15cf53ca28 67 kalman_predict(&filter_pitch, Gyro[0], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:d75dd19f5e59 68 kalman_update(&filter_pitch, acos(Acc[0]/R));
fxanhkhoa 1:4a15cf53ca28 69 kalman_predict(&filter_roll, Gyro[1], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:d75dd19f5e59 70 kalman_update(&filter_roll, acos(Acc[1]/R));
fxanhkhoa 0:d75dd19f5e59 71 kalman_predict(&filter_yaw, Gyro[2], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:d75dd19f5e59 72 kalman_update(&filter_yaw, acos(Acc[2]/R));
fxanhkhoa 1:4a15cf53ca28 73
fxanhkhoa 0:d75dd19f5e59 74 angle[0] = kalman_get_angle(&filter_pitch);
fxanhkhoa 0:d75dd19f5e59 75 angle[1] = kalman_get_angle(&filter_roll);
fxanhkhoa 0:d75dd19f5e59 76 angle[2] = kalman_get_angle(&filter_yaw);
fxanhkhoa 1:4a15cf53ca28 77
fxanhkhoa 0:d75dd19f5e59 78 timer = ProgramTimer.read_us();
fxanhkhoa 1:4a15cf53ca28 79
fxanhkhoa 0:d75dd19f5e59 80 //count++;
fxanhkhoa 0:d75dd19f5e59 81 //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
fxanhkhoa 0:d75dd19f5e59 82 //pc.printf("Pitch Angle X: %.6f Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]);
fxanhkhoa 0:d75dd19f5e59 83 central[0] = Rad2Dree * angle[1];
fxanhkhoa 0:d75dd19f5e59 84 central[1] = Rad2Dree * angle[0];
fxanhkhoa 1:4a15cf53ca28 85 central[2] = Rad2Dree * angle[2];
fxanhkhoa 1:4a15cf53ca28 86 }
fxanhkhoa 1:4a15cf53ca28 87 pc.printf("central %f %f\r\n",central[0],central[1]);
fxanhkhoa 0:d75dd19f5e59 88 while(1) {
fxanhkhoa 0:d75dd19f5e59 89 //Aquire new values for the Gyro and Acc
fxanhkhoa 0:d75dd19f5e59 90 ark.getAccelero(Acc);
fxanhkhoa 0:d75dd19f5e59 91 ark.getGyro(Gyro);
fxanhkhoa 1:4a15cf53ca28 92
fxanhkhoa 0:d75dd19f5e59 93 //Calcuate the resulting vector R from the 3 acc axes
fxanhkhoa 0:d75dd19f5e59 94 R = sqrt(std::pow(Acc[0] , 2) + std::pow(Acc[1] , 2) + std::pow(Acc[2] , 2));
fxanhkhoa 1:4a15cf53ca28 95
fxanhkhoa 1:4a15cf53ca28 96 kalman_predict(&filter_pitch, Gyro[0], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:d75dd19f5e59 97 kalman_update(&filter_pitch, acos(Acc[0]/R));
fxanhkhoa 1:4a15cf53ca28 98 kalman_predict(&filter_roll, Gyro[1], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:d75dd19f5e59 99 kalman_update(&filter_roll, acos(Acc[1]/R));
fxanhkhoa 0:d75dd19f5e59 100 kalman_predict(&filter_yaw, Gyro[2], (ProgramTimer.read_us() - timer));
fxanhkhoa 0:d75dd19f5e59 101 kalman_update(&filter_yaw, acos(Acc[2]/R));
fxanhkhoa 1:4a15cf53ca28 102
fxanhkhoa 0:d75dd19f5e59 103 angle[0] = kalman_get_angle(&filter_pitch);
fxanhkhoa 0:d75dd19f5e59 104 angle[1] = kalman_get_angle(&filter_roll);
fxanhkhoa 0:d75dd19f5e59 105 angle[2] = kalman_get_angle(&filter_yaw);
fxanhkhoa 1:4a15cf53ca28 106
fxanhkhoa 0:d75dd19f5e59 107 timer = ProgramTimer.read_us();
fxanhkhoa 0:d75dd19f5e59 108 angle[2] = (sqrt(Acc[0]*Acc[0] + (Acc[1] * Acc[1])) / Acc[2]);
fxanhkhoa 0:d75dd19f5e59 109 //count++;
fxanhkhoa 0:d75dd19f5e59 110 //if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
fxanhkhoa 1:4a15cf53ca28 111 //pc.printf("Pitch Angle X: %.6f Pitch Angle Y: %.6f\r\n Z: %.6f \n\r",Rad2Dree * angle[1],Rad2Dree * angle[0],Rad2Dree * angle[2]);
fxanhkhoa 0:d75dd19f5e59 112 delta[0] = Rad2Dree * angle[1];
fxanhkhoa 0:d75dd19f5e59 113 delta[1] = Rad2Dree * angle[0];
fxanhkhoa 0:d75dd19f5e59 114 delta[2] = Rad2Dree * angle[2];
fxanhkhoa 0:d75dd19f5e59 115 //float x,y,z;
fxanhkhoa 0:d75dd19f5e59 116 //y = cos(Rad2Dree * angle[1]) * dir;
fxanhkhoa 0:d75dd19f5e59 117 //x = cos(90-(Rad2Dree * angle[1])) * dir;
fxanhkhoa 0:d75dd19f5e59 118 //z = cos(Rad2Dree * angle[0]) * dir;
fxanhkhoa 0:d75dd19f5e59 119 //pc.printf("x: %f\n , y: %f\n , z: %f\n",x,y,z);
fxanhkhoa 0:d75dd19f5e59 120 //wait_ms(100);
fxanhkhoa 1:4a15cf53ca28 121 switch (drum_stt) {
fxanhkhoa 0:d75dd19f5e59 122 case 0:
fxanhkhoa 1:4a15cf53ca28 123 if ((delta[0] > delta_1_x[0]) && (delta[0] < delta_1_x[1]) && (delta[1] > delta_1_y[0]) && (delta[1] < delta_1_y[1])&& (delta[2] > delta_1_z[0]) && (delta[2] < delta_1_z[1])) drum_stt = 1;
fxanhkhoa 1:4a15cf53ca28 124 //if ((delta[0] >= 20)) drum_stt = 4;
fxanhkhoa 0:d75dd19f5e59 125 break;
fxanhkhoa 0:d75dd19f5e59 126 case 1:
fxanhkhoa 1:4a15cf53ca28 127 if ((delta[0] > delta_1_x[0]) && (delta[0] < delta_1_x[1]) && (delta[1] > delta_1_y[0]) && (delta[1] < delta_1_y[1])&& (delta[2] > delta_1_z[0]) && (delta[2] < delta_1_z[1]) && (drum_1_stt == 0)) {
fxanhkhoa 1:4a15cf53ca28 128 pc.printf("drum 1_1");
fxanhkhoa 1:4a15cf53ca28 129 drum_1_stt = 1;
fxanhkhoa 1:4a15cf53ca28 130 } else if ((delta[0] > delta_1_x[1])) {
fxanhkhoa 1:4a15cf53ca28 131 drum_stt = 0 ;
fxanhkhoa 1:4a15cf53ca28 132 drum_1_stt = 0;
fxanhkhoa 1:4a15cf53ca28 133 }
fxanhkhoa 0:d75dd19f5e59 134 break;
fxanhkhoa 0:d75dd19f5e59 135 case 2:
fxanhkhoa 1:4a15cf53ca28 136 if ((delta[0] > -30) && (delta[0] < -25) && (delta[1] < 18) && (delta[1] > 17) && (drum_2_stt == 0)) {
fxanhkhoa 1:4a15cf53ca28 137 pc.printf("drum 2_2");
fxanhkhoa 1:4a15cf53ca28 138 drum_2_stt = 1;
fxanhkhoa 1:4a15cf53ca28 139 } else if ((delta[0] > -25)) {
fxanhkhoa 1:4a15cf53ca28 140 drum_stt = 0;
fxanhkhoa 1:4a15cf53ca28 141 drum_2_stt = 0;
fxanhkhoa 1:4a15cf53ca28 142 } else if ((delta[0] <= -30)) drum_stt = 1;
fxanhkhoa 1:4a15cf53ca28 143 break;
fxanhkhoa 0:d75dd19f5e59 144 case 3:
fxanhkhoa 1:4a15cf53ca28 145 if ((delta[0] > -25) && (delta[0] < -20) && (delta[1] < 17) && (delta[1] > 16) && (drum_3_stt == 0)) {
fxanhkhoa 1:4a15cf53ca28 146 pc.printf("drum 3_3");
fxanhkhoa 1:4a15cf53ca28 147 drum_3_stt = 1;
fxanhkhoa 1:4a15cf53ca28 148 } else if ((delta[0] > -20)) {
fxanhkhoa 1:4a15cf53ca28 149 drum_stt = 0;
fxanhkhoa 1:4a15cf53ca28 150 drum_3_stt = 0;
fxanhkhoa 1:4a15cf53ca28 151 } else if ((delta[0] <= -25)) drum_stt = 2;
fxanhkhoa 0:d75dd19f5e59 152 break;
fxanhkhoa 0:d75dd19f5e59 153 case 4:
fxanhkhoa 1:4a15cf53ca28 154 if ((delta[0] > 20) && (delta[0] < 25) && (delta[1] > 16) && (delta[1] < 17) && (drum_4_stt == 0)) {
fxanhkhoa 1:4a15cf53ca28 155 pc.printf("drum 4_4");
fxanhkhoa 1:4a15cf53ca28 156 drum_4_stt = 1;
fxanhkhoa 1:4a15cf53ca28 157 } else if ((delta[0] < 20)) {
fxanhkhoa 1:4a15cf53ca28 158 drum_stt = 0;
fxanhkhoa 1:4a15cf53ca28 159 drum_4_stt = 0;
fxanhkhoa 1:4a15cf53ca28 160 } else if ((delta[0] >=25)) drum_stt = 5;
fxanhkhoa 0:d75dd19f5e59 161 break;
fxanhkhoa 0:d75dd19f5e59 162 case 5:
fxanhkhoa 1:4a15cf53ca28 163 if ((delta[0] >= 25) && (delta[1] > 23) && (delta[1] < 24) && (drum_5_stt == 0)) {
fxanhkhoa 1:4a15cf53ca28 164 pc.printf("drum 5_5");
fxanhkhoa 1:4a15cf53ca28 165 drum_5_stt = 1;
fxanhkhoa 1:4a15cf53ca28 166 } else if (delta[0] < 25) {
fxanhkhoa 1:4a15cf53ca28 167 drum_stt = 0;
fxanhkhoa 1:4a15cf53ca28 168 drum_5_stt = 0;
fxanhkhoa 1:4a15cf53ca28 169 }
fxanhkhoa 0:d75dd19f5e59 170 break;
fxanhkhoa 0:d75dd19f5e59 171 }
fxanhkhoa 1:4a15cf53ca28 172
fxanhkhoa 0:d75dd19f5e59 173 }
fxanhkhoa 0:d75dd19f5e59 174 }
fxanhkhoa 0:d75dd19f5e59 175
fxanhkhoa 0:d75dd19f5e59 176 void get()
fxanhkhoa 0:d75dd19f5e59 177 {
fxanhkhoa 0:d75dd19f5e59 178 drum[0] = Rad2Dree * angle[1];
fxanhkhoa 0:d75dd19f5e59 179 drum[1] = Rad2Dree * angle[0];
fxanhkhoa 0:d75dd19f5e59 180 drum[2] = Rad2Dree * angle[2];
fxanhkhoa 1:4a15cf53ca28 181 pc.printf("drum %f %f %f\r\n",drum[0],drum[1],drum[2]);
fxanhkhoa 0:d75dd19f5e59 182 pc.printf("deltaX: %f , deltaY: %f\r\n",delta[0],delta[1]);
fxanhkhoa 0:d75dd19f5e59 183 radius(drum[0],drum[1],drum[2],1);
fxanhkhoa 0:d75dd19f5e59 184 }
fxanhkhoa 0:d75dd19f5e59 185
fxanhkhoa 0:d75dd19f5e59 186 void radius(float x,float y,float z, int ind)
fxanhkhoa 0:d75dd19f5e59 187 {
fxanhkhoa 0:d75dd19f5e59 188 delta_1_x[0] = x - ratio;
fxanhkhoa 0:d75dd19f5e59 189 delta_1_x[1] = x + ratio;
fxanhkhoa 0:d75dd19f5e59 190 delta_1_y[0] = y - ratio;
fxanhkhoa 0:d75dd19f5e59 191 delta_1_y[1] = y + ratio;
fxanhkhoa 0:d75dd19f5e59 192 delta_1_z[0] = z - ratio;
fxanhkhoa 0:d75dd19f5e59 193 delta_1_z[1] = z + ratio;
fxanhkhoa 0:d75dd19f5e59 194 }