Diff: main.cpp

Revision:

2:30f96c159d9c

Parent:

1:ca296cfa603b

Child:

3:17236ad0ff1e

--- a/main.cpp	Sun Jan 06 07:11:34 2013 +0000
+++ b/main.cpp	Mon Jan 07 08:47:36 2013 +0000
@@ -4,6 +4,20 @@
 #define pi      3.1416
 #define data    6
 
+#define zer 0.0
+#define hal 0.5
+#define one 1.0
+#define dtr 0.01745
+#define rtd 57.3
+//parameter//
+#define dt1 0.2 // 1/N
+#define hdt 0.1 // 1/2N
+#define md  6 //kizami N
+#define mt  7 //md+1
+#define mx  4
+#define mv  8
+#define mh  56 //mt*mv
+
 Serial pc(USBTX, USBRX);
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
@@ -42,6 +56,68 @@
     D[4]    =   0x00;   D[5]    =   0x00;       // r_com
     // ******************** TORATANI ********************
     
+    // ******************** HOSHINO *********************
+    float   time    =   0;      //time
+    float   xs      =   0;
+    float   ys      =   0;
+    float   psis    =   0;
+    float   x0      =   0;
+    float   y0      =   0;
+    float   psi0    =   0;
+    float   xf      =   200;    //syuutandaunrenji
+    float   h0      =   200;
+    float   hf      =   10;
+    float   gm0     =   20*pi/180;
+    float   gmf     =   5*pi/180;
+    float   b0      =   h0;
+    float   b1      =   tan(gm0);
+    float   b2      =   (3*(hf-h0)-xf*(2*tan(gm0)+tan(gmf)))/(xf*xf);
+    float   b3      =   -(2*(hf-h0)-xf*(tan(gm0)+tan(gmf)))/(xf*xf*xf);
+    float   hr      =   0;
+    float   x       =   0;
+    float   ua = 10;    //initial horizontal velosity
+    float   xs1 = 0;
+    float   ys1 = 0;
+    float   xr = 0;
+    float   yr = 0;
+    float   drc = 0;
+    float   dr  = 0;
+    float   dt = 0.1;
+    float   psi = 1;
+    float   s = 0;
+    
+    //homotopy
+    float drf  = 800.0;
+    float rps0 = 90.0;
+    float rxi0 = -600.0;
+    float ret0 = -200.0;
+    float ps0  = rps0*dtr;
+    float xi0  = rxi0/drf;
+    float et0  = ret0/drf;
+    float eps  = 0.00000001;
+    float ueps = 0.000001;
+    float ier  = 0.0;
+    float sum  = 0.0;
+    float ph0[mh] = {0.0};
+    float phi[mh] = {0.0};
+    float fn[mh]  = {0.0};
+    float wk[mh]  = {0.0};
+    int    iwk[mh] = {0};
+    float dph1[mh] = {0.0};
+    float dph2[mh] = {0.0};
+    float dph3[mh] = {0.0};
+    float dph4[mh] = {0.0};
+    float fph[mh][mh] = {0.0};
+    float fpi[mh][mh] = {0.0};
+    float x1[4][mt]   = {0.0};
+    float rl[4][mt]   = {0.0};
+    float qmax=0.0;
+    float rdet =1.0;
+    float idet =0.0;
+    float t = 0.0;
+    float phf[mh]={0.0};
+    //********************* HOSHINO *********************
+    
     FILE *fp = fopen("/sd/sdtest.txt", "w");
     if(fp == NULL) {
         error("Could not open file for write\n");
@@ -80,16 +156,65 @@
             for(int i=0;i<2;i++){   GPS[i]  =   ((double)buf_dec[i+8]-32768)*0.1;   }
             // ******************** sequence No.3 ********************
             
-            // ******************** control law ********************
+            // ******************** initialization *******************
+            if(t == 0){
+            x0  =   GPS[0];
+            y0  =   GPS[1];
+            psi0=   azi;
+            }
+            xs  =   GPS[0]  -   x0;
+            ys  =   GPS[1]  -   y0;
+            psis=   azi     -   psi0;
+            // ******************** initialization *******************
+            
+            // ******************** interpolation ********************
+            dr = dr + ua * dt;
+            xr = xr + ua * dt * cos(psi);
+            yr = yr + ua * dt * sin(psi);
+
+            if (xs != xs1 || fabs(xs-xs1)<50) {
+                s = (xs-xs1)*(xs-xs1)+(ys-ys1)*(ys-ys1);
+                ua = 0.5*(ua+sqrt(s));
+                drc = drc+ua;
+                dr= drc;
+                xr=xs;
+                yr=ys;
+            }
+            xs1=xs;
+            ys1=ys;
+            // ******************** interpolation ********************
+            
+            // ******************** guidance law WO homo *************
+            hr =    b0  +   b1*x    +   b2*x*x  +   b3*x*x*x;
+            
+            /*
+            c = drp-dr[i];
+            c1= dr[i+1]-drp;
+
+            if(c >=0 && c1 >= 0)
+            {
+            c0=c/(dr[i+1]-dr[i]);
+            c1=c1/(dr[i+1]-dr[i]);
+            psc=c0*ps[i+1]+c1*ps[i];
+            xic=c0*xi[i+1]+c1*xi[i];
+            etc=c0*et[i+1]+c1*et[i];
+            break;
+             }
+            */
+            
+            
+            // ******************** guidance law WO homo *************
+            
+            // ******************** control law **********************
             // transrating dec to hex
             //com
-            // ******************** control law ********************
+            // ******************** control law **********************
             
             //pc.printf("%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]);
             //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]);
                         
         }
-             
+       time = time + 0.1;      
     }
     
     fclose(fp);