Kyohei Shinomoto / Mbed 2 deprecated YNU_MPUd2h

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_MPUd2 by hige dura

main.cpp@2:30f96c159d9c, 2013-01-07 (annotated)

Committer:
khoshino
Date:
Mon Jan 07 08:47:36 2013 +0000
Revision:
2:30f96c159d9c
Parent:
1:ca296cfa603b
Child:
3:17236ad0ff1e
????????????

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 0:9aa9be24636a 37 char buf_char;
higedura 0:9aa9be24636a 38 unsigned int buf_hex[30] = {0};
higedura 0:9aa9be24636a 39 unsigned int buf_dec[10] = {0};
higedura 0:9aa9be24636a 40
higedura 0:9aa9be24636a 41 // rx
higedura 0:9aa9be24636a 42 double acc[3] = {0};
higedura 0:9aa9be24636a 43 double gyro[3] = {0};
higedura 0:9aa9be24636a 44 double azi;
higedura 0:9aa9be24636a 45 double alt;
higedura 0:9aa9be24636a 46 double GPS[2] = {0};
higedura 0:9aa9be24636a 47
higedura 0:9aa9be24636a 48 // tx
higedura 0:9aa9be24636a 49 double com[3] = {0};
higedura 0:9aa9be24636a 50 char H;
higedura 0:9aa9be24636a 51 char D[data];
higedura 0:9aa9be24636a 52 char test[9];
higedura 0:9aa9be24636a 53 H = 0x02; // header of dateset2
higedura 0:9aa9be24636a 54 D[0] = 0xff; D[1] = 0xff; // p_com
higedura 0:9aa9be24636a 55 D[2] = 0x7f; D[3] = 0xff; // q_com
higedura 0:9aa9be24636a 56 D[4] = 0x00; D[5] = 0x00; // r_com
higedura 1:ca296cfa603b 57 // ******************** TORATANI ********************
higedura 0:9aa9be24636a 58
khoshino 2:30f96c159d9c 59 // ******************** HOSHINO *********************
khoshino 2:30f96c159d9c 60 float time = 0; //time
khoshino 2:30f96c159d9c 61 float xs = 0;
khoshino 2:30f96c159d9c 62 float ys = 0;
khoshino 2:30f96c159d9c 63 float psis = 0;
khoshino 2:30f96c159d9c 64 float x0 = 0;
khoshino 2:30f96c159d9c 65 float y0 = 0;
khoshino 2:30f96c159d9c 66 float psi0 = 0;
khoshino 2:30f96c159d9c 67 float xf = 200; //syuutandaunrenji
khoshino 2:30f96c159d9c 68 float h0 = 200;
khoshino 2:30f96c159d9c 69 float hf = 10;
khoshino 2:30f96c159d9c 70 float gm0 = 20*pi/180;
khoshino 2:30f96c159d9c 71 float gmf = 5*pi/180;
khoshino 2:30f96c159d9c 72 float b0 = h0;
khoshino 2:30f96c159d9c 73 float b1 = tan(gm0);
khoshino 2:30f96c159d9c 74 float b2 = (3*(hf-h0)-xf*(2*tan(gm0)+tan(gmf)))/(xf*xf);
khoshino 2:30f96c159d9c 75 float b3 = -(2*(hf-h0)-xf*(tan(gm0)+tan(gmf)))/(xf*xf*xf);
khoshino 2:30f96c159d9c 76 float hr = 0;
khoshino 2:30f96c159d9c 77 float x = 0;
khoshino 2:30f96c159d9c 78 float ua = 10; //initial horizontal velosity
khoshino 2:30f96c159d9c 79 float xs1 = 0;
khoshino 2:30f96c159d9c 80 float ys1 = 0;
khoshino 2:30f96c159d9c 81 float xr = 0;
khoshino 2:30f96c159d9c 82 float yr = 0;
khoshino 2:30f96c159d9c 83 float drc = 0;
khoshino 2:30f96c159d9c 84 float dr = 0;
khoshino 2:30f96c159d9c 85 float dt = 0.1;
khoshino 2:30f96c159d9c 86 float psi = 1;
khoshino 2:30f96c159d9c 87 float s = 0;
khoshino 2:30f96c159d9c 88
khoshino 2:30f96c159d9c 89 //homotopy
khoshino 2:30f96c159d9c 90 float drf = 800.0;
khoshino 2:30f96c159d9c 91 float rps0 = 90.0;
khoshino 2:30f96c159d9c 92 float rxi0 = -600.0;
khoshino 2:30f96c159d9c 93 float ret0 = -200.0;
khoshino 2:30f96c159d9c 94 float ps0 = rps0*dtr;
khoshino 2:30f96c159d9c 95 float xi0 = rxi0/drf;
khoshino 2:30f96c159d9c 96 float et0 = ret0/drf;
khoshino 2:30f96c159d9c 97 float eps = 0.00000001;
khoshino 2:30f96c159d9c 98 float ueps = 0.000001;
khoshino 2:30f96c159d9c 99 float ier = 0.0;
khoshino 2:30f96c159d9c 100 float sum = 0.0;
khoshino 2:30f96c159d9c 101 float ph0[mh] = {0.0};
khoshino 2:30f96c159d9c 102 float phi[mh] = {0.0};
khoshino 2:30f96c159d9c 103 float fn[mh] = {0.0};
khoshino 2:30f96c159d9c 104 float wk[mh] = {0.0};
khoshino 2:30f96c159d9c 105 int iwk[mh] = {0};
khoshino 2:30f96c159d9c 106 float dph1[mh] = {0.0};
khoshino 2:30f96c159d9c 107 float dph2[mh] = {0.0};
khoshino 2:30f96c159d9c 108 float dph3[mh] = {0.0};
khoshino 2:30f96c159d9c 109 float dph4[mh] = {0.0};
khoshino 2:30f96c159d9c 110 float fph[mh][mh] = {0.0};
khoshino 2:30f96c159d9c 111 float fpi[mh][mh] = {0.0};
khoshino 2:30f96c159d9c 112 float x1[4][mt] = {0.0};
khoshino 2:30f96c159d9c 113 float rl[4][mt] = {0.0};
khoshino 2:30f96c159d9c 114 float qmax=0.0;
khoshino 2:30f96c159d9c 115 float rdet =1.0;
khoshino 2:30f96c159d9c 116 float idet =0.0;
khoshino 2:30f96c159d9c 117 float t = 0.0;
khoshino 2:30f96c159d9c 118 float phf[mh]={0.0};
khoshino 2:30f96c159d9c 119 //********************* HOSHINO *********************
khoshino 2:30f96c159d9c 120
higedura 0:9aa9be24636a 121 FILE *fp = fopen("/sd/sdtest.txt", "w");
higedura 0:9aa9be24636a 122 if(fp == NULL) {
higedura 0:9aa9be24636a 123 error("Could not open file for write\n");
higedura 0:9aa9be24636a 124 }
higedura 0:9aa9be24636a 125
higedura 1:ca296cfa603b 126 while(1){
higedura 0:9aa9be24636a 127 //for(int j=0;j<1800;j++){ flag tukuru // for 2 minuts
higedura 0:9aa9be24636a 128
higedura 0:9aa9be24636a 129 // ******************** sequence No.1 ********************
higedura 0:9aa9be24636a 130 buf_char = mavc.getc();
higedura 0:9aa9be24636a 131 buf_hex[0] = (unsigned int)buf_char;
higedura 0:9aa9be24636a 132 for(int i=0;i<29;i++){ buf_hex[29-i] = buf_hex[28-i]; }
higedura 0:9aa9be24636a 133 // !!! switch bun ni suru
higedura 0:9aa9be24636a 134 if(buf_hex[29]==0x40 && buf_hex[28]==0x43 && buf_hex[27]==0x4D && buf_hex[26]==0x0D){
higedura 0:9aa9be24636a 135 // ******************** sequence No.1 ********************
higedura 0:9aa9be24636a 136
higedura 0:9aa9be24636a 137 // ******************** sequence No.2 ********************
higedura 0:9aa9be24636a 138 // send to mavc
higedura 0:9aa9be24636a 139 mavc.putc(H);
higedura 0:9aa9be24636a 140 for(int i=0;i<data;i++){ mavc.putc(D[i]); }
higedura 0:9aa9be24636a 141 //for(int i=0;i<9;i++){ mavc.putc(test[i]); }
higedura 0:9aa9be24636a 142 // ******************** sequence No.2 ********************
higedura 0:9aa9be24636a 143
higedura 0:9aa9be24636a 144 // ******************** sequence No.3 ********************
higedura 0:9aa9be24636a 145 // 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 146 // |header| AccX | AccY | AccZ | GyroX | GyroY | GyroZ | Azimuth | Altitude| GPSX | GPSY |
higedura 0:9aa9be24636a 147 // | H | D[1] | D[2] | D[3] | D[4] | D[5] | D[6] | D[7] | D[8] | D[9] | D[10] |
higedura 0:9aa9be24636a 148 // transrating hex to dec
higedura 0:9aa9be24636a 149 for(int i=0;i<10;i++){ buf_dec[i] = buf_hex[24-i*2]*256+buf_hex[23-i*2]; }
higedura 0:9aa9be24636a 150 // wakariyasuku surutame bunnkatsu, honnrai ha matrix ga yoi
higedura 0:9aa9be24636a 151 // -> saisyuteki niha matrix ni simasu?
higedura 0:9aa9be24636a 152 for(int i=0;i<3;i++){ acc[i] = ((double)buf_dec[i]-32768)/65535*20*9.8; }
higedura 0:9aa9be24636a 153 for(int i=0;i<3;i++){ gyro[i] = ((double)buf_dec[i+3]-32768)/65535*600/180*pi; }
higedura 0:9aa9be24636a 154 azi = (double)buf_dec[6]/65535*2*pi;
higedura 0:9aa9be24636a 155 alt = ((double)buf_dec[7]-32768)*0.1;
higedura 0:9aa9be24636a 156 for(int i=0;i<2;i++){ GPS[i] = ((double)buf_dec[i+8]-32768)*0.1; }
higedura 0:9aa9be24636a 157 // ******************** sequence No.3 ********************
higedura 0:9aa9be24636a 158
khoshino 2:30f96c159d9c 159 // ******************** initialization *******************
khoshino 2:30f96c159d9c 160 if(t == 0){
khoshino 2:30f96c159d9c 161 x0 = GPS[0];
khoshino 2:30f96c159d9c 162 y0 = GPS[1];
khoshino 2:30f96c159d9c 163 psi0= azi;
khoshino 2:30f96c159d9c 164 }
khoshino 2:30f96c159d9c 165 xs = GPS[0] - x0;
khoshino 2:30f96c159d9c 166 ys = GPS[1] - y0;
khoshino 2:30f96c159d9c 167 psis= azi - psi0;
khoshino 2:30f96c159d9c 168 // ******************** initialization *******************
khoshino 2:30f96c159d9c 169
khoshino 2:30f96c159d9c 170 // ******************** interpolation ********************
khoshino 2:30f96c159d9c 171 dr = dr + ua * dt;
khoshino 2:30f96c159d9c 172 xr = xr + ua * dt * cos(psi);
khoshino 2:30f96c159d9c 173 yr = yr + ua * dt * sin(psi);
khoshino 2:30f96c159d9c 174
khoshino 2:30f96c159d9c 175 if (xs != xs1 || fabs(xs-xs1)<50) {
khoshino 2:30f96c159d9c 176 s = (xs-xs1)*(xs-xs1)+(ys-ys1)*(ys-ys1);
khoshino 2:30f96c159d9c 177 ua = 0.5*(ua+sqrt(s));
khoshino 2:30f96c159d9c 178 drc = drc+ua;
khoshino 2:30f96c159d9c 179 dr= drc;
khoshino 2:30f96c159d9c 180 xr=xs;
khoshino 2:30f96c159d9c 181 yr=ys;
khoshino 2:30f96c159d9c 182 }
khoshino 2:30f96c159d9c 183 xs1=xs;
khoshino 2:30f96c159d9c 184 ys1=ys;
khoshino 2:30f96c159d9c 185 // ******************** interpolation ********************
khoshino 2:30f96c159d9c 186
khoshino 2:30f96c159d9c 187 // ******************** guidance law WO homo *************
khoshino 2:30f96c159d9c 188 hr = b0 + b1*x + b2*x*x + b3*x*x*x;
khoshino 2:30f96c159d9c 189
khoshino 2:30f96c159d9c 190 /*
khoshino 2:30f96c159d9c 191 c = drp-dr[i];
khoshino 2:30f96c159d9c 192 c1= dr[i+1]-drp;
khoshino 2:30f96c159d9c 193
khoshino 2:30f96c159d9c 194 if(c >=0 && c1 >= 0)
khoshino 2:30f96c159d9c 195 {
khoshino 2:30f96c159d9c 196 c0=c/(dr[i+1]-dr[i]);
khoshino 2:30f96c159d9c 197 c1=c1/(dr[i+1]-dr[i]);
khoshino 2:30f96c159d9c 198 psc=c0*ps[i+1]+c1*ps[i];
khoshino 2:30f96c159d9c 199 xic=c0*xi[i+1]+c1*xi[i];
khoshino 2:30f96c159d9c 200 etc=c0*et[i+1]+c1*et[i];
khoshino 2:30f96c159d9c 201 break;
khoshino 2:30f96c159d9c 202 }
khoshino 2:30f96c159d9c 203 */
khoshino 2:30f96c159d9c 204
khoshino 2:30f96c159d9c 205
khoshino 2:30f96c159d9c 206 // ******************** guidance law WO homo *************
khoshino 2:30f96c159d9c 207
khoshino 2:30f96c159d9c 208 // ******************** control law **********************
higedura 0:9aa9be24636a 209 // transrating dec to hex
higedura 0:9aa9be24636a 210 //com
khoshino 2:30f96c159d9c 211 // ******************** control law **********************
higedura 0:9aa9be24636a 212
higedura 1:ca296cfa603b 213 //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]);
higedura 1:ca296cfa603b 214 //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]);
higedura 0:9aa9be24636a 215
higedura 0:9aa9be24636a 216 }
khoshino 2:30f96c159d9c 217 time = time + 0.1;
higedura 0:9aa9be24636a 218 }
higedura 0:9aa9be24636a 219
higedura 0:9aa9be24636a 220 fclose(fp);
higedura 0:9aa9be24636a 221 led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura 0:9aa9be24636a 222
higedura 0:9aa9be24636a 223 }