main.cpp

00001 #include "mbed.h"
00002 #include "SDFileSystem.h"
00003 
00004 #define pi      3.1416
00005 #define data    6
00006 
00007 #define zer 0.0
00008 #define hal 0.5
00009 #define one 1.0
00010 #define dtr 0.01745
00011 #define rtd 57.3
00012 //parameter//
00013 #define dt1 0.2 // 1/N
00014 #define hdt 0.1 // 1/2N
00015 #define md  6 //kizami N
00016 #define mt  7 //md+1
00017 #define mx  4
00018 #define mv  8
00019 #define mh  56 //mt*mv
00020 
00021 Serial pc(USBTX, USBRX);
00022 DigitalOut led1(LED1);
00023 DigitalOut led2(LED2);
00024 DigitalOut led3(LED3);
00025 DigitalOut led4(LED4);
00026 
00027 SDFileSystem sd(p5, p6, p7, p8, "sd");
00028 Serial mavc(p9, p10);
00029 DigitalIn stop_sd(p11);
00030 
00031 int main() {
00032         
00033     // ******************** TORATANI ********************
00034     //pc.baud(921600);
00035     mavc.baud(115200);
00036     
00037     char  buf_char;
00038     unsigned int    buf_hex[30] =   {0};
00039     unsigned int    buf_dec[10] =   {0};
00040     
00041     // rx
00042     double  acc[3]  =   {0};
00043     double  gyro[3] =   {0};
00044     double  azi;
00045     double  alt;
00046     double  GPS[2]  =   {0};
00047     
00048     // tx
00049     double  com[3]  =   {0};
00050     char H;
00051     char D[data];
00052     char test[9];
00053     H       =   0x02;   // header of dateset2
00054     D[0]    =   0xff;   D[1]    =   0xff;       // p_com
00055     D[2]    =   0x7f;   D[3]    =   0xff;       // q_com
00056     D[4]    =   0x00;   D[5]    =   0x00;       // r_com
00057     // ******************** TORATANI ********************
00058     
00059     // ******************** HOSHINO *********************
00060     float   time    =   0;      //time
00061     float   xs      =   0;
00062     float   ys      =   0;
00063     float   psis    =   0;
00064     float   x0      =   0;
00065     float   y0      =   0;
00066     float   psi0    =   0;
00067     float   xf      =   200;    //syuutandaunrenji
00068     float   h0      =   200;
00069     float   hf      =   10;
00070     float   gm0     =   20*pi/180;
00071     float   gmf     =   5*pi/180;
00072     float   b0      =   h0;
00073     float   b1      =   tan(gm0);
00074     float   b2      =   (3*(hf-h0)-xf*(2*tan(gm0)+tan(gmf)))/(xf*xf);
00075     float   b3      =   -(2*(hf-h0)-xf*(tan(gm0)+tan(gmf)))/(xf*xf*xf);
00076     float   hr      =   0;
00077     float   x       =   0;
00078     float   ua = 10;    //initial horizontal velosity
00079     float   xs1 = 0;
00080     float   ys1 = 0;
00081     float   xr = 0;
00082     float   yr = 0;
00083     float   drc = 0;
00084     float   dr  = 0;
00085     float   dt = 0.1;
00086     float   psi = 1;
00087     float   s = 0;
00088     
00089     //homotopy
00090     float drf  = 800.0;
00091     float rps0 = 90.0;
00092     float rxi0 = -600.0;
00093     float ret0 = -200.0;
00094     float ps0  = rps0*dtr;
00095     float xi0  = rxi0/drf;
00096     float et0  = ret0/drf;
00097     float eps  = 0.00000001;
00098     float ueps = 0.000001;
00099     float ier  = 0.0;
00100     float sum  = 0.0;
00101     float ph0[mh] = {0.0};
00102     float phi[mh] = {0.0};
00103     float fn[mh]  = {0.0};
00104     float wk[mh]  = {0.0};
00105     int    iwk[mh] = {0};
00106     float dph1[mh] = {0.0};
00107     float dph2[mh] = {0.0};
00108     float dph3[mh] = {0.0};
00109     float dph4[mh] = {0.0};
00110     float fph[mh][mh] = {0.0};
00111     float fpi[mh][mh] = {0.0};
00112     float x1[4][mt]   = {0.0};
00113     float rl[4][mt]   = {0.0};
00114     float qmax=0.0;
00115     float rdet =1.0;
00116     float idet =0.0;
00117     float t = 0.0;
00118     float phf[mh]={0.0};
00119     //********************* HOSHINO *********************
00120     
00121     FILE *fp = fopen("/sd/sdtest.txt", "w");
00122     if(fp == NULL) {
00123         error("Could not open file for write\n");
00124     }
00125     
00126     while(1){
00127     //for(int j=0;j<1800;j++){      flag tukuru     // for 2 minuts
00128         
00129         // ******************** sequence No.1 ********************
00130         buf_char       =   mavc.getc();
00131         buf_hex[0]  =   (unsigned int)buf_char;
00132         for(int i=0;i<29;i++){  buf_hex[29-i]   =   buf_hex[28-i];  }
00133         // !!! switch bun ni suru
00134         if(buf_hex[29]==0x40 && buf_hex[28]==0x43 && buf_hex[27]==0x4D && buf_hex[26]==0x0D){
00135         // ******************** sequence No.1 ********************
00136             
00137             // ******************** sequence No.2 ********************
00138             // send to mavc
00139             mavc.putc(H);
00140             for(int i=0;i<data;i++){  mavc.putc(D[i]);  }
00141             //for(int i=0;i<9;i++){  mavc.putc(test[i]);  }
00142             // ******************** sequence No.2 ********************
00143             
00144             // ******************** sequence No.3 ********************
00145             // buf |  25  | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 |  9 |  8 |  7 |  6 |  5  |
00146             //     |header|   AccX  |   AccY  |   AccZ  |  GyroX  |  GyroY  |   GyroZ | Azimuth | Altitude|   GPSX  |   GPSY   |
00147             //     |   H  |   D[1]  |   D[2]  |   D[3]  |   D[4]  |   D[5]  |   D[6]  |   D[7]  |   D[8]  |   D[9]  |   D[10]  |
00148             // transrating hex to dec
00149             for(int i=0;i<10;i++){  buf_dec[i]   =   buf_hex[24-i*2]*256+buf_hex[23-i*2];   }
00150             // wakariyasuku surutame bunnkatsu, honnrai ha matrix ga yoi
00151             // -> saisyuteki niha matrix ni simasu?
00152             for(int i=0;i<3;i++){   acc[i]  =   ((double)buf_dec[i]-32768)/65535*20*9.8;    }
00153             for(int i=0;i<3;i++){   gyro[i] =   ((double)buf_dec[i+3]-32768)/65535*600/180*pi;  }
00154             azi =   (double)buf_dec[6]/65535*2*pi;
00155             alt =   ((double)buf_dec[7]-32768)*0.1;
00156             for(int i=0;i<2;i++){   GPS[i]  =   ((double)buf_dec[i+8]-32768)*0.1;   }
00157             // ******************** sequence No.3 ********************
00158             
00159             // ******************** initialization *******************
00160             if(t == 0){
00161             x0  =   GPS[0];
00162             y0  =   GPS[1];
00163             psi0=   azi;
00164             }
00165             xs  =   GPS[0]  -   x0;
00166             ys  =   GPS[1]  -   y0;
00167             psis=   azi     -   psi0;
00168             // ******************** initialization *******************
00169             
00170             // ******************** interpolation ********************
00171             dr = dr + ua * dt;
00172             xr = xr + ua * dt * cos(psi);
00173             yr = yr + ua * dt * sin(psi);
00174 
00175             if (xs != xs1 || fabs(xs-xs1)<50) {
00176                 s = (xs-xs1)*(xs-xs1)+(ys-ys1)*(ys-ys1);
00177                 ua = 0.5*(ua+sqrt(s));
00178                 drc = drc+ua;
00179                 dr= drc;
00180                 xr=xs;
00181                 yr=ys;
00182             }
00183             xs1=xs;
00184             ys1=ys;
00185             // ******************** interpolation ********************
00186             
00187             // ******************** guidance law WO homo *************
00188             hr =    b0  +   b1*x    +   b2*x*x  +   b3*x*x*x;
00189             
00190             /*
00191             c = drp-dr[i];
00192             c1= dr[i+1]-drp;
00193 
00194             if(c >=0 && c1 >= 0)
00195             {
00196             c0=c/(dr[i+1]-dr[i]);
00197             c1=c1/(dr[i+1]-dr[i]);
00198             psc=c0*ps[i+1]+c1*ps[i];
00199             xic=c0*xi[i+1]+c1*xi[i];
00200             etc=c0*et[i+1]+c1*et[i];
00201             break;
00202              }
00203             */
00204             
00205             // ******************** guidance law WO homo *************
00206             
00207             // ******************** control law **********************
00208             // transrating dec to hex
00209             //com
00210             // ******************** control law **********************
00211             
00212             // !!! HOSHINO !!! fprintf add guidance, velocity and control
00213             fprintf(fp, "%7.4f %7.4f %7.4f %7.3f %7.3f %7.3f %8.5f %8.1f %8.1f %8.1f\r\n", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], azi, alt, GPS[0], GPS[1]);
00214                         
00215         }
00216        
00217         time = time + 0.1;
00218           
00219     }
00220     
00221     fclose(fp); 
00222     led1    =   1;      led2    =   1;      led3    =   1;      led4    =   1;
00223     
00224 }