yal kaiyo / Mbed 2 deprecated YNU_MPU_tora

for hige

Fork of YNU_MPU_1 by yal kaiyo

main.cpp@5:192835ac6106, 2013-01-08 (annotated)

Committer:
higedura
Date:
Tue Jan 08 12:54:47 2013 +0000
Revision:
5:192835ac6106
Parent:
4:037fab837823
Child:
6:3792b68b1fc9
130108_ add_@lc_toratani

Who changed what in which revision?

UserRevisionLine numberNew contents of line
higedura 0:9aa9be24636a 1 #include "mbed.h"
higedura 0:9aa9be24636a 2 #include "SDFileSystem.h"
higedura 0:9aa9be24636a 3
higedura 0:9aa9be24636a 4 #define pi 3.1416
higedura 0:9aa9be24636a 5 #define data 6
higedura 0:9aa9be24636a 6
khoshino 2:30f96c159d9c 7 #define zer 0.0
khoshino 2:30f96c159d9c 8 #define hal 0.5
khoshino 2:30f96c159d9c 9 #define one 1.0
khoshino 2:30f96c159d9c 10 #define dtr 0.01745
khoshino 2:30f96c159d9c 11 #define rtd 57.3
khoshino 2:30f96c159d9c 12 //parameter//
khoshino 2:30f96c159d9c 13 #define dt1 0.2 // 1/N
khoshino 2:30f96c159d9c 14 #define hdt 0.1 // 1/2N
khoshino 2:30f96c159d9c 15 #define md 6 //kizami N
khoshino 2:30f96c159d9c 16 #define mt 7 //md+1
khoshino 2:30f96c159d9c 17 #define mx 4
khoshino 2:30f96c159d9c 18 #define mv 8
khoshino 2:30f96c159d9c 19 #define mh 56 //mt*mv
khoshino 2:30f96c159d9c 20
higedura 0:9aa9be24636a 21 Serial pc(USBTX, USBRX);
higedura 0:9aa9be24636a 22 DigitalOut led1(LED1);
higedura 0:9aa9be24636a 23 DigitalOut led2(LED2);
higedura 0:9aa9be24636a 24 DigitalOut led3(LED3);
higedura 0:9aa9be24636a 25 DigitalOut led4(LED4);
higedura 0:9aa9be24636a 26
higedura 0:9aa9be24636a 27 SDFileSystem sd(p5, p6, p7, p8, "sd");
higedura 0:9aa9be24636a 28 Serial mavc(p9, p10);
higedura 0:9aa9be24636a 29 DigitalIn stop_sd(p11);
higedura 0:9aa9be24636a 30
higedura 0:9aa9be24636a 31 int main() {
higedura 1:ca296cfa603b 32
higedura 1:ca296cfa603b 33 // ******************** TORATANI ********************
higedura 1:ca296cfa603b 34 //pc.baud(921600);
higedura 0:9aa9be24636a 35 mavc.baud(115200);
higedura 0:9aa9be24636a 36
higedura 5:192835ac6106 37 int flag_lc = 0;
higedura 5:192835ac6106 38 int flag_cm = 0;
higedura 5:192835ac6106 39 int loop = 1200; // for 2 minuts
khoshino 4:037fab837823 40
higedura 0:9aa9be24636a 41 char buf_char;
higedura 0:9aa9be24636a 42 unsigned int buf_hex[30] = {0};
higedura 0:9aa9be24636a 43 unsigned int buf_dec[10] = {0};
higedura 0:9aa9be24636a 44
higedura 0:9aa9be24636a 45 // rx
higedura 0:9aa9be24636a 46 double acc[3] = {0};
higedura 0:9aa9be24636a 47 double gyro[3] = {0};
higedura 0:9aa9be24636a 48 double azi;
higedura 0:9aa9be24636a 49 double alt;
higedura 0:9aa9be24636a 50 double GPS[2] = {0};
higedura 0:9aa9be24636a 51
higedura 0:9aa9be24636a 52 // tx
higedura 0:9aa9be24636a 53 double com[3] = {0};
higedura 0:9aa9be24636a 54 char H;
higedura 0:9aa9be24636a 55 char D[data];
higedura 0:9aa9be24636a 56 char test[9];
higedura 0:9aa9be24636a 57 H = 0x02; // header of dateset2
higedura 0:9aa9be24636a 58 D[0] = 0xff; D[1] = 0xff; // p_com
higedura 0:9aa9be24636a 59 D[2] = 0x7f; D[3] = 0xff; // q_com
higedura 0:9aa9be24636a 60 D[4] = 0x00; D[5] = 0x00; // r_com
higedura 1:ca296cfa603b 61 // ******************** TORATANI ********************
higedura 0:9aa9be24636a 62
khoshino 2:30f96c159d9c 63 // ******************** HOSHINO *********************
khoshino 2:30f96c159d9c 64 float time = 0; //time
khoshino 4:037fab837823 65 float xis = 0;
khoshino 4:037fab837823 66 float ets = 0;
khoshino 2:30f96c159d9c 67 float psis = 0;
khoshino 2:30f96c159d9c 68 float x0 = 0;
khoshino 2:30f96c159d9c 69 float y0 = 0;
khoshino 4:037fab837823 70 float drf = 800; //syuutandaunrenji
khoshino 4:037fab837823 71 float ht0 = 200;
khoshino 4:037fab837823 72 float htf = 10;
khoshino 2:30f96c159d9c 73 float gm0 = 20*pi/180;
khoshino 2:30f96c159d9c 74 float gmf = 5*pi/180;
khoshino 4:037fab837823 75 float b0 = ht0;
khoshino 2:30f96c159d9c 76 float b1 = tan(gm0);
khoshino 4:037fab837823 77 float b2 = (3*(htf-ht0)-drf*(2*tan(gm0)+tan(gmf)))/(drf*drf);
khoshino 4:037fab837823 78 float b3 = -(2*(htf-ht0)-drf*(tan(gm0)+tan(gmf)))/(drf*drf*drf);
khoshino 4:037fab837823 79 float htc = 0;
khoshino 4:037fab837823 80 float x = 0; //down range
khoshino 2:30f96c159d9c 81 float ua = 10; //initial horizontal velosity
khoshino 4:037fab837823 82 float xis1 = 0;
khoshino 4:037fab837823 83 float ets1 = 0;
khoshino 4:037fab837823 84 float xir = 0;
khoshino 4:037fab837823 85 float etr = 0;
khoshino 2:30f96c159d9c 86 float drc = 0;
khoshino 2:30f96c159d9c 87 float dr = 0;
khoshino 4:037fab837823 88 float dt = 0.1;
khoshino 4:037fab837823 89 float psr = 0;
khoshino 4:037fab837823 90 float htr = 0;
khoshino 4:037fab837823 91 float avp = 0;
khoshino 4:037fab837823 92 float avq = 0;
khoshino 4:037fab837823 93 float avr = 0;
khoshino 2:30f96c159d9c 94 float s = 0;
khoshino 2:30f96c159d9c 95
khoshino 2:30f96c159d9c 96 //homotopy
khoshino 2:30f96c159d9c 97 float rps0 = 90.0;
khoshino 4:037fab837823 98 float rxi0 = -600.0; //terminal xi
khoshino 4:037fab837823 99 float ret0 = -200.0; //terminal et
khoshino 2:30f96c159d9c 100 float ps0 = rps0*dtr;
khoshino 2:30f96c159d9c 101 float xi0 = rxi0/drf;
khoshino 2:30f96c159d9c 102 float et0 = ret0/drf;
khoshino 2:30f96c159d9c 103 float eps = 0.00000001;
khoshino 2:30f96c159d9c 104 float ueps = 0.000001;
khoshino 2:30f96c159d9c 105 float ier = 0.0;
khoshino 2:30f96c159d9c 106 float sum = 0.0;
khoshino 4:037fab837823 107 float ph0[mh] = {0};
khoshino 4:037fab837823 108 float phi[mh] = {0};
khoshino 4:037fab837823 109 float fn[mh] = {0};
khoshino 4:037fab837823 110 float wk[mh] = {0};
khoshino 2:30f96c159d9c 111 int iwk[mh] = {0};
khoshino 4:037fab837823 112 float dph1[mh] = {0};
khoshino 4:037fab837823 113 float dph2[mh] = {0};
khoshino 4:037fab837823 114 float dph3[mh] = {0};
khoshino 4:037fab837823 115 float dph4[mh] = {0};
khoshino 4:037fab837823 116 float fph[mh][mh] = {0};
khoshino 4:037fab837823 117 float fpi[mh][mh] = {0};
khoshino 4:037fab837823 118 float x1[4][mt] = {0};
khoshino 4:037fab837823 119 float rl[4][mt] = {0};
khoshino 2:30f96c159d9c 120 float qmax=0.0;
khoshino 2:30f96c159d9c 121 float rdet =1.0;
khoshino 2:30f96c159d9c 122 float idet =0.0;
khoshino 2:30f96c159d9c 123 float t = 0.0;
khoshino 4:037fab837823 124 float phf[mh]={0};
khoshino 2:30f96c159d9c 125 //********************* HOSHINO *********************
khoshino 2:30f96c159d9c 126
khoshino 4:037fab837823 127 //********************* OHTSU ***********************
higedura 5:192835ac6106 128
khoshino 4:037fab837823 129 //********************* OHTSU ***********************
khoshino 4:037fab837823 130
higedura 0:9aa9be24636a 131 FILE *fp = fopen("/sd/sdtest.txt", "w");
higedura 0:9aa9be24636a 132 if(fp == NULL) {
higedura 0:9aa9be24636a 133 error("Could not open file for write\n");
higedura 0:9aa9be24636a 134 }
higedura 0:9aa9be24636a 135
higedura 5:192835ac6106 136 // ******************** waiting @LC ********************
higedura 5:192835ac6106 137 while(flag_lc==0){
higedura 5:192835ac6106 138 buf_char = mavc.getc();
higedura 5:192835ac6106 139 buf_hex[0] = (unsigned int)buf_char;
higedura 5:192835ac6106 140 for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; }
higedura 5:192835ac6106 141 if(buf_hex[29]==0x40 && buf_hex[28]==0x4C && buf_hex[27]==0x43 && buf_hex[26]==0x0D){
higedura 5:192835ac6106 142 // ******************** waiting @LC ********************
higedura 5:192835ac6106 143
higedura 5:192835ac6106 144 // ******************** homotopy ********************
higedura 5:192835ac6106 145
higedura 5:192835ac6106 146 flag_lc = 1; // end of homotopy
higedura 5:192835ac6106 147 // ******************** homotopy ********************
higedura 5:192835ac6106 148
higedura 5:192835ac6106 149 }
higedura 5:192835ac6106 150 }
higedura 5:192835ac6106 151
higedura 5:192835ac6106 152 while(flag_cm<loop){
higedura 0:9aa9be24636a 153
higedura 0:9aa9be24636a 154 // ******************** sequence No.1 ********************
higedura 0:9aa9be24636a 155 buf_char = mavc.getc();
higedura 0:9aa9be24636a 156 buf_hex[0] = (unsigned int)buf_char;
higedura 0:9aa9be24636a 157 for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; }
higedura 0:9aa9be24636a 158 if(buf_hex[29]==0x40 && buf_hex[28]==0x43 && buf_hex[27]==0x4D && buf_hex[26]==0x0D){
higedura 0:9aa9be24636a 159 // ******************** sequence No.1 ********************
higedura 0:9aa9be24636a 160
higedura 0:9aa9be24636a 161 // ******************** sequence No.2 ********************
higedura 0:9aa9be24636a 162 // send to mavc
higedura 0:9aa9be24636a 163 mavc.putc(H);
higedura 0:9aa9be24636a 164 for(int i=0;i<data;i++){ mavc.putc(D[i]); }
higedura 0:9aa9be24636a 165 //for(int i=0;i<9;i++){ mavc.putc(test[i]); }
higedura 0:9aa9be24636a 166 // ******************** sequence No.2 ********************
higedura 0:9aa9be24636a 167
higedura 0:9aa9be24636a 168 // ******************** sequence No.3 ********************
higedura 0:9aa9be24636a 169 // buf | 25 | 24 | 23 | 22 | 21 | 20 | 19 | 18 | 17 | 16 | 15 | 14 | 13 | 12 | 11 | 10 | 9 | 8 | 7 | 6 | 5 |
higedura 0:9aa9be24636a 170 // |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY |
higedura 0:9aa9be24636a 171 // | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] |
higedura 0:9aa9be24636a 172 // transrating hex to dec
higedura 0:9aa9be24636a 173 for(int i=0;i<10;i++){ buf_dec[i] = buf_hex[24-i*2]*256+buf_hex[23-i*2]; }
higedura 0:9aa9be24636a 174 // wakariyasuku surutame bunnkatsu, honnrai ha matrix ga yoi
higedura 0:9aa9be24636a 175 // -> saisyuteki niha matrix ni simasu?
higedura 0:9aa9be24636a 176 for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec[i]-32768)/65535*20*9.8; }
higedura 0:9aa9be24636a 177 for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec[i+3]-32768)/65535*600/180*pi; }
higedura 0:9aa9be24636a 178 azi = (double)buf_dec[6]/65535*2*pi;
higedura 0:9aa9be24636a 179 alt = ((double)buf_dec[7]-32768)*0.1;
higedura 0:9aa9be24636a 180 for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec[i+8]-32768)*0.1; }
higedura 0:9aa9be24636a 181 // ******************** sequence No.3 ********************
higedura 0:9aa9be24636a 182
khoshino 2:30f96c159d9c 183 // ******************** initialization *******************
khoshino 2:30f96c159d9c 184 if(t == 0){
khoshino 4:037fab837823 185 x0 = GPS[0];
khoshino 4:037fab837823 186 y0 = GPS[1];
khoshino 4:037fab837823 187 ps0 = azi;
khoshino 2:30f96c159d9c 188 }
khoshino 4:037fab837823 189 xis = GPS[0] - x0;
khoshino 4:037fab837823 190 ets = GPS[1] - y0;
khoshino 4:037fab837823 191 psr = azi - ps0;
khoshino 4:037fab837823 192 htr = alt;
khoshino 4:037fab837823 193 avp = gyro[0];
khoshino 4:037fab837823 194 avq = gyro[1];
khoshino 4:037fab837823 195 avr = gyro[2];
khoshino 2:30f96c159d9c 196 // ******************** initialization *******************
khoshino 2:30f96c159d9c 197
khoshino 2:30f96c159d9c 198 // ******************** interpolation ********************
khoshino 2:30f96c159d9c 199 dr = dr + ua * dt;
khoshino 4:037fab837823 200 xir = xir + ua * dt * cos(psr);
khoshino 4:037fab837823 201 etr = etr + ua * dt * sin(psr);
khoshino 2:30f96c159d9c 202
khoshino 4:037fab837823 203 if (xis != xis1 || fabs(xis-xis1)<50) {
khoshino 4:037fab837823 204 s = (xis-xis1)*(xis-xis1)+(ets-ets1)*(ets-ets1);
khoshino 2:30f96c159d9c 205 ua = 0.5*(ua+sqrt(s));
khoshino 2:30f96c159d9c 206 drc = drc+ua;
khoshino 2:30f96c159d9c 207 dr= drc;
khoshino 4:037fab837823 208 xir=xis;
khoshino 4:037fab837823 209 etr=ets;
khoshino 2:30f96c159d9c 210 }
khoshino 4:037fab837823 211 xis1=xis;
khoshino 4:037fab837823 212 ets1=ets;
khoshino 2:30f96c159d9c 213 // ******************** interpolation ********************
khoshino 2:30f96c159d9c 214
khoshino 2:30f96c159d9c 215 // ******************** guidance law WO homo *************
khoshino 4:037fab837823 216 htc = b0 + b1*dr + b2*dr*dr + b3*dr*dr*dr;
khoshino 2:30f96c159d9c 217
khoshino 2:30f96c159d9c 218 /*
khoshino 2:30f96c159d9c 219 c = drp-dr[i];
khoshino 2:30f96c159d9c 220 c1= dr[i+1]-drp;
khoshino 2:30f96c159d9c 221
khoshino 2:30f96c159d9c 222 if(c >=0 && c1 >= 0)
khoshino 2:30f96c159d9c 223 {
khoshino 2:30f96c159d9c 224 c0=c/(dr[i+1]-dr[i]);
khoshino 2:30f96c159d9c 225 c1=c1/(dr[i+1]-dr[i]);
khoshino 2:30f96c159d9c 226 psc=c0*ps[i+1]+c1*ps[i];
khoshino 2:30f96c159d9c 227 xic=c0*xi[i+1]+c1*xi[i];
khoshino 2:30f96c159d9c 228 etc=c0*et[i+1]+c1*et[i];
khoshino 2:30f96c159d9c 229 break;
khoshino 2:30f96c159d9c 230 }
khoshino 4:037fab837823 231 *///kurikaesituika
khoshino 2:30f96c159d9c 232
khoshino 2:30f96c159d9c 233 // ******************** guidance law WO homo *************
khoshino 2:30f96c159d9c 234
khoshino 2:30f96c159d9c 235 // ******************** control law **********************
higedura 0:9aa9be24636a 236 // transrating dec to hex
higedura 0:9aa9be24636a 237 //com
khoshino 2:30f96c159d9c 238 // ******************** control law **********************
higedura 0:9aa9be24636a 239
higedura 3:17236ad0ff1e 240 // !!! HOSHINO !!! fprintf add guidance, velocity and control
higedura 3:17236ad0ff1e 241 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]);
khoshino 4:037fab837823 242
higedura 5:192835ac6106 243 flag_cm++;
khoshino 4:037fab837823 244
higedura 0:9aa9be24636a 245 }
higedura 3:17236ad0ff1e 246
higedura 3:17236ad0ff1e 247 time = time + 0.1;
higedura 3:17236ad0ff1e 248
higedura 0:9aa9be24636a 249 }
higedura 0:9aa9be24636a 250
higedura 0:9aa9be24636a 251 fclose(fp);
higedura 0:9aa9be24636a 252 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:9aa9be24636a 253
higedura 0:9aa9be24636a 254 }