drum_axis
Dependencies: invisdrum kalman mbed
Revision 1:4a15cf53ca28, committed 2016-10-26
- Comitter:
- fxanhkhoa
- Date:
- Wed Oct 26 11:09:20 2016 +0000
- Parent:
- 0:d75dd19f5e59
- Commit message:
- ok
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r d75dd19f5e59 -r 4a15cf53ca28 main.cpp
--- a/main.cpp Wed Oct 26 10:41:37 2016 +0000
+++ b/main.cpp Wed Oct 26 11:09:20 2016 +0000
@@ -7,30 +7,30 @@
#define Rad2Dree 57.295779513082320876798154814105
#define dir 20
-#define ratio 2
+#define ratio 10
Serial pc(USBTX, USBRX);
-
-void get();
-void rafius(int, int, int, int);
-
+
+void get();
+void radius(float, float, float, int);
+
Timer GlobalTime;
Timer ProgramTimer;
InterruptIn mybutton(USER_BUTTON);
-
+
float R;
double angle[3];
unsigned long timer;
long loopStartTime;
MPU6050 ark(PB_9,PB_8);
-kalman filter_pitch;
+kalman filter_pitch;
kalman filter_roll;
kalman filter_yaw;
volatile float central[3] = {0,0,0},drum[3] = {0,0,0}, delta[3] = {0,0,0};
volatile float delta_1_x[2] ;
-volatile float delta_1_y[2] = {23,24}, delta_2_y[2] = {17,18}, delta_3_y[2] = {16,17}, delta_4_y[2] = {16,17}, delta_5_y[2] = {23,24};
+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};
volatile float delta_1_z[2];
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;
@@ -39,76 +39,76 @@
mybutton.fall(get);
//int count = 0;
GlobalTime.start();
-
+
float Acc[3]= {0, 0, 0};
ark.getAccelero(Acc);
-
+
float Gyro[3]= {0,0,0};
ark.getGyro(Gyro);
-
-
- // Parameters ( R_angle, Q_gyro, Q_angle )
- kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
- kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+
+
+ // Parameters ( R_angle, Q_gyro, Q_angle )
+ kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+ kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
-
+
ProgramTimer.start();
loopStartTime = ProgramTimer.read_us();
timer = loopStartTime;
int i;
- for (i = 0; i< 1000; i++)
- {
+ for (i = 0; i< 1000; i++) {
//Aquire new values for the Gyro and Acc
ark.getAccelero(Acc);
ark.getGyro(Gyro);
-
+
//Calcuate the resulting vector R from the 3 acc axes
R = sqrt(std::pow(Acc[0] , 2) + std::pow(Acc[1] , 2) + std::pow(Acc[2] , 2));
-
- kalman_predict(&filter_pitch, Gyro[0], (ProgramTimer.read_us() - timer));
+
+ kalman_predict(&filter_pitch, Gyro[0], (ProgramTimer.read_us() - timer));
kalman_update(&filter_pitch, acos(Acc[0]/R));
- kalman_predict(&filter_roll, Gyro[1], (ProgramTimer.read_us() - timer));
+ kalman_predict(&filter_roll, Gyro[1], (ProgramTimer.read_us() - timer));
kalman_update(&filter_roll, acos(Acc[1]/R));
kalman_predict(&filter_yaw, Gyro[2], (ProgramTimer.read_us() - timer));
kalman_update(&filter_yaw, acos(Acc[2]/R));
-
+
angle[0] = kalman_get_angle(&filter_pitch);
angle[1] = kalman_get_angle(&filter_roll);
angle[2] = kalman_get_angle(&filter_yaw);
-
+
timer = ProgramTimer.read_us();
-
+
//count++;
//if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
//pc.printf("Pitch Angle X: %.6f Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]);
central[0] = Rad2Dree * angle[1];
central[1] = Rad2Dree * angle[0];
- }
- pc.printf("central %f %f\r\n",central[0],central[1]);
+ central[2] = Rad2Dree * angle[2];
+ }
+ pc.printf("central %f %f\r\n",central[0],central[1]);
while(1) {
//Aquire new values for the Gyro and Acc
ark.getAccelero(Acc);
ark.getGyro(Gyro);
-
+
//Calcuate the resulting vector R from the 3 acc axes
R = sqrt(std::pow(Acc[0] , 2) + std::pow(Acc[1] , 2) + std::pow(Acc[2] , 2));
-
- kalman_predict(&filter_pitch, Gyro[0], (ProgramTimer.read_us() - timer));
+
+ kalman_predict(&filter_pitch, Gyro[0], (ProgramTimer.read_us() - timer));
kalman_update(&filter_pitch, acos(Acc[0]/R));
- kalman_predict(&filter_roll, Gyro[1], (ProgramTimer.read_us() - timer));
+ kalman_predict(&filter_roll, Gyro[1], (ProgramTimer.read_us() - timer));
kalman_update(&filter_roll, acos(Acc[1]/R));
kalman_predict(&filter_yaw, Gyro[2], (ProgramTimer.read_us() - timer));
kalman_update(&filter_yaw, acos(Acc[2]/R));
-
+
angle[0] = kalman_get_angle(&filter_pitch);
angle[1] = kalman_get_angle(&filter_roll);
angle[2] = kalman_get_angle(&filter_yaw);
-
+
timer = ProgramTimer.read_us();
angle[2] = (sqrt(Acc[0]*Acc[0] + (Acc[1] * Acc[1])) / Acc[2]);
//count++;
//if(count == 10000){pc.printf("Program time: %i\n\r", ProgramTimer.read_us() - loopStartTime);loopStartTime = ProgramTimer.read_us(); count = 0;}
- 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]);
+ //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]);
delta[0] = Rad2Dree * angle[1];
delta[1] = Rad2Dree * angle[0];
delta[2] = Rad2Dree * angle[2];
@@ -118,77 +118,58 @@
//z = cos(Rad2Dree * angle[0]) * dir;
//pc.printf("x: %f\n , y: %f\n , z: %f\n",x,y,z);
//wait_ms(100);
- switch (drum_stt)
- {
+ switch (drum_stt) {
case 0:
- 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;
- if ((delta[0] >= 20)) drum_stt = 4;
+ 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;
+ //if ((delta[0] >= 20)) drum_stt = 4;
break;
case 1:
- 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))
- {
- pc.printf("drum 1_1");
- drum_1_stt = 1;
- }
- else if ((delta[0] > delta_1_x[1]))
- {
- drum_stt = 0 ;
- drum_1_stt = 0;
- }
+ 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)) {
+ pc.printf("drum 1_1");
+ drum_1_stt = 1;
+ } else if ((delta[0] > delta_1_x[1])) {
+ drum_stt = 0 ;
+ drum_1_stt = 0;
+ }
break;
case 2:
- if ((delta[0] > -30) && (delta[0] < -25) && (delta[1] < 18) && (delta[1] > 17) && (drum_2_stt == 0))
- {
- pc.printf("drum 2_2");
- drum_2_stt = 1;
- }
- else if ((delta[0] > -25))
- {
- drum_stt = 0;
- drum_2_stt = 0;
- }
- else if ((delta[0] <= -30)) drum_stt = 1;
- break;
+ if ((delta[0] > -30) && (delta[0] < -25) && (delta[1] < 18) && (delta[1] > 17) && (drum_2_stt == 0)) {
+ pc.printf("drum 2_2");
+ drum_2_stt = 1;
+ } else if ((delta[0] > -25)) {
+ drum_stt = 0;
+ drum_2_stt = 0;
+ } else if ((delta[0] <= -30)) drum_stt = 1;
+ break;
case 3:
- if ((delta[0] > -25) && (delta[0] < -20) && (delta[1] < 17) && (delta[1] > 16) && (drum_3_stt == 0))
- {
- pc.printf("drum 3_3");
- drum_3_stt = 1;
- }
- else if ((delta[0] > -20))
- {
- drum_stt = 0;
- drum_3_stt = 0;
- }
- else if ((delta[0] <= -25)) drum_stt = 2;
+ if ((delta[0] > -25) && (delta[0] < -20) && (delta[1] < 17) && (delta[1] > 16) && (drum_3_stt == 0)) {
+ pc.printf("drum 3_3");
+ drum_3_stt = 1;
+ } else if ((delta[0] > -20)) {
+ drum_stt = 0;
+ drum_3_stt = 0;
+ } else if ((delta[0] <= -25)) drum_stt = 2;
break;
case 4:
- if ((delta[0] > 20) && (delta[0] < 25) && (delta[1] > 16) && (delta[1] < 17) && (drum_4_stt == 0))
- {
- pc.printf("drum 4_4");
- drum_4_stt = 1;
- }
- else if ((delta[0] < 20))
- {
- drum_stt = 0;
- drum_4_stt = 0;
- }
- else if ((delta[0] >=25)) drum_stt = 5;
+ if ((delta[0] > 20) && (delta[0] < 25) && (delta[1] > 16) && (delta[1] < 17) && (drum_4_stt == 0)) {
+ pc.printf("drum 4_4");
+ drum_4_stt = 1;
+ } else if ((delta[0] < 20)) {
+ drum_stt = 0;
+ drum_4_stt = 0;
+ } else if ((delta[0] >=25)) drum_stt = 5;
break;
case 5:
- if ((delta[0] >= 25) && (delta[1] > 23) && (delta[1] < 24) && (drum_5_stt == 0))
- {
- pc.printf("drum 5_5");
- drum_5_stt = 1;
- }
- else if (delta[0] < 25)
- {
- drum_stt = 0;
- drum_5_stt = 0;
- }
+ if ((delta[0] >= 25) && (delta[1] > 23) && (delta[1] < 24) && (drum_5_stt == 0)) {
+ pc.printf("drum 5_5");
+ drum_5_stt = 1;
+ } else if (delta[0] < 25) {
+ drum_stt = 0;
+ drum_5_stt = 0;
+ }
break;
}
-
+
}
}
@@ -197,7 +178,7 @@
drum[0] = Rad2Dree * angle[1];
drum[1] = Rad2Dree * angle[0];
drum[2] = Rad2Dree * angle[2];
- pc.printf("drum %f %f\r\n",drum[0],drum[1]);
+ pc.printf("drum %f %f %f\r\n",drum[0],drum[1],drum[2]);
pc.printf("deltaX: %f , deltaY: %f\r\n",delta[0],delta[1]);
radius(drum[0],drum[1],drum[2],1);
}