yal kaiyo
/
130110_YNU_MPU_1
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.
Fork of
YNU_MPU_0108
by 
hige dura
Diff: main.cpp
- Revision:
- 4:037fab837823
- Parent:
- 3:17236ad0ff1e
- Child:
- 5:192835ac6106
--- a/main.cpp Mon Jan 07 08:58:34 2013 +0000
+++ b/main.cpp Tue Jan 08 12:22:43 2013 +0000
@@ -34,6 +34,8 @@
//pc.baud(921600);
mavc.baud(115200);
+ int flag = 0;
+
char buf_char;
unsigned int buf_hex[30] = {0};
unsigned int buf_dec[10] = {0};
@@ -58,39 +60,41 @@
// ******************** HOSHINO *********************
float time = 0; //time
- float xs = 0;
- float ys = 0;
+ float xis = 0;
+ float ets = 0;
float psis = 0;
float x0 = 0;
float y0 = 0;
- float psi0 = 0;
- float xf = 200; //syuutandaunrenji
- float h0 = 200;
- float hf = 10;
+ float drf = 800; //syuutandaunrenji
+ float ht0 = 200;
+ float htf = 10;
float gm0 = 20*pi/180;
float gmf = 5*pi/180;
- float b0 = h0;
+ float b0 = ht0;
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 b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf);
+ float b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
+ float htc = 0;
+ float x = 0; //down range
float ua = 10; //initial horizontal velosity
- float xs1 = 0;
- float ys1 = 0;
- float xr = 0;
- float yr = 0;
+ float xis1 = 0;
+ float ets1 = 0;
+ float xir = 0;
+ float etr = 0;
float drc = 0;
float dr = 0;
- float dt = 0.1;
- float psi = 1;
+ float dt = 0.1;
+ float psr = 0;
+ float htr = 0;
+ float avp = 0;
+ float avq = 0;
+ float avr = 0;
float s = 0;
//homotopy
- float drf = 800.0;
float rps0 = 90.0;
- float rxi0 = -600.0;
- float ret0 = -200.0;
+ float rxi0 = -600.0; //terminal xi
+ float ret0 = -200.0; //terminal et
float ps0 = rps0*dtr;
float xi0 = rxi0/drf;
float et0 = ret0/drf;
@@ -98,33 +102,35 @@
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};
+ float ph0[mh] = {0};
+ float phi[mh] = {0};
+ float fn[mh] = {0};
+ float wk[mh] = {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 dph1[mh] = {0};
+ float dph2[mh] = {0};
+ float dph3[mh] = {0};
+ float dph4[mh] = {0};
+ float fph[mh][mh] = {0};
+ float fpi[mh][mh] = {0};
+ float x1[4][mt] = {0};
+ float rl[4][mt] = {0};
float qmax=0.0;
float rdet =1.0;
float idet =0.0;
float t = 0.0;
- float phf[mh]={0.0};
+ float phf[mh]={0};
//********************* HOSHINO *********************
+ //********************* OHTSU ***********************
+ //********************* OHTSU ***********************
+
FILE *fp = fopen("/sd/sdtest.txt", "w");
if(fp == NULL) {
error("Could not open file for write\n");
}
- while(1){
- //for(int j=0;j<1800;j++){ flag tukuru // for 2 minuts
+ while(flag<120){ //flag tukuru // for 2 minuts
// ******************** sequence No.1 ********************
buf_char = mavc.getc();
@@ -158,34 +164,38 @@
// ******************** initialization *******************
if(t == 0){
- x0 = GPS[0];
- y0 = GPS[1];
- psi0= azi;
+ x0 = GPS[0];
+ y0 = GPS[1];
+ ps0 = azi;
}
- xs = GPS[0] - x0;
- ys = GPS[1] - y0;
- psis= azi - psi0;
+ xis = GPS[0] - x0;
+ ets = GPS[1] - y0;
+ psr = azi - ps0;
+ htr = alt;
+ avp = gyro[0];
+ avq = gyro[1];
+ avr = gyro[2];
// ******************** initialization *******************
// ******************** interpolation ********************
dr = dr + ua * dt;
- xr = xr + ua * dt * cos(psi);
- yr = yr + ua * dt * sin(psi);
+ xir = xir + ua * dt * cos(psr);
+ etr = etr + ua * dt * sin(psr);
- if (xs != xs1 || fabs(xs-xs1)<50) {
- s = (xs-xs1)*(xs-xs1)+(ys-ys1)*(ys-ys1);
+ if (xis != xis1 || fabs(xis-xis1)<50) {
+ s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1);
ua = 0.5*(ua+sqrt(s));
drc = drc+ua;
dr= drc;
- xr=xs;
- yr=ys;
+ xir=xis;
+ etr=ets;
}
- xs1=xs;
- ys1=ys;
+ xis1=xis;
+ ets1=ets;
// ******************** interpolation ********************
// ******************** guidance law WO homo *************
- hr = b0 + b1*x + b2*x*x + b3*x*x*x;
+ htc = b0 + b1*dr + b2*dr*dr + b3*dr*dr*dr;
/*
c = drp-dr[i];
@@ -200,7 +210,7 @@
etc=c0*et[i+1]+c1*et[i];
break;
}
- */
+ *///kurikaesituika
// ******************** guidance law WO homo *************
@@ -211,7 +221,9 @@
// !!! HOSHINO !!! fprintf add guidance, velocity and control
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]);
-
+
+ flag++;
+
}
time = time + 0.1;