Kiyoteru Hayama / Mbed 2 deprecated GPSRobotCar_141114

This program is made for the GPS Robot Car contest 2014

Dependencies:   Servo TextLCD USBHost mbed

/media/uploads/hayama/gpsrobotcar-small.jpg

Presentation for Geospatial EXPO 2014. /media/uploads/hayama/presen.pdf

temp.cpp@0:c2262bac9aa6, 2014-11-13 (annotated)

Committer:
hayama
Date:
Thu Nov 13 15:33:47 2014 +0000
Revision:
0:c2262bac9aa6
GPSrobotCar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hayama 0:c2262bac9aa6 1
hayama 0:c2262bac9aa6 2
hayama 0:c2262bac9aa6 3 //case 7: QZWPWrite(); break;
hayama 0:c2262bac9aa6 4 /*
hayama 0:c2262bac9aa6 5
hayama 0:c2262bac9aa6 6 // file out QZSS way point, 4 point only
hayama 0:c2262bac9aa6 7 void writeQZWp4(){
hayama 0:c2262bac9aa6 8 FILE *fp = fopen("/local/QZpoint4.txt","w");
hayama 0:c2262bac9aa6 9 fprintf(fp, "%f\n",wyQ[1][1]);
hayama 0:c2262bac9aa6 10 fprintf(fp, "%f\n",wxQ[1][1]);
hayama 0:c2262bac9aa6 11 fprintf(fp, "%f\n",wyQ[7][1]);
hayama 0:c2262bac9aa6 12 fprintf(fp, "%f\n",wxQ[7][1]);
hayama 0:c2262bac9aa6 13 fprintf(fp, "%f\n",wyQ[1][7]);
hayama 0:c2262bac9aa6 14 fprintf(fp, "%f\n",wxQ[1][7]);
hayama 0:c2262bac9aa6 15 fprintf(fp, "%f\n",wyQ[7][7]);
hayama 0:c2262bac9aa6 16 fprintf(fp, "%f\n",wxQ[7][7]);
hayama 0:c2262bac9aa6 17 fclose(fp);
hayama 0:c2262bac9aa6 18 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 19 Thread::wait(1000);
hayama 0:c2262bac9aa6 20 }
hayama 0:c2262bac9aa6 21
hayama 0:c2262bac9aa6 22 // file read QZSS way point, 4 point only
hayama 0:c2262bac9aa6 23 void readQZWp4(){
hayama 0:c2262bac9aa6 24 char st[20];
hayama 0:c2262bac9aa6 25 FILE *fp = fopen("/local/QZpoint4.txt","r");
hayama 0:c2262bac9aa6 26 fgets(st,20,fp); wyQ[1][1]=atof(st);
hayama 0:c2262bac9aa6 27 fgets(st,20,fp); wxQ[1][1]=atof(st);
hayama 0:c2262bac9aa6 28 fgets(st,20,fp); wyQ[7][1]=atof(st);
hayama 0:c2262bac9aa6 29 fgets(st,20,fp); wxQ[7][1]=atof(st);
hayama 0:c2262bac9aa6 30 fgets(st,20,fp); wyQ[1][7]=atof(st);
hayama 0:c2262bac9aa6 31 fgets(st,20,fp); wxQ[1][7]=atof(st);
hayama 0:c2262bac9aa6 32 fgets(st,20,fp); wyQ[7][7]=atof(st);
hayama 0:c2262bac9aa6 33 fgets(st,20,fp); wxQ[7][7]=atof(st);
hayama 0:c2262bac9aa6 34 fclose(fp);
hayama 0:c2262bac9aa6 35 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 36 Thread::wait(1000);
hayama 0:c2262bac9aa6 37 calcQZWp();
hayama 0:c2262bac9aa6 38 }
hayama 0:c2262bac9aa6 39
hayama 0:c2262bac9aa6 40 */
hayama 0:c2262bac9aa6 41
hayama 0:c2262bac9aa6 42
hayama 0:c2262bac9aa6 43 /*
hayama 0:c2262bac9aa6 44 // calculation of all QZSS waypoint from 4 point, old version
hayama 0:c2262bac9aa6 45 void calcQZWp(){
hayama 0:c2262bac9aa6 46
hayama 0:c2262bac9aa6 47 // xQ11=-0.7057; yQ11=3.5358;
hayama 0:c2262bac9aa6 48 // xQ71=7.78; yQ71=12.01;
hayama 0:c2262bac9aa6 49 // xQ17=-13.42; yQ17=16.26;
hayama 0:c2262bac9aa6 50
hayama 0:c2262bac9aa6 51 xQ11=wxQ[1][1]; yQ11=wyQ[1][1]; // xQ11=wxQ11(A1,B1), yQ11=wyQ11(A1,B1)
hayama 0:c2262bac9aa6 52 xQ71=wxQ[7][1]; yQ71=wyQ[7][1]; // xQ71=wxQ71(A7,B1), yQ71=wyQ71(A7,B1)
hayama 0:c2262bac9aa6 53 xQ17=wxQ[1][7]; yQ17=wyQ[1][7]; // xQ17=wxQ17(A1,B7), yQ17=wyQ17(A1,B7)
hayama 0:c2262bac9aa6 54 xQ77=wxQ[7][7]; yQ77=wyQ[7][7];
hayama 0:c2262bac9aa6 55
hayama 0:c2262bac9aa6 56 dxQ71=xQ71-xQ11; dyQ71=yQ71-yQ11;
hayama 0:c2262bac9aa6 57 dxQ17=xQ17-xQ11; dyQ17=yQ17-yQ11;
hayama 0:c2262bac9aa6 58
hayama 0:c2262bac9aa6 59 theta=atan(dxQ71/dyQ71);
hayama 0:c2262bac9aa6 60 magx=sqrt(dxQ71*dxQ71+dyQ71*dyQ71)/6;
hayama 0:c2262bac9aa6 61 magy=sqrt(dxQ17*dxQ17+dyQ17*dyQ17)/6;
hayama 0:c2262bac9aa6 62
hayama 0:c2262bac9aa6 63 for (int i=1;i<8;i++){
hayama 0:c2262bac9aa6 64 for(int j=1;j<8;j++){
hayama 0:c2262bac9aa6 65 wxQ[i][j]=magx*i*cos(theta)-magy*j*sin(theta);
hayama 0:c2262bac9aa6 66 wyQ[i][j]=magx*i*sin(theta)+magy*j*cos(theta);
hayama 0:c2262bac9aa6 67 }
hayama 0:c2262bac9aa6 68 }
hayama 0:c2262bac9aa6 69 }
hayama 0:c2262bac9aa6 70 */
hayama 0:c2262bac9aa6 71
hayama 0:c2262bac9aa6 72 /*
hayama 0:c2262bac9aa6 73 // QZSS wp wirte
hayama 0:c2262bac9aa6 74 void QZWPWrite(){
hayama 0:c2262bac9aa6 75 int i;
hayama 0:c2262bac9aa6 76 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 77 wxQ[i][1]= 47.6213;
hayama 0:c2262bac9aa6 78 }
hayama 0:c2262bac9aa6 79 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 80 wxQ[i][2]= 47.621483;
hayama 0:c2262bac9aa6 81 }
hayama 0:c2262bac9aa6 82 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 83 wxQ[i][3]= 47.621666;
hayama 0:c2262bac9aa6 84 }
hayama 0:c2262bac9aa6 85 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 86 wxQ[i][4]= 47.62185;
hayama 0:c2262bac9aa6 87 }
hayama 0:c2262bac9aa6 88 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 89 wxQ[i][5]= 47.622033;
hayama 0:c2262bac9aa6 90 }
hayama 0:c2262bac9aa6 91 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 92 wxQ[i][6]= 47.622166;
hayama 0:c2262bac9aa6 93 }
hayama 0:c2262bac9aa6 94 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 95 wxQ[i][7]= 47.6224;
hayama 0:c2262bac9aa6 96 }
hayama 0:c2262bac9aa6 97
hayama 0:c2262bac9aa6 98 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 99 wyQ[1][i]= 39.9302;
hayama 0:c2262bac9aa6 100 }
hayama 0:c2262bac9aa6 101 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 102 wyQ[2][i]= 39.39183;
hayama 0:c2262bac9aa6 103 }
hayama 0:c2262bac9aa6 104 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 105 wyQ[3][i]= 39.93347;
hayama 0:c2262bac9aa6 106 }
hayama 0:c2262bac9aa6 107 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 108 wyQ[4][i]= 39.9351;
hayama 0:c2262bac9aa6 109 }
hayama 0:c2262bac9aa6 110 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 111 wyQ[5][i]= 39.93673;
hayama 0:c2262bac9aa6 112 }
hayama 0:c2262bac9aa6 113 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 114 wyQ[6][i]= 39.93837;
hayama 0:c2262bac9aa6 115 }
hayama 0:c2262bac9aa6 116 for(i=1;i<8;i++){
hayama 0:c2262bac9aa6 117 wxQ[7][i]= 39.94;
hayama 0:c2262bac9aa6 118 }
hayama 0:c2262bac9aa6 119 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 120 Thread::wait(1000);
hayama 0:c2262bac9aa6 121 }
hayama 0:c2262bac9aa6 122
hayama 0:c2262bac9aa6 123 */
hayama 0:c2262bac9aa6 124
hayama 0:c2262bac9aa6 125
hayama 0:c2262bac9aa6 126 /*
hayama 0:c2262bac9aa6 127 void countR(){
hayama 0:c2262bac9aa6 128 float tmp;
hayama 0:c2262bac9aa6 129 cntR++;
hayama 0:c2262bac9aa6 130 tmp=timer.read_ms();
hayama 0:c2262bac9aa6 131 spdR=30/(tmp-timR); // m/s ?
hayama 0:c2262bac9aa6 132 timR=tmp;
hayama 0:c2262bac9aa6 133 }
hayama 0:c2262bac9aa6 134 void countL(){
hayama 0:c2262bac9aa6 135 float tmp;
hayama 0:c2262bac9aa6 136 cntL++;
hayama 0:c2262bac9aa6 137 tmp=timer.read_ms();
hayama 0:c2262bac9aa6 138 spdL=30/(tmp-timL); // m/s ?
hayama 0:c2262bac9aa6 139 timL=tmp;
hayama 0:c2262bac9aa6 140 }
hayama 0:c2262bac9aa6 141
hayama 0:c2262bac9aa6 142 */
hayama 0:c2262bac9aa6 143
hayama 0:c2262bac9aa6 144 /*
hayama 0:c2262bac9aa6 145 #define DEF_WR 2 // ウェイポイントの半径
hayama 0:c2262bac9aa6 146 #define DEF_KS 0.1 // ステアリング切れ角比例係数
hayama 0:c2262bac9aa6 147 #define DEF_STRMIN 0.2 // ステアリング左最大切れ角
hayama 0:c2262bac9aa6 148 #define DEF_STRMAX 0.8 // ステアリング右最大切れ角
hayama 0:c2262bac9aa6 149 #define DEF_STRN 0.5 // ステアリング停止時の中立値
hayama 0:c2262bac9aa6 150 #define DEF_SPD 0.2 // 走行時のモータの値
hayama 0:c2262bac9aa6 151
hayama 0:c2262bac9aa6 152 // resetを押しながらリセットをかけた場合は，パラメータをデフォルト値に戻す
hayama 0:c2262bac9aa6 153 if (resetSW==1){
hayama 0:c2262bac9aa6 154 readParam();
hayama 0:c2262bac9aa6 155 } else { // パラメータをデフォルト値に戻す
hayama 0:c2262bac9aa6 156 wr=DEF_WR;
hayama 0:c2262bac9aa6 157 ks=DEF_KS;
hayama 0:c2262bac9aa6 158 strmin=DEF_STRMIN;
hayama 0:c2262bac9aa6 159 strmax=DEF_STRMAX;
hayama 0:c2262bac9aa6 160 strn=DEF_STRN;
hayama 0:c2262bac9aa6 161 spd=DEF_SPD;
hayama 0:c2262bac9aa6 162 tNum=DEF_TNUM;
hayama 0:c2262bac9aa6 163 writeParam();
hayama 0:c2262bac9aa6 164 }
hayama 0:c2262bac9aa6 165
hayama 0:c2262bac9aa6 166 */
hayama 0:c2262bac9aa6 167
hayama 0:c2262bac9aa6 168
hayama 0:c2262bac9aa6 169 //long wp=4; // wp:0-15まで,ウェイポイントの数 -1
hayama 0:c2262bac9aa6 170 //double wy[]={0,111.111, 222.222, 333.333};
hayama 0:c2262bac9aa6 171 //double wx[]={0,123.456, 234.567, 345.678};
hayama 0:c2262bac9aa6 172
hayama 0:c2262bac9aa6 173 /*
hayama 0:c2262bac9aa6 174 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 175 // ウェイポイントの読み出し、表示。GPSのジャンパーを外して、シリアルポートに接続
hayama 0:c2262bac9aa6 176 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 177 void wpOut(){
hayama 0:c2262bac9aa6 178 while(resetSW==1){
hayama 0:c2262bac9aa6 179 pc.printf("wp= %d\n",wp);
hayama 0:c2262bac9aa6 180 for(int i=0;i<16;i++){
hayama 0:c2262bac9aa6 181 pc.printf("%d, %f, %f\n",i, wy[i],wx[i]);
hayama 0:c2262bac9aa6 182 }
hayama 0:c2262bac9aa6 183 Thread::wait(1000);
hayama 0:c2262bac9aa6 184 }
hayama 0:c2262bac9aa6 185 }
hayama 0:c2262bac9aa6 186
hayama 0:c2262bac9aa6 187 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 188 // パラメータのシリアル出力
hayama 0:c2262bac9aa6 189 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 190 void paramOut(){
hayama 0:c2262bac9aa6 191 while(resetSW==1){
hayama 0:c2262bac9aa6 192 pc.printf("wr =%f\n", wr);
hayama 0:c2262bac9aa6 193 pc.printf("ks =%f\n", ks);
hayama 0:c2262bac9aa6 194 pc.printf("strmin=%f\n", strmin);
hayama 0:c2262bac9aa6 195 pc.printf("strmax=%f\n", strmax);
hayama 0:c2262bac9aa6 196 pc.printf("strn =%d\n", strn);
hayama 0:c2262bac9aa6 197 pc.printf("spd =%d\n", spd);
hayama 0:c2262bac9aa6 198 Thread::wait(1000);
hayama 0:c2262bac9aa6 199 }
hayama 0:c2262bac9aa6 200 }
hayama 0:c2262bac9aa6 201 */
hayama 0:c2262bac9aa6 202
hayama 0:c2262bac9aa6 203
hayama 0:c2262bac9aa6 204 //writePoint();
hayama 0:c2262bac9aa6 205
hayama 0:c2262bac9aa6 206 /*
hayama 0:c2262bac9aa6 207 printf("%d\n",wp);
hayama 0:c2262bac9aa6 208 for(int i=0;i<wp;i++){
hayama 0:c2262bac9aa6 209 printf("%f\n",wy[i]);
hayama 0:c2262bac9aa6 210 printf("%f\n",wx[i]);
hayama 0:c2262bac9aa6 211 }
hayama 0:c2262bac9aa6 212 */
hayama 0:c2262bac9aa6 213
hayama 0:c2262bac9aa6 214 /*
hayama 0:c2262bac9aa6 215 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 216 // シリアルポートで文字列受信
hayama 0:c2262bac9aa6 217 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 218 void recvStr(char *buf)
hayama 0:c2262bac9aa6 219 {
hayama 0:c2262bac9aa6 220 int i = 0;
hayama 0:c2262bac9aa6 221 char c;
hayama 0:c2262bac9aa6 222 while (1) {
hayama 0:c2262bac9aa6 223 if (Serial.available()) {
hayama 0:c2262bac9aa6 224 c = Serial.read();
hayama 0:c2262bac9aa6 225 buf[i] = c;
hayama 0:c2262bac9aa6 226 if (c == '\n') break;
hayama 0:c2262bac9aa6 227 i++;
hayama 0:c2262bac9aa6 228 }
hayama 0:c2262bac9aa6 229 }
hayama 0:c2262bac9aa6 230 buf[i] = '\0'; // \0: end of string
hayama 0:c2262bac9aa6 231 }
hayama 0:c2262bac9aa6 232
hayama 0:c2262bac9aa6 233
hayama 0:c2262bac9aa6 234 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 235 // GPS受信
hayama 0:c2262bac9aa6 236 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 237 int recvGPS(){
hayama 0:c2262bac9aa6 238 if (Serial.available()) {
hayama 0:c2262bac9aa6 239 recvStr(str);
hayama 0:c2262bac9aa6 240 if(strcmp(strtok(str,","),"$GPRMC")==0){ //if RMC line
hayama 0:c2262bac9aa6 241 strtok(NULL,",");
hayama 0:c2262bac9aa6 242 strtok(NULL,",");
hayama 0:c2262bac9aa6 243 latitude=strtok(NULL,","); //get latitude
hayama 0:c2262bac9aa6 244 strtok(NULL,",");
hayama 0:c2262bac9aa6 245 longitude=strtok(NULL,","); //get longtude
hayama 0:c2262bac9aa6 246 strtok(NULL,","); // E読み飛ばし
hayama 0:c2262bac9aa6 247 knot=strtok(NULL,","); // 速度 get
hayama 0:c2262bac9aa6 248 direct=strtok(NULL,","); // 進行方向 get
hayama 0:c2262bac9aa6 249
hayama 0:c2262bac9aa6 250 gps_val(); // GPS信号の数値返還
hayama 0:c2262bac9aa6 251 gps_cal(); // 現在地とウェイポイントとの関係を計算
hayama 0:c2262bac9aa6 252 return(1);
hayama 0:c2262bac9aa6 253 }
hayama 0:c2262bac9aa6 254 }
hayama 0:c2262bac9aa6 255 return(0);
hayama 0:c2262bac9aa6 256 }
hayama 0:c2262bac9aa6 257 */
hayama 0:c2262bac9aa6 258
hayama 0:c2262bac9aa6 259
hayama 0:c2262bac9aa6 260 //lcd.cls();
hayama 0:c2262bac9aa6 261 //lcd.printf("%f \n", latit);
hayama 0:c2262bac9aa6 262 //lcd.printf("%f \n", longit);
hayama 0:c2262bac9aa6 263 //lcd.printf("%f %f", latit, longit);
hayama 0:c2262bac9aa6 264 //printf("%s\n", str);
hayama 0:c2262bac9aa6 265
hayama 0:c2262bac9aa6 266 /*
hayama 0:c2262bac9aa6 267 while(1){
hayama 0:c2262bac9aa6 268 if(SW1==0){
hayama 0:c2262bac9aa6 269 lcd.cls();
hayama 0:c2262bac9aa6 270 lcd.printf("#S1# ---- ----");
hayama 0:c2262bac9aa6 271 servoC1 = POSC;
hayama 0:c2262bac9aa6 272 servoC2 = POSC;
hayama 0:c2262bac9aa6 273 }
hayama 0:c2262bac9aa6 274 else if(SW2==0){
hayama 0:c2262bac9aa6 275 lcd.cls();
hayama 0:c2262bac9aa6 276 lcd.printf("---- #S2# ----");
hayama 0:c2262bac9aa6 277 servoC1 = POSC+0.2;
hayama 0:c2262bac9aa6 278 servoC2 = POSC+0.2;
hayama 0:c2262bac9aa6 279 }
hayama 0:c2262bac9aa6 280 if(SW3==0){
hayama 0:c2262bac9aa6 281 lcd.cls();
hayama 0:c2262bac9aa6 282 lcd.printf("---- ---- #S3#");
hayama 0:c2262bac9aa6 283 servoC1 = POSC-0.2;
hayama 0:c2262bac9aa6 284 servoC2 = POSC-0.2;
hayama 0:c2262bac9aa6 285 }
hayama 0:c2262bac9aa6 286 // for usb
hayama 0:c2262bac9aa6 287 // led=!led;
hayama 0:c2262bac9aa6 288 // Thread::wait(500);
hayama 0:c2262bac9aa6 289 }
hayama 0:c2262bac9aa6 290 */
hayama 0:c2262bac9aa6 291
hayama 0:c2262bac9aa6 292
hayama 0:c2262bac9aa6 293 /*
hayama 0:c2262bac9aa6 294 void serial_task(void const*) {
hayama 0:c2262bac9aa6 295 USBHostSerial serial;
hayama 0:c2262bac9aa6 296
hayama 0:c2262bac9aa6 297 while(1) {
hayama 0:c2262bac9aa6 298
hayama 0:c2262bac9aa6 299 // try to connect a serial device
hayama 0:c2262bac9aa6 300 while(!serial.connect())
hayama 0:c2262bac9aa6 301 Thread::wait(500);
hayama 0:c2262bac9aa6 302
hayama 0:c2262bac9aa6 303 // in a loop, print all characters received
hayama 0:c2262bac9aa6 304 // if the device is disconnected, we try to connect it again
hayama 0:c2262bac9aa6 305 while (1) {
hayama 0:c2262bac9aa6 306
hayama 0:c2262bac9aa6 307 // if device disconnected, try to connect it again
hayama 0:c2262bac9aa6 308 if (!serial.connected())
hayama 0:c2262bac9aa6 309 break;
hayama 0:c2262bac9aa6 310
hayama 0:c2262bac9aa6 311 // print characters received
hayama 0:c2262bac9aa6 312 while (serial.available()) {
hayama 0:c2262bac9aa6 313 printf("%c", serial.getc());
hayama 0:c2262bac9aa6 314 }
hayama 0:c2262bac9aa6 315
hayama 0:c2262bac9aa6 316 Thread::wait(50);
hayama 0:c2262bac9aa6 317 }
hayama 0:c2262bac9aa6 318 }
hayama 0:c2262bac9aa6 319 }
hayama 0:c2262bac9aa6 320 */