Dependencies:   mbed FatFileSystem

Files at this revision

API Documentation at this revision

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
diff -r 75227c386257 -r 0c42db451cb9 main.cpp
--- 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");
     }
     
diff -r 75227c386257 -r 0c42db451cb9 mbed.bld
--- 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