yal kaiyo
for hige
Fork of
YNU_MPU_1
by 
yal kaiyo
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);