hige dura
/
dotHR_EKF_phithe
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed FatFileSystem
Revision 1:0c42db451cb9, committed 2012-09-17
- Comitter:
- higedura
- Date:
- Mon Sep 17 09:39:54 2012 +0000
- Parent:
- 0:75227c386257
- Commit message:
- dotHR_phothe
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sun Jun 10 08:44:32 2012 +0000
+++ b/main.cpp Mon Sep 17 09:39:54 2012 +0000
@@ -54,24 +54,24 @@
int main(){
pc.baud(921600);
- navi.baud(57600);
+ navi.baud(115200);
xbee.baud(57600);
+ // SD card
FILE *fp = fopen("/sd/sdtest.txt", "w");
if(fp == NULL) {
error("Could not open file for write\n");
while(1){ led2 = 1; wait(.5); led2 = 0; led3 = 0; wait(.5); led3 = 0; }
}
- int correct_period = 100;
+ int correct_period = 200;
int navi_period = 10;
int xbee_period = 100;
int correct_loop = 0;
int navi_loop = 10;
int xbee_loop = 100;
- double dt_wait = 0.002;
- //double dt_wait = 0.01;
+ double dt_wait = 0.002; // 100Hz
double dt = 0.01;
double t = 0;
@@ -108,33 +108,47 @@
// Optimal regulator
double state[L];
-
-
+/*
// Gain (q1=100, q2=100, q3=50, q4=50, q5=1)
-
- // Gain (q1=100, q2=100, q3=50, q4=50, q5=5)
-
+ double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 0.408},
+ {-5, 2.887,-3.610, 2.084,-0.408},
+ {-5,-2.887,-3.610,-2.084, 0.408},
+ { 0,-5.774, 0,-4.168,-0.408},
+ { 5,-2.887, 3.610,-2.084, 0.408},
+ { 5, 2.887, 3.610, 2.084,-0.408}};
+*//*
// Gain (q1=100, q2=100, q3=50, q4=50, q5=10)
-
- // Gain (q1=100, q2=100, q3=50, q4=50, q5=20)
-
- // Gain (q1=100, q2=100, q3=50, q4=50, q5=30)
- double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 0.408},
- {-5, 2.887,-3.610, 2.084,-0.408},
- {-5,-2.887,-3.610,-2.084,-0.408},
- { 0,-5.774, 0,-4.168, 0.408},
- { 5,-2.887, 3.610,-2.084,-0.408},
- { 5, 2.887, 3.610, 2.084, 0.408}};
-
- // Coefficient for moving control
- double Ccom_r = 0.2;
- double Ccom_p = 0.2;
+ double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 0.9129},
+ {-5, 2.887,-3.610, 2.084,-0.9129},
+ {-5,-2.887,-3.610,-2.084, 0.9129},
+ { 0,-5.774, 0,-4.168,-0.9129},
+ { 5,-2.887, 3.610,-2.084, 0.9129},
+ { 5, 2.887, 3.610, 2.084,-0.9129}};
+*/
+ double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 17},
+ {-5, 2.887,-3.610, 2.084,-17},
+ {-5,-2.887,-3.610,-2.084, 17},
+ { 0,-5.774, 0,-4.168,-17},
+ { 5,-2.887, 3.610,-2.084, 17},
+ { 5, 2.887, 3.610, 2.084,-17}};
+/*
+ // Gain (q1=100, q2=100, q3=50, q4=50, q5=10)
+ double Kr[M][L] = {{ 0, 5.774, 0, 4.168, 1.291},
+ {-5, 2.887,-3.610, 2.084,-1.291},
+ {-5,-2.887,-3.610,-2.084, 1.291},
+ { 0,-5.774, 0,-4.168,-1.291},
+ { 5,-2.887, 3.610,-2.084, 1.291},
+ { 5, 2.887, 3.610, 2.084,-1.291}};
+*/
+ // Coefficient for position control
+ double Ccom_r = 0.3;
+ double Ccom_p = 0.3;
double Ccom_y = 0.1;
- double motor[M] = {0};
- double motor_control[M] = {0}; // Pulth width generated by the transmitter
- double motor_attitude[M] = {0}; // Pulth width generated by the attitude control
-
+ double motor_control[M] = {0}; // Pulth width generated by the transmitter [s]
+ double motor_attitude[M] = {0}; // Pulth width generated by the attitude control [s]
+ double motor[M] = {0}; // motor_control + motor_attitude [s]
+
// Coefficient of transfering Force to Puls
double Cfp = 2*pow(10.0, -7);
@@ -192,6 +206,12 @@
// Wait some time for ESC (about 10s)
wait(5);
+ // *** Calibration routine ***
+ p_calib = calib();
+ for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; }
+ for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; }
+ //compass_basis_rad = *p_calib;
+
// *** Setting up navigator ***
//pc.printf("Setting up navigator\r\n");
xbee.printf("Setting up navigator\r\n");
@@ -199,21 +219,17 @@
navi_flag = 1;
for( int i=0;i<chan_buf;i++ ){
navi.scanf("%c",&command_buf1[i]);
+ //pc.printf("%c",command_buf1[i]);
if(command_buf1[i]=='a'){
set_navi_flag=1;
+ //pc.printf("\r\nbreak!!\r\n");
break;
}
}
navi_flag = 0;
wait(.1);
}
-
- // *** Calibration routine ***
- p_calib = calib();
- for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; }
- for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; }
- //compass_basis_rad = *p_calib;
-
+
led1 = 1; led2 = 1; led3 = 1; led4 = 1;
//pc.printf("Starting IMU unit\n\r");
@@ -221,7 +237,6 @@
//pc.printf(" time phi the P Q R accX accY accZ LWMAX LWMAY LWMAZ alt T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r");
xbee.printf(" time phi the P Q R accX accY accZ LWMAX LWMAY LWMAZ T1 T2 T3 T4 T5 T6 thro roll pitch yaw\n\r");
- //while(1){
while( stop==0 ){
// Navigation
@@ -233,8 +248,8 @@
for( int i=0;i<chan_buf;i++ ){
command_buf2[i] = (int)command_buf1[i]-48;
if( command_buf2[i]==-16 ){ command_buf2[i]=0; }
+ //pc.printf("%d",command_buf2[i]);
}
- //for( int i=0;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]+0.001; }
command[0] = 0.0001*command_buf2[1]+0.00001*command_buf2[2]+0.000001*command_buf2[3]+0.001;
for( int i=1;i<chan;i++ ){ command[i] = 0.0001*command_buf2[1+i*4]+0.00001*command_buf2[2+i*4]+0.000001*command_buf2[3+i*4]-0.0005; }
@@ -243,7 +258,6 @@
}
navi_loop++;
- //command[0] = 0.0016; // Shiyougo comentout surukoto!!
// Updating accelerometer and compass
accelerometer.getOutput(bit_acc);
@@ -253,23 +267,23 @@
acc[0] = (((int16_t)bit_acc[0]-calib_acc[0])-((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0);
acc[1] = (((int16_t)bit_acc[0]-calib_acc[0])+((int16_t)bit_acc[1]-calib_acc[1]))*0.004/sqrt(2.0);
acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
-
+
gyro_deg[0] = ((gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0);
gyro_deg[1] = (-(gyro.getGyroX()-calib_gyro[0])-(gyro.getGyroY()-calib_gyro[1]))/14.375/sqrt(2.0); // Modify the differencial of the sensor axis
gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375;
-
+
// Limitter for acc
if( acc[0]<-1 ){ acc[0] = -1; } if( 1<acc[0] ){ acc[0] = 1; }
if( acc[1]<-1 ){ acc[1] = -1; } if( 1<acc[1] ){ acc[1] = 1; }
if( acc[2]<-0 ){ acc[2] = -0; } if( 2<acc[2] ){ acc[2] = 2; }
-
+
for( int i=0;i<N;i++ ){ d_acc[i] = acc[i]-acc_pre[i]; }
if( abs(d_acc[0])>=0.5 && abs(acc[0])>=0.3 ){ acc[0] = acc_pre[0]; }
if( abs(d_acc[1])>=0.5 && abs(acc[1])>=0.3 ){ acc[1] = acc_pre[1]; }
if( abs(d_acc[2])>=0.5 ){ acc[2] = acc_pre[2]; }
-
+
for( int i=0;i<N_2;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
-
+
// LWMA (Observation)
p_zLWMA = LWMA(acc);
for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA; p_zLWMA = p_zLWMA+1; }
@@ -293,7 +307,7 @@
for( int i=0;i<N_2;i++ ){ if( abs(angle_rad[i])>=1.05 ){ angle_rad[i] = angle_rad_pre[i]; } }
for( int i=0;i<N_2;i++ ){ angle_deg[i] = angle_rad[i]*180/pi; }
-
+
state[0] = angle_deg[0]; state[1] = angle_deg[1];
state[2] = gyro_deg[0]; state[3] = gyro_deg[1]; state[4] = gyro_deg[2];
@@ -310,39 +324,36 @@
motor_control[5] = command[0]+1*Ccom_r*command[1]-1*Ccom_p*command[2]-1*Ccom_y*command[3];
for( int i=0;i<M;i++ ){ motor[i] = motor_control[i]+motor_attitude[i]*Cfp; }
- //for( int i=0;i<M;i++ ){ motor[i] = command[0]; }
-
- // pulsewidth 0.0011~0.0019 (0.001~0.002?)
+
+ // pulsewidth 0.0011~0.0019 [s] (0.001~0.002?)
for( int i=0;i<M;i++ ){
- if( motor[i]>0.00195 ){ motor[i]=0.00195; }
- if( motor[i]<0.0012 ){ motor[i]=0.00105; }
+ if( motor[i]>0.00195 ){ motor[i]=0.00195; } // Notice! If motor inputs are too large, motors and ESCs may burst.
+ if( motor[i]<0.0012 ){ motor[i]=0.0012; } // Notice! If motor inputs are about 0.0011 [s] for some time, motors and ESCs may burst.
if( command[0]<0.0011 ){ motor[i]=0.00105; }
}
-
+
+ // Updating ESC pulse width
ESC1.pulsewidth(motor[0]); ESC2.pulsewidth(motor[1]); ESC3.pulsewidth(motor[2]); ESC4.pulsewidth(motor[3]); ESC5.pulsewidth(motor[4]); ESC6.pulsewidth(motor[5]);
//for( int i=0;i<M;i++ ){ thrust[i] = 630000.0*motor[i]*motor[i]-700.0*motor[i]; }
- //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
- //pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
- //fprintf(fp,"%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %6.3f\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], command[0]*1000);
fprintf(fp,"%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
if( xbee_loop>=xbee_period ){
xbee.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f, %6.3f\n\r", t, state[0], state[1], state[2], state[3], state[4], acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2], motor[0]*1000, motor[1]*1000, motor[2]*1000, motor[3]*1000, motor[4]*1000, motor[5]*1000, command[0]*1000, command[1]*1000, command[2]*1000, command[3]*1000);
xbee_loop = 0;
}
-
+
for( int i=0;i<N;i++ ){ acc_pre[i] = acc[i]; }
//for( int i=0;i<N;i++ ){ gyro_deg_pre[i] = gyro_deg[i]; }
for( int i=0;i<N_2;i++ ){ angle_rad_pre[i] = angle_rad[i]; }
-
+
xbee_loop++;
wait(dt_wait);
t += dt;
}
- // pulsewidth 0.0011~0.00195
+ // pulsewidth 0.0011~0.00195 [s]
ESC1.pulsewidth(0.001); ESC2.pulsewidth(0.001); ESC3.pulsewidth(0.001); ESC4.pulsewidth(0.001); ESC5.pulsewidth(0.001); ESC6.pulsewidth(0.001);
led1 = 0; led2 = 0; led3 = 0; led4 = 0;
fclose(fp);
@@ -567,6 +578,7 @@
f[0] = x[0]+x[1]*sin(y[0])*tan(y[1]);
f[1] = x[1]*cos(y[0]);
+ // Without a, f[1] is not passed main function for unknown reason.
a = sin(b);
return p_f;
@@ -589,7 +601,7 @@
double* p_calib_result = calib_result;
- pc.printf("Don't touch... Calibrating now!!\n\r");
+ //pc.printf("Don't touch... Calibrating now!!\n\r");
xbee.printf("Don't touch... Calibrating now!!\n\r");
led1 = 1;
@@ -628,9 +640,9 @@
compass_basis_rad = atan(compass_basis_rad);
led1 = 0; led2 = 0; led3 = 0; led4 = 0;
- pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
+ //pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
xbee.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
- pc.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
+ //pc.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
xbee.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
for( int i=0;i<3;i++ ){ calib_result[i] = calib_acc[i]; }
@@ -638,7 +650,7 @@
calib_result[6] = compass_basis_rad;
if( calib_result[0]==0 && calib_result[1]==0 && calib_result[2]==0 ){
- pc.printf("Accelerometer is not available.\r\n");
+ //pc.printf("Accelerometer is not available.\r\n");
xbee.printf("Accelerometer is not available.\r\n");
}
--- a/mbed.bld Sun Jun 10 08:44:32 2012 +0000 +++ b/mbed.bld Mon Sep 17 09:39:54 2012 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/b4b9f287a47e +http://mbed.org/users/mbed_official/code/mbed/builds/b4b9f287a47e \ No newline at end of file