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

main.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 #include "mbed.h"
hayama 0:c2262bac9aa6 2 #include "TextLCD.h"
hayama 0:c2262bac9aa6 3 #include "Servo.h"
hayama 0:c2262bac9aa6 4 #include "USBHostSerial.h"
hayama 0:c2262bac9aa6 5
hayama 0:c2262bac9aa6 6 #define POSC 0.5
hayama 0:c2262bac9aa6 7 #define KANG 5 // factor of gyro output to angle rot right + val
hayama 0:c2262bac9aa6 8 #define GYROADJ (-0.0001)
hayama 0:c2262bac9aa6 9 #define FSMTHS 1 // smoothing factor for sensors
hayama 0:c2262bac9aa6 10
hayama 0:c2262bac9aa6 11 // 各種定数または初期値の設定
hayama 0:c2262bac9aa6 12 #define PI 3.14159 // pai
hayama 0:c2262bac9aa6 13 #define DEF_TNUM 1
hayama 0:c2262bac9aa6 14
hayama 0:c2262bac9aa6 15 // メニュー関係
hayama 0:c2262bac9aa6 16 int nmenu=36;
hayama 0:c2262bac9aa6 17 const char *menuS[36]={ // メニューの文字列
hayama 0:c2262bac9aa6 18 "Disp GPS ","Set way point ","Disp way point ","Clear way point ","Read way point ",
hayama 0:c2262bac9aa6 19 "Write way point ","Set team number ","Set QZSS Wp ","Read QZSS Wp ","Write QZSS Wp ",
hayama 0:c2262bac9aa6 20 "Disp QZSS Msg ","Clear QZSS Msg ","Read QZSS Msg ","Write QZSS Msg ","Run GPS no Gyro ",
hayama 0:c2262bac9aa6 21 "Run GPS use Gyro","Run QZSS no Gyro","Run QZSS use Gyr","Test servo ","Test motor ",
hayama 0:c2262bac9aa6 22 "Init neutral ","Init steer min ","Set steer max ","Init WP radius ","Init Ks ",
hayama 0:c2262bac9aa6 23 "Init KsGyro ","Init PosH ","Init PosL ","Init Speed H ","Init Speed L ",
hayama 0:c2262bac9aa6 24 "Test Run ","Write parameter ","Read Parameter ","Test encoder ","Test Gyro ",
hayama 0:c2262bac9aa6 25 "Test Compus "
hayama 0:c2262bac9aa6 26 };
hayama 0:c2262bac9aa6 27
hayama 0:c2262bac9aa6 28 const double arrayCompus[]={180,225,45,0,135,270,90,315};
hayama 0:c2262bac9aa6 29 volatile double dirCompus;
hayama 0:c2262bac9aa6 30 double rdCpy, dirCompusCpy;
hayama 0:c2262bac9aa6 31
hayama 0:c2262bac9aa6 32 int pmode=0;
hayama 0:c2262bac9aa6 33 volatile int recvGPS=0;
hayama 0:c2262bac9aa6 34 volatile int recvQZSS=0;
hayama 0:c2262bac9aa6 35 volatile long encR=0, encRB=0;
hayama 0:c2262bac9aa6 36 volatile long encL=0, encLB=0;
hayama 0:c2262bac9aa6 37 volatile int cntR=0;
hayama 0:c2262bac9aa6 38 volatile int cntL=0;
hayama 0:c2262bac9aa6 39 volatile int cntC=0; // center both cntR and cntL
hayama 0:c2262bac9aa6 40 volatile int cntE=0; // timing counter for cntR/L read
hayama 0:c2262bac9aa6 41
hayama 0:c2262bac9aa6 42 // gyro sensor
hayama 0:c2262bac9aa6 43 volatile float gyro, gyItg, gyOff;
hayama 0:c2262bac9aa6 44 volatile float angle;
hayama 0:c2262bac9aa6 45
hayama 0:c2262bac9aa6 46 // ウェイポイント設定(基準点からのN,E方向への座標)、最高16ポイント
hayama 0:c2262bac9aa6 47 int np=0; // soukoutyuu目標ポイントナンバー
hayama 0:c2262bac9aa6 48 int wp=0; // number of waypoint, initial value read from file of "waypoint.txt"
hayama 0:c2262bac9aa6 49 double wy[16]={0}; // latitude of waypoints
hayama 0:c2262bac9aa6 50 double wx[16]={0}; // longitude of waypoints
hayama 0:c2262bac9aa6 51
hayama 0:c2262bac9aa6 52 // QZSS-ウェイポイント設定(基準点からのN,E方向への座標)、最高16ポイント
hayama 0:c2262bac9aa6 53 int npQ=0; // 目標ポイントナンバー
hayama 0:c2262bac9aa6 54 int wpQ=0; // wp:0-15まで,ウェイポイントの数 -1
hayama 0:c2262bac9aa6 55 double wyQ11,wxQ11;
hayama 0:c2262bac9aa6 56 double wyQ[8][8]={0}; // index 1 to 7, wyQ[A],[B]
hayama 0:c2262bac9aa6 57 double wxQ[8][8]={0}; // index 1 to 7, wxQ[A],[B]
hayama 0:c2262bac9aa6 58
hayama 0:c2262bac9aa6 59 double xQ11,yQ11,xQ71,yQ71,xQ17,yQ17,xQ77,yQ77,dxQ71,dyQ71,dxQ17,dyQ17;
hayama 0:c2262bac9aa6 60 double theta;
hayama 0:c2262bac9aa6 61 double magx,magy;
hayama 0:c2262bac9aa6 62
hayama 0:c2262bac9aa6 63
hayama 0:c2262bac9aa6 64 // 走行処理関係変数
hayama 0:c2262bac9aa6 65 double gy, gx; // 北緯，東経を原点からの距離（メートル）に数値化
hayama 0:c2262bac9aa6 66 double ry, rx, rv, rd; // 北緯（正規化），東経（正規化），速度（ノット），方向（単位：度）の数値化
hayama 0:c2262bac9aa6 67 double dy, dx, dr, dd; // ウェイポイントに対する北緯，東経の差分(m)，距離の差分，方位の差分
hayama 0:c2262bac9aa6 68 double ddCpy; // 走行中にddの保存
hayama 0:c2262bac9aa6 69
hayama 0:c2262bac9aa6 70 double wr; // ウェイポイントの半径
hayama 0:c2262bac9aa6 71 double ks; // ステアリング切れ角比例係数
hayama 0:c2262bac9aa6 72 double ksg; // ステアリング切れ角比例係数, with gyro
hayama 0:c2262bac9aa6 73
hayama 0:c2262bac9aa6 74 float strn; // ニュートラルの位置
hayama 0:c2262bac9aa6 75 float posSteer; // ステアリングの位置
hayama 0:c2262bac9aa6 76 float strmin; // ステアリングの最小値
hayama 0:c2262bac9aa6 77 float strmax; // ステアリングの最大値
hayama 0:c2262bac9aa6 78 float posH, posL; // servo position for high/low speed of motor
hayama 0:c2262bac9aa6 79 int spdH,spdL; // スピード, count of encoder per 100ms
hayama 0:c2262bac9aa6 80
hayama 0:c2262bac9aa6 81
hayama 0:c2262bac9aa6 82 // GPS受信用変数，ポインタ
hayama 0:c2262bac9aa6 83 char str[200]; // GPSほか読み取りのための文字列バッファ
hayama 0:c2262bac9aa6 84 char *header;
hayama 0:c2262bac9aa6 85 char *latitude, *longitude, *knot, *direct;
hayama 0:c2262bac9aa6 86 double latit, longit, kn; // 緯度，経度，ノット保存用の変数
hayama 0:c2262bac9aa6 87 double latitB, longitB, rdB; // 差分で方位計算する時に前の座標保持用, 方位保管用
hayama 0:c2262bac9aa6 88 double dy2, dx2, rd2; // 一つ前の値との北緯，東経の差分(m)，差分で求めた方位
hayama 0:c2262bac9aa6 89
hayama 0:c2262bac9aa6 90 // QZSS
hayama 0:c2262bac9aa6 91 char strtmp[10];
hayama 0:c2262bac9aa6 92 char *messages;
hayama 0:c2262bac9aa6 93 long teamNum,year,numA,sizeA,numB,sizeB,numT;
hayama 0:c2262bac9aa6 94 long startA, startB;
hayama 0:c2262bac9aa6 95 long targetA[20];
hayama 0:c2262bac9aa6 96 long targetB[20];
hayama 0:c2262bac9aa6 97 long tNum=0;
hayama 0:c2262bac9aa6 98
hayama 0:c2262bac9aa6 99 InterruptIn ptR(p5);
hayama 0:c2262bac9aa6 100 InterruptIn ptL(p6);
hayama 0:c2262bac9aa6 101 //DigitalIn S3(p7)-S4(p8);
hayama 0:c2262bac9aa6 102
hayama 0:c2262bac9aa6 103 DigitalIn resetSW(p13);
hayama 0:c2262bac9aa6 104 DigitalIn setSW(p14);
hayama 0:c2262bac9aa6 105 DigitalIn startSW(p9);
hayama 0:c2262bac9aa6 106 DigitalOut led(p21);
hayama 0:c2262bac9aa6 107
hayama 0:c2262bac9aa6 108 AnalogIn gyroOut(p15);
hayama 0:c2262bac9aa6 109 //AnalogIn A2(p16)-(p20);
hayama 0:c2262bac9aa6 110
hayama 0:c2262bac9aa6 111 Servo strServo(p26);
hayama 0:c2262bac9aa6 112 Servo motorAmp(p25);
hayama 0:c2262bac9aa6 113 //Servo servoC3(p24)-(p21);
hayama 0:c2262bac9aa6 114
hayama 0:c2262bac9aa6 115 BusIn compus(p18, p19, p20); // compus, 3bit 8 direction
hayama 0:c2262bac9aa6 116 BusOut leds( LED3, LED2, LED1); // for LED display
hayama 0:c2262bac9aa6 117 TextLCD lcd(p11, p12, p27, p28, p29, p30); // rs, e, d4-d7
hayama 0:c2262bac9aa6 118 Ticker timerS; // defince interval timer
hayama 0:c2262bac9aa6 119 Timer timer;
hayama 0:c2262bac9aa6 120 Serial pc(USBTX, USBRX);
hayama 0:c2262bac9aa6 121 LocalFileSystem local("local");
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 writeParam(){
hayama 0:c2262bac9aa6 128 FILE *fp = fopen("/local/param.txt","w");
hayama 0:c2262bac9aa6 129 fprintf(fp, "%f\n",wr);
hayama 0:c2262bac9aa6 130 fprintf(fp, "%f\n",ks);
hayama 0:c2262bac9aa6 131 fprintf(fp, "%f\n",ksg);
hayama 0:c2262bac9aa6 132 fprintf(fp, "%f\n",strmin);
hayama 0:c2262bac9aa6 133 fprintf(fp, "%f\n",strmax);
hayama 0:c2262bac9aa6 134 fprintf(fp, "%f\n",strn);
hayama 0:c2262bac9aa6 135 fprintf(fp, "%f\n",posH);
hayama 0:c2262bac9aa6 136 fprintf(fp, "%f\n",posL);
hayama 0:c2262bac9aa6 137 fprintf(fp, "%d\n",spdH);
hayama 0:c2262bac9aa6 138 fprintf(fp, "%d\n",spdL);
hayama 0:c2262bac9aa6 139 fclose(fp);
hayama 0:c2262bac9aa6 140 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 141 Thread::wait(1000);
hayama 0:c2262bac9aa6 142 }
hayama 0:c2262bac9aa6 143
hayama 0:c2262bac9aa6 144 void readParam(){
hayama 0:c2262bac9aa6 145 char st[20];
hayama 0:c2262bac9aa6 146 FILE *fp = fopen("/local/param.txt","r");
hayama 0:c2262bac9aa6 147 fgets(st,20,fp); wr=atof(st);
hayama 0:c2262bac9aa6 148 fgets(st,20,fp); ks=atof(st);
hayama 0:c2262bac9aa6 149 fgets(st,20,fp); ksg=atof(st);
hayama 0:c2262bac9aa6 150 fgets(st,20,fp); strmin=atof(st);
hayama 0:c2262bac9aa6 151 fgets(st,20,fp); strmax=atof(st);
hayama 0:c2262bac9aa6 152 fgets(st,20,fp); strn=atof(st);
hayama 0:c2262bac9aa6 153 fgets(st,20,fp); posH=atof(st);
hayama 0:c2262bac9aa6 154 fgets(st,20,fp); posL=atof(st);
hayama 0:c2262bac9aa6 155 fgets(st,20,fp); spdH=atoi(st);
hayama 0:c2262bac9aa6 156 fgets(st,20,fp); spdL=atoi(st);
hayama 0:c2262bac9aa6 157 fclose(fp);
hayama 0:c2262bac9aa6 158 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 159 Thread::wait(1000);
hayama 0:c2262bac9aa6 160 }
hayama 0:c2262bac9aa6 161
hayama 0:c2262bac9aa6 162 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 163 // ウェイポイントの保管と読み出し
hayama 0:c2262bac9aa6 164 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 165 void writeWp(){
hayama 0:c2262bac9aa6 166 FILE *fp = fopen("/local/waypoint.txt","w");
hayama 0:c2262bac9aa6 167 fprintf(fp, "%d\n",wp);
hayama 0:c2262bac9aa6 168 for(int i=0;i<wp;i++){
hayama 0:c2262bac9aa6 169 fprintf(fp,"%f\n",wy[i]);
hayama 0:c2262bac9aa6 170 fprintf(fp,"%f\n",wx[i]);
hayama 0:c2262bac9aa6 171 }
hayama 0:c2262bac9aa6 172 fclose(fp);
hayama 0:c2262bac9aa6 173
hayama 0:c2262bac9aa6 174 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 175 Thread::wait(1000);
hayama 0:c2262bac9aa6 176 }
hayama 0:c2262bac9aa6 177
hayama 0:c2262bac9aa6 178 void readWp(){
hayama 0:c2262bac9aa6 179 char st[20];
hayama 0:c2262bac9aa6 180 FILE *fp = fopen("/local/waypoint.txt","r");
hayama 0:c2262bac9aa6 181 fgets(st,20,fp); wp=atof(st);
hayama 0:c2262bac9aa6 182 for(int i=0;i<wp;i++){
hayama 0:c2262bac9aa6 183 fgets(st,20,fp); wy[i]=atof(st);
hayama 0:c2262bac9aa6 184 fgets(st,20,fp); wx[i]=atof(st);
hayama 0:c2262bac9aa6 185 }
hayama 0:c2262bac9aa6 186 fclose(fp);
hayama 0:c2262bac9aa6 187
hayama 0:c2262bac9aa6 188 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 189 Thread::wait(1000);
hayama 0:c2262bac9aa6 190 }
hayama 0:c2262bac9aa6 191
hayama 0:c2262bac9aa6 192 // QZSS message file out
hayama 0:c2262bac9aa6 193 void writeQmsg(){
hayama 0:c2262bac9aa6 194 FILE *fp = fopen("/local/QZmsg.txt","w");
hayama 0:c2262bac9aa6 195 fprintf(fp, "%d\n",year);
hayama 0:c2262bac9aa6 196 fprintf(fp, "%d\n",teamNum);
hayama 0:c2262bac9aa6 197 fprintf(fp, "%d\n",numA);
hayama 0:c2262bac9aa6 198 fprintf(fp, "%d\n",sizeA);
hayama 0:c2262bac9aa6 199 fprintf(fp, "%d\n",numB);
hayama 0:c2262bac9aa6 200 fprintf(fp, "%d\n",sizeB);
hayama 0:c2262bac9aa6 201 fprintf(fp, "%d\n",numT);
hayama 0:c2262bac9aa6 202 fprintf(fp, "%d\n",startA);
hayama 0:c2262bac9aa6 203 fprintf(fp, "%d\n",startB);
hayama 0:c2262bac9aa6 204 for(int i=0; i<numT; i++){
hayama 0:c2262bac9aa6 205 fprintf(fp,"%d\n",targetA[i]); // 座標のインデックスは１から、配列のインデックスは０から
hayama 0:c2262bac9aa6 206 fprintf(fp,"%d\n",targetB[i]);
hayama 0:c2262bac9aa6 207 }
hayama 0:c2262bac9aa6 208 fprintf(fp, "%d\n",recvQZSS);
hayama 0:c2262bac9aa6 209 fclose(fp);
hayama 0:c2262bac9aa6 210
hayama 0:c2262bac9aa6 211 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 212 Thread::wait(1000);
hayama 0:c2262bac9aa6 213 }
hayama 0:c2262bac9aa6 214
hayama 0:c2262bac9aa6 215 // QZSS message file in
hayama 0:c2262bac9aa6 216 void readQmsg(){
hayama 0:c2262bac9aa6 217 char st[20];
hayama 0:c2262bac9aa6 218 FILE *fp = fopen("/local/QZmsg.txt","r");
hayama 0:c2262bac9aa6 219 fgets(st,20,fp); year=atoi(st);
hayama 0:c2262bac9aa6 220 fgets(st,20,fp); teamNum=atoi(st);
hayama 0:c2262bac9aa6 221 fgets(st,20,fp); numA=atoi(st);
hayama 0:c2262bac9aa6 222 fgets(st,20,fp); sizeA=atoi(st);
hayama 0:c2262bac9aa6 223 fgets(st,20,fp); numB=atoi(st);
hayama 0:c2262bac9aa6 224 fgets(st,20,fp); sizeB=atoi(st);
hayama 0:c2262bac9aa6 225 fgets(st,20,fp); numT=atoi(st);
hayama 0:c2262bac9aa6 226 fgets(st,20,fp); startA=atoi(st);
hayama 0:c2262bac9aa6 227 fgets(st,20,fp); startB=atoi(st);
hayama 0:c2262bac9aa6 228 for(int i=0; i<numT; i++){
hayama 0:c2262bac9aa6 229 fgets(st,20,fp); targetA[i]=atof(st); // 座標のインデックスは１から、配列のインデックスは０から
hayama 0:c2262bac9aa6 230 fgets(st,20,fp); targetB[i]=atof(st);
hayama 0:c2262bac9aa6 231 }
hayama 0:c2262bac9aa6 232 fgets(st,20,fp); recvQZSS=atoi(st);
hayama 0:c2262bac9aa6 233 if (recvQZSS==1) tNum=teamNum;
hayama 0:c2262bac9aa6 234 fclose(fp);
hayama 0:c2262bac9aa6 235
hayama 0:c2262bac9aa6 236 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 237 Thread::wait(1000);
hayama 0:c2262bac9aa6 238 }
hayama 0:c2262bac9aa6 239
hayama 0:c2262bac9aa6 240 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 241 // ウェイポイントの設定
hayama 0:c2262bac9aa6 242 // (GPSが受信できてない時はポイント追加できない)
hayama 0:c2262bac9aa6 243 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 244 void setWp(){
hayama 0:c2262bac9aa6 245 lcd.locate(0,1); lcd.printf("present WP: %d ",wp); // ウェイポイント番号を進表示
hayama 0:c2262bac9aa6 246 while (resetSW==1){ // STARTスイッチが押されたら抜ける
hayama 0:c2262bac9aa6 247
hayama 0:c2262bac9aa6 248 // start-swが押されたらウェイポイントを記録
hayama 0:c2262bac9aa6 249 if (startSW==0) {
hayama 0:c2262bac9aa6 250 Thread::wait(10);
hayama 0:c2262bac9aa6 251 while(startSW==0); // チャタリング防止
hayama 0:c2262bac9aa6 252 Thread::wait(10);
hayama 0:c2262bac9aa6 253
hayama 0:c2262bac9aa6 254 // GPRMCを受信できて値が正常であるまで待つ
hayama 0:c2262bac9aa6 255 while(recvGPS==0 || (latit==0) || (longit==0));
hayama 0:c2262bac9aa6 256
hayama 0:c2262bac9aa6 257 wy[wp] = latit;
hayama 0:c2262bac9aa6 258 wx[wp] = longit;
hayama 0:c2262bac9aa6 259 lcd.locate(0,1); lcd.printf("set WP: %d ",wp);
hayama 0:c2262bac9aa6 260 Thread::wait(2000);
hayama 0:c2262bac9aa6 261 lcd.locate(0,0); lcd.printf("wy[%d]=%f \n", wp, wy[wp]);
hayama 0:c2262bac9aa6 262 lcd.locate(0,1); lcd.printf("wx[%d]=%f \n", wp, wx[wp]);
hayama 0:c2262bac9aa6 263 Thread::wait(2000);
hayama 0:c2262bac9aa6 264 wp++; if (wp>15) wp=15; // 次のウェイポイント入力状態にするウェイポイントの追加は１５まで、０からで計１６個
hayama 0:c2262bac9aa6 265 // ウェイポイント追加
hayama 0:c2262bac9aa6 266 }
hayama 0:c2262bac9aa6 267
hayama 0:c2262bac9aa6 268 // set-swが押されたらウェイポイントdecrement
hayama 0:c2262bac9aa6 269 if (setSW==0) {
hayama 0:c2262bac9aa6 270 Thread::wait(10);
hayama 0:c2262bac9aa6 271 while(setSW==0); // チャタリング防止
hayama 0:c2262bac9aa6 272 Thread::wait(10);
hayama 0:c2262bac9aa6 273 wp--; if (wp<0) wp=0;
hayama 0:c2262bac9aa6 274 wy[wp] =0;
hayama 0:c2262bac9aa6 275 wx[wp] =0;
hayama 0:c2262bac9aa6 276 lcd.locate(0,1); lcd.printf("delete WP: %d ",wp);
hayama 0:c2262bac9aa6 277 Thread::wait(2000);
hayama 0:c2262bac9aa6 278 lcd.locate(0,0); lcd.printf("wy[%d]=%f \n", wp, wy[wp]);
hayama 0:c2262bac9aa6 279 lcd.locate(0,1); lcd.printf("wx[%d]=%f \n", wp, wx[wp]);
hayama 0:c2262bac9aa6 280 Thread::wait(2000);
hayama 0:c2262bac9aa6 281 }
hayama 0:c2262bac9aa6 282
hayama 0:c2262bac9aa6 283 lcd.cls();
hayama 0:c2262bac9aa6 284 lcd.printf("LA=%f \n", latit);
hayama 0:c2262bac9aa6 285 lcd.printf("LO=%f \n", longit);
hayama 0:c2262bac9aa6 286 Thread::wait(200);
hayama 0:c2262bac9aa6 287 }
hayama 0:c2262bac9aa6 288 }
hayama 0:c2262bac9aa6 289
hayama 0:c2262bac9aa6 290 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 291 // display way point
hayama 0:c2262bac9aa6 292 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 293 void dispWp(){
hayama 0:c2262bac9aa6 294 if(wp==0){
hayama 0:c2262bac9aa6 295 lcd.locate(0,1); lcd.printf("no way point !! ");
hayama 0:c2262bac9aa6 296 return;
hayama 0:c2262bac9aa6 297 }
hayama 0:c2262bac9aa6 298 int i=0;
hayama 0:c2262bac9aa6 299 lcd.cls();
hayama 0:c2262bac9aa6 300 while(resetSW==1){
hayama 0:c2262bac9aa6 301 if (setSW==0){
hayama 0:c2262bac9aa6 302 Thread::wait(10);
hayama 0:c2262bac9aa6 303 while (setSW==0);
hayama 0:c2262bac9aa6 304 Thread::wait(10);
hayama 0:c2262bac9aa6 305 i--; if (i<0) i=wp-1;
hayama 0:c2262bac9aa6 306 }
hayama 0:c2262bac9aa6 307 if (startSW==0){
hayama 0:c2262bac9aa6 308 Thread::wait(10);
hayama 0:c2262bac9aa6 309 while (startSW==0);
hayama 0:c2262bac9aa6 310 Thread::wait(10);
hayama 0:c2262bac9aa6 311 i++; if (i==wp) i=0;
hayama 0:c2262bac9aa6 312 }
hayama 0:c2262bac9aa6 313 lcd.locate(0,0); lcd.printf("wy[%d]=%f \n", i, wy[i]);
hayama 0:c2262bac9aa6 314 lcd.locate(0,1); lcd.printf("wx[%d]=%f \n", i, wx[i]);
hayama 0:c2262bac9aa6 315 Thread::wait(200);
hayama 0:c2262bac9aa6 316 }
hayama 0:c2262bac9aa6 317 Thread::wait(200);
hayama 0:c2262bac9aa6 318 }
hayama 0:c2262bac9aa6 319 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 320 // ウェイポイントの消去
hayama 0:c2262bac9aa6 321 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 322 void clearWp(){
hayama 0:c2262bac9aa6 323 wp=0;
hayama 0:c2262bac9aa6 324 for(int i=0;i<16;i++){ wy[i]=0; wx[i]=0; }
hayama 0:c2262bac9aa6 325 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 326 Thread::wait(1000);
hayama 0:c2262bac9aa6 327 }
hayama 0:c2262bac9aa6 328
hayama 0:c2262bac9aa6 329 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 330 // QZSS display way point
hayama 0:c2262bac9aa6 331 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 332 void dispQmsg(){
hayama 0:c2262bac9aa6 333 int i=0;
hayama 0:c2262bac9aa6 334
hayama 0:c2262bac9aa6 335 if (recvQZSS==1){
hayama 0:c2262bac9aa6 336 lcd.locate(0,1); lcd.printf("QZSS message OK!");
hayama 0:c2262bac9aa6 337 } else {
hayama 0:c2262bac9aa6 338 lcd.locate(0,1); lcd.printf("QZSS no message ");
hayama 0:c2262bac9aa6 339 }
hayama 0:c2262bac9aa6 340 Thread::wait(3000);
hayama 0:c2262bac9aa6 341
hayama 0:c2262bac9aa6 342 while(resetSW==1){
hayama 0:c2262bac9aa6 343 if (setSW==0){
hayama 0:c2262bac9aa6 344 Thread::wait(10);
hayama 0:c2262bac9aa6 345 while (setSW==0);
hayama 0:c2262bac9aa6 346 Thread::wait(10);
hayama 0:c2262bac9aa6 347 i--; if (i<0) i=24;
hayama 0:c2262bac9aa6 348 }
hayama 0:c2262bac9aa6 349 if (startSW==0){
hayama 0:c2262bac9aa6 350 Thread::wait(10);
hayama 0:c2262bac9aa6 351 while (startSW==0);
hayama 0:c2262bac9aa6 352 Thread::wait(10);
hayama 0:c2262bac9aa6 353 i++; if (i>24) i=0;
hayama 0:c2262bac9aa6 354 }
hayama 0:c2262bac9aa6 355 switch (i){
hayama 0:c2262bac9aa6 356 case 0: lcd.locate(0,1); lcd.printf("year=%d \n", year); break;
hayama 0:c2262bac9aa6 357 case 1: lcd.locate(0,1); lcd.printf("teamNum=%d \n", teamNum); break;
hayama 0:c2262bac9aa6 358 case 2: lcd.locate(0,1); lcd.printf("numA=%d \n", numA); break;
hayama 0:c2262bac9aa6 359 case 3: lcd.locate(0,1); lcd.printf("sizeA=%d \n", sizeA); break;
hayama 0:c2262bac9aa6 360 case 4: lcd.locate(0,1); lcd.printf("numB=%d \n", numB); break;
hayama 0:c2262bac9aa6 361 case 5: lcd.locate(0,1); lcd.printf("sizeB=%d \n", sizeB); break;
hayama 0:c2262bac9aa6 362 case 6: lcd.locate(0,1); lcd.printf("numT=%d \n", numT); break;
hayama 0:c2262bac9aa6 363 case 7: lcd.locate(0,1); lcd.printf("startA=%d \n", startA); break;
hayama 0:c2262bac9aa6 364 case 8: lcd.locate(0,1); lcd.printf("startB=%d \n", startB); break;
hayama 0:c2262bac9aa6 365 case 9: lcd.locate(0,1); lcd.printf("targetA[1]=%d \n", targetA[0]); break; // 座標のインデックスは１から、配列のインデックスは０から
hayama 0:c2262bac9aa6 366 case 10: lcd.locate(0,1); lcd.printf("targetB[1]=%d \n", targetB[0]); break;
hayama 0:c2262bac9aa6 367 case 11: lcd.locate(0,1); lcd.printf("targetA[2]=%d \n", targetA[1]); break;
hayama 0:c2262bac9aa6 368 case 12: lcd.locate(0,1); lcd.printf("targetB[2]=%d \n", targetB[1]); break;
hayama 0:c2262bac9aa6 369 case 13: lcd.locate(0,1); lcd.printf("targetA[3]=%d \n", targetA[2]); break;
hayama 0:c2262bac9aa6 370 case 14: lcd.locate(0,1); lcd.printf("targetB[3]=%d \n", targetB[2]); break;
hayama 0:c2262bac9aa6 371 case 15: lcd.locate(0,1); lcd.printf("targetA[4]=%d \n", targetA[3]); break;
hayama 0:c2262bac9aa6 372 case 16: lcd.locate(0,1); lcd.printf("targetB[4]=%d \n", targetB[3]); break;
hayama 0:c2262bac9aa6 373 case 17: lcd.locate(0,1); lcd.printf("targetA[5]=%d \n", targetA[4]); break;
hayama 0:c2262bac9aa6 374 case 18: lcd.locate(0,1); lcd.printf("targetB[5]=%d \n", targetB[4]); break;
hayama 0:c2262bac9aa6 375 case 19: lcd.locate(0,1); lcd.printf("targetA[6]=%d \n", targetA[5]); break;
hayama 0:c2262bac9aa6 376 case 20: lcd.locate(0,1); lcd.printf("targetB[6]=%d \n", targetB[5]); break;
hayama 0:c2262bac9aa6 377 case 21: lcd.locate(0,1); lcd.printf("targetA[7]=%d \n", targetA[6]); break;
hayama 0:c2262bac9aa6 378 case 22: lcd.locate(0,1); lcd.printf("targetB[7]=%d \n", targetB[6]); break;
hayama 0:c2262bac9aa6 379 case 23: lcd.locate(0,1); lcd.printf("targetA[8]=%d \n", targetA[7]); break;
hayama 0:c2262bac9aa6 380 case 24: lcd.locate(0,1); lcd.printf("targetB[8]=%d \n", targetB[7]); break;
hayama 0:c2262bac9aa6 381 }
hayama 0:c2262bac9aa6 382 Thread::wait(100);
hayama 0:c2262bac9aa6 383 }
hayama 0:c2262bac9aa6 384 Thread::wait(200);
hayama 0:c2262bac9aa6 385 }
hayama 0:c2262bac9aa6 386
hayama 0:c2262bac9aa6 387 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 388 // QZSS ウェイポイントの消去
hayama 0:c2262bac9aa6 389 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 390 void clearQmsg(){
hayama 0:c2262bac9aa6 391 for(int i=0;i<numT;i++){ targetA[i]=targetB[i]=0; }
hayama 0:c2262bac9aa6 392 year=teamNum=numA=sizeA=numB=sizeB=numT=startA=startB=0;
hayama 0:c2262bac9aa6 393 recvQZSS=0;
hayama 0:c2262bac9aa6 394 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 395 Thread::wait(1000);
hayama 0:c2262bac9aa6 396 }
hayama 0:c2262bac9aa6 397
hayama 0:c2262bac9aa6 398
hayama 0:c2262bac9aa6 399 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 400 // set team number, inc by startSW, dec by setSW, max 25
hayama 0:c2262bac9aa6 401 // compare to the teamNum from QZSS massage
hayama 0:c2262bac9aa6 402 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 403 void setTeamNum(){
hayama 0:c2262bac9aa6 404 while(resetSW==1){
hayama 0:c2262bac9aa6 405 if (startSW==0){tNum++; if (tNum>25) tNum=0;}
hayama 0:c2262bac9aa6 406 if (setSW==0) {tNum--; if (tNum<0) tNum=25;}
hayama 0:c2262bac9aa6 407 lcd.locate(0,1); lcd.printf("team number=%d \n", tNum);
hayama 0:c2262bac9aa6 408 Thread::wait(200);
hayama 0:c2262bac9aa6 409 }
hayama 0:c2262bac9aa6 410 }
hayama 0:c2262bac9aa6 411
hayama 0:c2262bac9aa6 412
hayama 0:c2262bac9aa6 413 // calculation of all QZSS waypoint from 4 point
hayama 0:c2262bac9aa6 414 void calcQZWp(){
hayama 0:c2262bac9aa6 415
hayama 0:c2262bac9aa6 416 wxQ11=wxQ[1][1]; wyQ11=wyQ[1][1];
hayama 0:c2262bac9aa6 417 xQ11=wxQ[1][1]-wxQ11; yQ11=wyQ[1][1]-wyQ11; // (A1,B1)
hayama 0:c2262bac9aa6 418 xQ71=wxQ[7][1]-wxQ11; yQ71=wyQ[7][1]-wyQ11; // (A7,B1)
hayama 0:c2262bac9aa6 419 xQ17=wxQ[1][7]-wxQ11; yQ17=wyQ[1][7]-wyQ11; // (A1,B7)
hayama 0:c2262bac9aa6 420 xQ77=wxQ[7][7]-wxQ11; yQ77=wyQ[7][7]-wyQ11; // (A7,B7)
hayama 0:c2262bac9aa6 421
hayama 0:c2262bac9aa6 422 dxQ71=xQ71-xQ11; dyQ71=yQ71-yQ11;
hayama 0:c2262bac9aa6 423 dxQ17=xQ17-xQ11; dyQ17=yQ17-yQ11;
hayama 0:c2262bac9aa6 424
hayama 0:c2262bac9aa6 425 theta=atan(dxQ71/dyQ71);
hayama 0:c2262bac9aa6 426 magx=sqrt(dxQ71*dxQ71+dyQ71*dyQ71)/6;
hayama 0:c2262bac9aa6 427 magy=sqrt(dxQ17*dxQ17+dyQ17*dyQ17)/6;
hayama 0:c2262bac9aa6 428
hayama 0:c2262bac9aa6 429 for (int i=1;i<8;i++){
hayama 0:c2262bac9aa6 430 for(int j=1;j<8;j++){
hayama 0:c2262bac9aa6 431 wxQ[i][j]=magx*(j-1)*cos(theta)-magy*(i-1)*sin(theta)+wxQ11;
hayama 0:c2262bac9aa6 432 wyQ[i][j]=magx*(j-1)*sin(theta)+magy*(i-1)*cos(theta)+wyQ11;
hayama 0:c2262bac9aa6 433 }
hayama 0:c2262bac9aa6 434 }
hayama 0:c2262bac9aa6 435 }
hayama 0:c2262bac9aa6 436
hayama 0:c2262bac9aa6 437
hayama 0:c2262bac9aa6 438 // set QZSS way point for (A1,B1)(A7,B1)(A1,B7)(A7,B7)
hayama 0:c2262bac9aa6 439 void setQZWp(){
hayama 0:c2262bac9aa6 440 int qa=1,qb=1;
hayama 0:c2262bac9aa6 441 int qn=3;
hayama 0:c2262bac9aa6 442
hayama 0:c2262bac9aa6 443 while (resetSW==1){ // STARTスイッチが押されたら抜ける
hayama 0:c2262bac9aa6 444
hayama 0:c2262bac9aa6 445 // set-swが押されたらchange A,B axis
hayama 0:c2262bac9aa6 446 if (setSW==0) {
hayama 0:c2262bac9aa6 447 Thread::wait(10);
hayama 0:c2262bac9aa6 448 while(setSW==0); // チャタリング防止
hayama 0:c2262bac9aa6 449 Thread::wait(10);
hayama 0:c2262bac9aa6 450
hayama 0:c2262bac9aa6 451 qn++; if (qn>3) qn=0; // 次のウェイポイント入力状態にするウェイポイントの追加は１５まで、０からで計１６個
hayama 0:c2262bac9aa6 452 switch(qn){
hayama 0:c2262bac9aa6 453 case 0: qa=1; qb=1; break;
hayama 0:c2262bac9aa6 454 case 1: qa=7; qb=1; break;
hayama 0:c2262bac9aa6 455 case 2: qa=1; qb=7; break;
hayama 0:c2262bac9aa6 456 case 3: qa=7; qb=7; break;
hayama 0:c2262bac9aa6 457 }
hayama 0:c2262bac9aa6 458 lcd.cls();
hayama 0:c2262bac9aa6 459 lcd.locate(0,1); lcd.printf("(A%d,B%d)", qa,qb);
hayama 0:c2262bac9aa6 460 Thread::wait(2000);
hayama 0:c2262bac9aa6 461 lcd.cls();
hayama 0:c2262bac9aa6 462 lcd.locate(0,0); lcd.printf("wyQ[%d][%d]=%f \n", qa,qb, wyQ[qa][qb]);
hayama 0:c2262bac9aa6 463 lcd.locate(0,1); lcd.printf("wxQ[%d][%d]=%f \n", qa,qb, wxQ[qa][qb]);
hayama 0:c2262bac9aa6 464 Thread::wait(2000);
hayama 0:c2262bac9aa6 465 }
hayama 0:c2262bac9aa6 466
hayama 0:c2262bac9aa6 467 // start-swが押されたらウェイポイントを記録
hayama 0:c2262bac9aa6 468 if (startSW==0) {
hayama 0:c2262bac9aa6 469 Thread::wait(10);
hayama 0:c2262bac9aa6 470 while(startSW==0); // チャタリング防止
hayama 0:c2262bac9aa6 471 Thread::wait(10);
hayama 0:c2262bac9aa6 472
hayama 0:c2262bac9aa6 473 // GPRMCを受信できて値が正常であるまで待つ
hayama 0:c2262bac9aa6 474 while(recvGPS==0 || (latit==0) || (longit==0));
hayama 0:c2262bac9aa6 475
hayama 0:c2262bac9aa6 476 wyQ[qa][qb] = latit;
hayama 0:c2262bac9aa6 477 wxQ[qa][qb] = longit;
hayama 0:c2262bac9aa6 478 lcd.cls();
hayama 0:c2262bac9aa6 479 lcd.locate(0,0); lcd.printf("wyQ[%d][%d]=%f \n", qa,qb, wyQ[qa][qb]);
hayama 0:c2262bac9aa6 480 lcd.locate(0,1); lcd.printf("wxQ[%d][%d]=%f \n", qa,qb, wxQ[qa][qb]);
hayama 0:c2262bac9aa6 481 Thread::wait(2000);
hayama 0:c2262bac9aa6 482 }
hayama 0:c2262bac9aa6 483
hayama 0:c2262bac9aa6 484 lcd.cls();
hayama 0:c2262bac9aa6 485 lcd.printf("LA=%f \n", latit);
hayama 0:c2262bac9aa6 486 lcd.printf("LO=%f \n", longit);
hayama 0:c2262bac9aa6 487 Thread::wait(200);
hayama 0:c2262bac9aa6 488 }
hayama 0:c2262bac9aa6 489 calcQZWp();
hayama 0:c2262bac9aa6 490 }
hayama 0:c2262bac9aa6 491
hayama 0:c2262bac9aa6 492 // file out QZSS way point, 4 point only
hayama 0:c2262bac9aa6 493 void writeQZWp4(){
hayama 0:c2262bac9aa6 494 FILE *fp = fopen("/local/QZpoint4.txt","w");
hayama 0:c2262bac9aa6 495 fprintf(fp, "%f\n",xQ11);
hayama 0:c2262bac9aa6 496 fprintf(fp, "%f\n",yQ11);
hayama 0:c2262bac9aa6 497 fprintf(fp, "%f\n",xQ71);
hayama 0:c2262bac9aa6 498 fprintf(fp, "%f\n",yQ71);
hayama 0:c2262bac9aa6 499 fprintf(fp, "%f\n",xQ17);
hayama 0:c2262bac9aa6 500 fprintf(fp, "%f\n",yQ17);
hayama 0:c2262bac9aa6 501 fprintf(fp, "%f\n",xQ77);
hayama 0:c2262bac9aa6 502 fprintf(fp, "%f\n",yQ77);
hayama 0:c2262bac9aa6 503 fclose(fp);
hayama 0:c2262bac9aa6 504 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 505 Thread::wait(1000);
hayama 0:c2262bac9aa6 506 }
hayama 0:c2262bac9aa6 507
hayama 0:c2262bac9aa6 508 // file read QZSS way point, 4 point only
hayama 0:c2262bac9aa6 509 void readQZWp4(){
hayama 0:c2262bac9aa6 510 char st[20];
hayama 0:c2262bac9aa6 511 FILE *fp = fopen("/local/QZpoint4.txt","r");
hayama 0:c2262bac9aa6 512 fgets(st,20,fp); xQ11=atof(st);
hayama 0:c2262bac9aa6 513 fgets(st,20,fp); yQ11=atof(st);
hayama 0:c2262bac9aa6 514 fgets(st,20,fp); xQ71=atof(st);
hayama 0:c2262bac9aa6 515 fgets(st,20,fp); yQ71=atof(st);
hayama 0:c2262bac9aa6 516 fgets(st,20,fp); xQ17=atof(st);
hayama 0:c2262bac9aa6 517 fgets(st,20,fp); yQ17=atof(st);
hayama 0:c2262bac9aa6 518 fgets(st,20,fp); xQ77=atof(st);
hayama 0:c2262bac9aa6 519 fgets(st,20,fp); yQ77=atof(st);
hayama 0:c2262bac9aa6 520 fclose(fp);
hayama 0:c2262bac9aa6 521 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 522 Thread::wait(1000);
hayama 0:c2262bac9aa6 523 calcQZWp();
hayama 0:c2262bac9aa6 524 }
hayama 0:c2262bac9aa6 525
hayama 0:c2262bac9aa6 526
hayama 0:c2262bac9aa6 527 // file out QZSS way point
hayama 0:c2262bac9aa6 528 void writeQZWp(){
hayama 0:c2262bac9aa6 529 FILE *fp = fopen("/local/QZpoint.txt","w");
hayama 0:c2262bac9aa6 530 for(int i=1;i<8;i++){
hayama 0:c2262bac9aa6 531 for(int j=1;j<8;j++){
hayama 0:c2262bac9aa6 532 fprintf(fp, "%f\n",wyQ[i][j]);
hayama 0:c2262bac9aa6 533 fprintf(fp, "%f\n",wxQ[i][j]);
hayama 0:c2262bac9aa6 534 }
hayama 0:c2262bac9aa6 535 }
hayama 0:c2262bac9aa6 536 fclose(fp);
hayama 0:c2262bac9aa6 537 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 538 Thread::wait(1000);
hayama 0:c2262bac9aa6 539 }
hayama 0:c2262bac9aa6 540
hayama 0:c2262bac9aa6 541 // file read QZSS way point
hayama 0:c2262bac9aa6 542 void readQZWp(){
hayama 0:c2262bac9aa6 543 char st[20];
hayama 0:c2262bac9aa6 544 FILE *fp = fopen("/local/QZpoint.txt","r");
hayama 0:c2262bac9aa6 545 for(int i=1;i<8;i++){
hayama 0:c2262bac9aa6 546 for(int j=1;j<8;j++){
hayama 0:c2262bac9aa6 547 fgets(st,20,fp); wyQ[i][j]=atof(st);
hayama 0:c2262bac9aa6 548 fgets(st,20,fp); wxQ[i][j]=atof(st);
hayama 0:c2262bac9aa6 549 }
hayama 0:c2262bac9aa6 550 }
hayama 0:c2262bac9aa6 551 fclose(fp);
hayama 0:c2262bac9aa6 552 lcd.locate(0,1); lcd.printf("Completed! ");
hayama 0:c2262bac9aa6 553 Thread::wait(1000);
hayama 0:c2262bac9aa6 554 }
hayama 0:c2262bac9aa6 555
hayama 0:c2262bac9aa6 556
hayama 0:c2262bac9aa6 557 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 558 // スムージング関数(cData:現在のデータ，pData:一つ前のデータ,factor:係数，大きい現在地の割合大きい
hayama 0:c2262bac9aa6 559 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 560 float smooth(float cData, float pData, float factor){
hayama 0:c2262bac9aa6 561 return(cData*factor + pData*(1-factor));
hayama 0:c2262bac9aa6 562 }
hayama 0:c2262bac9aa6 563
hayama 0:c2262bac9aa6 564 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 565 // count up by encoder
hayama 0:c2262bac9aa6 566 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 567 void countR(){
hayama 0:c2262bac9aa6 568 encR++;
hayama 0:c2262bac9aa6 569 }
hayama 0:c2262bac9aa6 570 void countL(){
hayama 0:c2262bac9aa6 571 encL++;
hayama 0:c2262bac9aa6 572 }
hayama 0:c2262bac9aa6 573
hayama 0:c2262bac9aa6 574 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 575 // ジャイロのオフセット取得
hayama 0:c2262bac9aa6 576 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 577 int gyro_offset(void){
hayama 0:c2262bac9aa6 578 int i,tmp;
hayama 0:c2262bac9aa6 579
hayama 0:c2262bac9aa6 580 lcd.cls();
hayama 0:c2262bac9aa6 581 Thread::wait(1000);
hayama 0:c2262bac9aa6 582 gyOff=0;
hayama 0:c2262bac9aa6 583 for (i=0;i<16;i++){
hayama 0:c2262bac9aa6 584 gyOff+=gyroOut; // read gyro sensor
hayama 0:c2262bac9aa6 585 Thread::wait(10);
hayama 0:c2262bac9aa6 586 }
hayama 0:c2262bac9aa6 587 gyOff=gyOff/16+GYROADJ;
hayama 0:c2262bac9aa6 588
hayama 0:c2262bac9aa6 589 gyItg=0;
hayama 0:c2262bac9aa6 590 for (i=0;i<10;i++){
hayama 0:c2262bac9aa6 591 tmp=(int)(fabs(angle));
hayama 0:c2262bac9aa6 592 lcd.locate(0,1); lcd.printf("angle=%d \n", tmp);
hayama 0:c2262bac9aa6 593 Thread::wait(100);
hayama 0:c2262bac9aa6 594 }
hayama 0:c2262bac9aa6 595 return ((int)fabs(angle)); // スタートスイッチで復帰，ただしジャイロ使えないレベル
hayama 0:c2262bac9aa6 596 }
hayama 0:c2262bac9aa6 597
hayama 0:c2262bac9aa6 598 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 599 // ジャイロのリセット
hayama 0:c2262bac9aa6 600 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 601 void gyro_reset(void){
hayama 0:c2262bac9aa6 602 gyItg=0;
hayama 0:c2262bac9aa6 603 angle=0;
hayama 0:c2262bac9aa6 604 }
hayama 0:c2262bac9aa6 605
hayama 0:c2262bac9aa6 606 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 607 // interrupt by us timerS
hayama 0:c2262bac9aa6 608 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 609 void sensor() {
hayama 0:c2262bac9aa6 610 // read gyro sensor
hayama 0:c2262bac9aa6 611 gyro=-(gyroOut-gyOff);
hayama 0:c2262bac9aa6 612 gyItg+=gyro;
hayama 0:c2262bac9aa6 613 angle=gyItg*KANG;
hayama 0:c2262bac9aa6 614
hayama 0:c2262bac9aa6 615 cntE++; if (cntE>200){
hayama 0:c2262bac9aa6 616 cntE=0;
hayama 0:c2262bac9aa6 617 cntR=encR-encRB;
hayama 0:c2262bac9aa6 618 cntL=encL-encLB;
hayama 0:c2262bac9aa6 619 encRB=encR;
hayama 0:c2262bac9aa6 620 encLB=encL;
hayama 0:c2262bac9aa6 621 cntC=(cntR>cntL)? cntR: cntL; // max of cntR/L.
hayama 0:c2262bac9aa6 622 }
hayama 0:c2262bac9aa6 623 dirCompus=arrayCompus[compus];
hayama 0:c2262bac9aa6 624 }
hayama 0:c2262bac9aa6 625
hayama 0:c2262bac9aa6 626 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 627 // テスト，パラメータ設定
hayama 0:c2262bac9aa6 628 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 629
hayama 0:c2262bac9aa6 630 // サーボのテスト
hayama 0:c2262bac9aa6 631 void testServo() {
hayama 0:c2262bac9aa6 632 strServo=strmin; // 最初左に曲がる
hayama 0:c2262bac9aa6 633 Thread::wait(1000);
hayama 0:c2262bac9aa6 634 strServo=strmax; // 次に右に曲がる
hayama 0:c2262bac9aa6 635 Thread::wait(1000);
hayama 0:c2262bac9aa6 636 strServo=strn; // ニュートラル
hayama 0:c2262bac9aa6 637 Thread::wait(1000);
hayama 0:c2262bac9aa6 638 }
hayama 0:c2262bac9aa6 639
hayama 0:c2262bac9aa6 640 // モータテスト、実行スイッチで正転、セットスイッチで逆転、リセットを押すまでモータ回り続ける
hayama 0:c2262bac9aa6 641 void testMotor(){
hayama 0:c2262bac9aa6 642 while(resetSW==1){
hayama 0:c2262bac9aa6 643 if (startSW==0) motorAmp=POSC-posL;
hayama 0:c2262bac9aa6 644 else if (setSW==0) motorAmp=POSC+posL;
hayama 0:c2262bac9aa6 645 //else motorAmp=POSC;
hayama 0:c2262bac9aa6 646 }
hayama 0:c2262bac9aa6 647 }
hayama 0:c2262bac9aa6 648
hayama 0:c2262bac9aa6 649 // encoderテスト
hayama 0:c2262bac9aa6 650 void testEncoder(){
hayama 0:c2262bac9aa6 651 lcd.cls();
hayama 0:c2262bac9aa6 652 while(resetSW==1){
hayama 0:c2262bac9aa6 653 lcd.locate(0,0); lcd.printf("eR%d eL%d \n", encR, encL);
hayama 0:c2262bac9aa6 654 lcd.locate(0,1); lcd.printf("cR=%d cL=%d cC=%d \n", cntR, cntL, cntC);
hayama 0:c2262bac9aa6 655 }
hayama 0:c2262bac9aa6 656 }
hayama 0:c2262bac9aa6 657
hayama 0:c2262bac9aa6 658 // test gyro sensor
hayama 0:c2262bac9aa6 659 void testGyro(){
hayama 0:c2262bac9aa6 660 lcd.cls();
hayama 0:c2262bac9aa6 661 gyro_reset();
hayama 0:c2262bac9aa6 662 while(resetSW==1){
hayama 0:c2262bac9aa6 663 if (setSW==0) {
hayama 0:c2262bac9aa6 664 gyro_offset();
hayama 0:c2262bac9aa6 665 gyro_reset();
hayama 0:c2262bac9aa6 666 }
hayama 0:c2262bac9aa6 667 lcd.locate(0,0); lcd.printf("gyItg=%f \n", gyItg);
hayama 0:c2262bac9aa6 668 lcd.locate(0,1); lcd.printf("angle=%f \n", angle);
hayama 0:c2262bac9aa6 669 }
hayama 0:c2262bac9aa6 670 }
hayama 0:c2262bac9aa6 671
hayama 0:c2262bac9aa6 672 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 673 // モータスタート
hayama 0:c2262bac9aa6 674 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 675 void motorStart(){
hayama 0:c2262bac9aa6 676 motorAmp=POSC-posL;
hayama 0:c2262bac9aa6 677 Thread::wait(1000);
hayama 0:c2262bac9aa6 678 }
hayama 0:c2262bac9aa6 679
hayama 0:c2262bac9aa6 680 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 681 // モータストップ
hayama 0:c2262bac9aa6 682 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 683 void motorStop(){
hayama 0:c2262bac9aa6 684 motorAmp=POSC;
hayama 0:c2262bac9aa6 685 Thread::wait(500);
hayama 0:c2262bac9aa6 686 }
hayama 0:c2262bac9aa6 687
hayama 0:c2262bac9aa6 688 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 689 // ステアリングニュートラル
hayama 0:c2262bac9aa6 690 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 691 void steerNeutral(){
hayama 0:c2262bac9aa6 692 strServo=strn;
hayama 0:c2262bac9aa6 693 }
hayama 0:c2262bac9aa6 694
hayama 0:c2262bac9aa6 695 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 696 // ニュートラルのセット、スタートでプラス、セットでマイナス、リセットで戻る
hayama 0:c2262bac9aa6 697 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 698 void initNeutral(){
hayama 0:c2262bac9aa6 699 while(resetSW==1){
hayama 0:c2262bac9aa6 700 if (startSW==0){strn+=0.01; if (strn>strmax) strn=strmax; }
hayama 0:c2262bac9aa6 701 if (setSW==0) {strn-=0.01; if (strn<strmin) strn=strmin; }
hayama 0:c2262bac9aa6 702 strServo=strn; // ニュートラル
hayama 0:c2262bac9aa6 703 lcd.locate(0,1); lcd.printf("str nuetral=%f \n", strn);
hayama 0:c2262bac9aa6 704 Thread::wait(100);
hayama 0:c2262bac9aa6 705 }
hayama 0:c2262bac9aa6 706 }
hayama 0:c2262bac9aa6 707
hayama 0:c2262bac9aa6 708 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 709 // ステアリング最小値のセット、スタートでプラス、セットでマイナス、リセットで戻る
hayama 0:c2262bac9aa6 710 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 711 void initStrmin(){
hayama 0:c2262bac9aa6 712 while(resetSW==1){
hayama 0:c2262bac9aa6 713 if (startSW==0) if (strmin<0.5) strmin+=0.02; // 可変範囲は45-90度まで
hayama 0:c2262bac9aa6 714 if (setSW==0) if (strmin>0.02) strmin-=0.02;
hayama 0:c2262bac9aa6 715 strServo=strmin; // ステアリング最小値に動かす
hayama 0:c2262bac9aa6 716 lcd.locate(0,1); lcd.printf("strmin=%f \n", strmin);
hayama 0:c2262bac9aa6 717 Thread::wait(100);
hayama 0:c2262bac9aa6 718 }
hayama 0:c2262bac9aa6 719 }
hayama 0:c2262bac9aa6 720
hayama 0:c2262bac9aa6 721 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 722 // ステアリング最大値のセット、スタートでプラス、セットでマイナス、リセットで戻る
hayama 0:c2262bac9aa6 723 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 724 void initStrmax(){
hayama 0:c2262bac9aa6 725 while(resetSW==1){
hayama 0:c2262bac9aa6 726 if (startSW==0) if (strmax<0.98) strmax+=0.02; // 可変範囲は90-135度まで
hayama 0:c2262bac9aa6 727 if (setSW==0) if (strmax>0.5 ) strmax-=0.02;
hayama 0:c2262bac9aa6 728 strServo=strmax; // ステアリング最大値に動かす
hayama 0:c2262bac9aa6 729 lcd.locate(0,1); lcd.printf("strmax=%f \n", strmax);
hayama 0:c2262bac9aa6 730 Thread::wait(200);
hayama 0:c2262bac9aa6 731 }
hayama 0:c2262bac9aa6 732 }
hayama 0:c2262bac9aa6 733
hayama 0:c2262bac9aa6 734 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 735 // ウェイポイントの半径をセット、スタートでプラス、セットでマイナス、リセットで戻る
hayama 0:c2262bac9aa6 736 // ２進数で半径を表示
hayama 0:c2262bac9aa6 737 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 738 void initWr(){
hayama 0:c2262bac9aa6 739 while(resetSW==1){
hayama 0:c2262bac9aa6 740 if (startSW==0){wr+=0.1; if (wr>10) wr=10; }
hayama 0:c2262bac9aa6 741 if (setSW==0) {wr-=0.1; if (wr<0.1) wr=0.1; }
hayama 0:c2262bac9aa6 742 lcd.locate(0,1); lcd.printf("wr=%f \n", wr);
hayama 0:c2262bac9aa6 743 Thread::wait(200);
hayama 0:c2262bac9aa6 744 }
hayama 0:c2262bac9aa6 745 }
hayama 0:c2262bac9aa6 746
hayama 0:c2262bac9aa6 747 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 748 // ステアリング制御の比例係数をセット、スタートでプラス、セットでマイナス、リセットで戻る
hayama 0:c2262bac9aa6 749 // 比例係数の50倍を２進数で表示
hayama 0:c2262bac9aa6 750 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 751 void initKs(){
hayama 0:c2262bac9aa6 752 while(resetSW==1){
hayama 0:c2262bac9aa6 753 if (startSW==0){ks+=0.001; if (ks>1) ks=1;}
hayama 0:c2262bac9aa6 754 if (setSW==0) {ks-=0.001; if (ks<-1) ks=-1;}
hayama 0:c2262bac9aa6 755 lcd.locate(0,1); lcd.printf("ks=%f \n", ks);
hayama 0:c2262bac9aa6 756 Thread::wait(200);
hayama 0:c2262bac9aa6 757 }
hayama 0:c2262bac9aa6 758 }
hayama 0:c2262bac9aa6 759
hayama 0:c2262bac9aa6 760 void initKsG(){
hayama 0:c2262bac9aa6 761 while(resetSW==1){
hayama 0:c2262bac9aa6 762 if (startSW==0){ksg+=0.001; if (ksg>1) ksg=1;}
hayama 0:c2262bac9aa6 763 if (setSW==0) {ksg-=0.001; if (ksg<-1) ksg=-1;}
hayama 0:c2262bac9aa6 764 lcd.locate(0,1); lcd.printf("ksg=%f \n", ksg);
hayama 0:c2262bac9aa6 765 Thread::wait(200);
hayama 0:c2262bac9aa6 766 }
hayama 0:c2262bac9aa6 767 }
hayama 0:c2262bac9aa6 768
hayama 0:c2262bac9aa6 769 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 770 // 直進スピードをセット、スタートでプラス、セットでマイナス、リセットで戻る
hayama 0:c2262bac9aa6 771 // スピードの1/16を2進数で表示
hayama 0:c2262bac9aa6 772 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 773 void initPosH(){
hayama 0:c2262bac9aa6 774 while(resetSW==1){
hayama 0:c2262bac9aa6 775 motorAmp=POSC-posH; // モータを回転させる
hayama 0:c2262bac9aa6 776 if (startSW==0){posH+=0.01; if (posH>0.8) posH=0.8;}
hayama 0:c2262bac9aa6 777 if (setSW==0) {posH-=0.01; if (posH<0) posH=0;}
hayama 0:c2262bac9aa6 778 lcd.locate(0,1); lcd.printf("PosH=%f \n", posH);
hayama 0:c2262bac9aa6 779 Thread::wait(200);
hayama 0:c2262bac9aa6 780 }
hayama 0:c2262bac9aa6 781 }
hayama 0:c2262bac9aa6 782
hayama 0:c2262bac9aa6 783 void initPosL(){
hayama 0:c2262bac9aa6 784 while(resetSW==1){
hayama 0:c2262bac9aa6 785 motorAmp=POSC-posL; // モータを回転させる
hayama 0:c2262bac9aa6 786 if (startSW==0){posL+=0.01; if (posL>0.8) posL=0.8;}
hayama 0:c2262bac9aa6 787 if (setSW==0) {posL-=0.01; if (posL<0) posL=0;}
hayama 0:c2262bac9aa6 788 lcd.locate(0,1); lcd.printf("PosL=%f \n", posL);
hayama 0:c2262bac9aa6 789 Thread::wait(200);
hayama 0:c2262bac9aa6 790 }
hayama 0:c2262bac9aa6 791 }
hayama 0:c2262bac9aa6 792
hayama 0:c2262bac9aa6 793
hayama 0:c2262bac9aa6 794 void initSpdH(){ // 走行スピードhighの設定
hayama 0:c2262bac9aa6 795 while(resetSW==1){
hayama 0:c2262bac9aa6 796 if (startSW==0){spdH++; if (spdH>100) spdH=100;}
hayama 0:c2262bac9aa6 797 if (setSW==0) {spdH--; if (spdH<0) spdH=0;}
hayama 0:c2262bac9aa6 798 lcd.locate(0,1); lcd.printf("speed High=%d \n", spdH);
hayama 0:c2262bac9aa6 799 Thread::wait(200);
hayama 0:c2262bac9aa6 800 }
hayama 0:c2262bac9aa6 801 }
hayama 0:c2262bac9aa6 802
hayama 0:c2262bac9aa6 803 void initSpdL(){ // 走行スピードlowの設定
hayama 0:c2262bac9aa6 804 while(resetSW==1){
hayama 0:c2262bac9aa6 805 if (startSW==0){spdL++; if (spdL>100) spdL=100;}
hayama 0:c2262bac9aa6 806 if (setSW==0) {spdL--; if (spdL<0) spdL=0;}
hayama 0:c2262bac9aa6 807 lcd.locate(0,1); lcd.printf("speed Low=%d \n", spdL);
hayama 0:c2262bac9aa6 808 Thread::wait(200);
hayama 0:c2262bac9aa6 809 }
hayama 0:c2262bac9aa6 810 }
hayama 0:c2262bac9aa6 811
hayama 0:c2262bac9aa6 812 void testRun(){ // test run
hayama 0:c2262bac9aa6 813 int setSpd=0;
hayama 0:c2262bac9aa6 814 while(resetSW==1){
hayama 0:c2262bac9aa6 815 if (setSW==0) setSpd=spdL;
hayama 0:c2262bac9aa6 816 if (startSW==0) setSpd=spdH;
hayama 0:c2262bac9aa6 817 lcd.locate(0,1); lcd.printf("set speed=%d \n", setSpd);
hayama 0:c2262bac9aa6 818
hayama 0:c2262bac9aa6 819 if (setSpd>cntC){
hayama 0:c2262bac9aa6 820 motorAmp=POSC-posH;
hayama 0:c2262bac9aa6 821 } else {
hayama 0:c2262bac9aa6 822 motorAmp=POSC-posL;
hayama 0:c2262bac9aa6 823 }
hayama 0:c2262bac9aa6 824 Thread::wait(50);
hayama 0:c2262bac9aa6 825 }
hayama 0:c2262bac9aa6 826 motorAmp=POSC;
hayama 0:c2262bac9aa6 827 }
hayama 0:c2262bac9aa6 828
hayama 0:c2262bac9aa6 829
hayama 0:c2262bac9aa6 830 void testCompus(){
hayama 0:c2262bac9aa6 831 lcd.cls();
hayama 0:c2262bac9aa6 832 while(resetSW==1){
hayama 0:c2262bac9aa6 833 lcd.locate(0,0); lcd.printf("compus=%f \n", arrayCompus[compus]);
hayama 0:c2262bac9aa6 834 lcd.locate(0,1); lcd.printf("rd=%f \n", rd);
hayama 0:c2262bac9aa6 835 Thread::wait(50);
hayama 0:c2262bac9aa6 836 }
hayama 0:c2262bac9aa6 837 }
hayama 0:c2262bac9aa6 838
hayama 0:c2262bac9aa6 839
hayama 0:c2262bac9aa6 840 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 841 // GPS受信テスト，補足状況をLED点灯の数で示す
hayama 0:c2262bac9aa6 842 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 843 void dispGPS(){
hayama 0:c2262bac9aa6 844 while(resetSW==1){
hayama 0:c2262bac9aa6 845 lcd.cls();
hayama 0:c2262bac9aa6 846 if (latit==0) lcd.printf("GPS Not Found \n");
hayama 0:c2262bac9aa6 847 else {
hayama 0:c2262bac9aa6 848 lcd.cls();
hayama 0:c2262bac9aa6 849 lcd.printf("LA=%f \n", latit);
hayama 0:c2262bac9aa6 850 lcd.printf("LO=%f \n", longit);
hayama 0:c2262bac9aa6 851 Thread::wait(1500);
hayama 0:c2262bac9aa6 852 lcd.cls();
hayama 0:c2262bac9aa6 853 lcd.printf("rv=%f3 \n",rv);
hayama 0:c2262bac9aa6 854 lcd.printf("rd=%f3 \n",rd);
hayama 0:c2262bac9aa6 855 Thread::wait(1500);
hayama 0:c2262bac9aa6 856 }
hayama 0:c2262bac9aa6 857 Thread::wait(200);
hayama 0:c2262bac9aa6 858
hayama 0:c2262bac9aa6 859 }
hayama 0:c2262bac9aa6 860 Thread::wait(200);
hayama 0:c2262bac9aa6 861 }
hayama 0:c2262bac9aa6 862
hayama 0:c2262bac9aa6 863 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 864 // GPS信号の数値返還
hayama 0:c2262bac9aa6 865 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 866 void gps_val(){
hayama 0:c2262bac9aa6 867 latitB=latit; longitB=longit; // 一つ前の値を保管
hayama 0:c2262bac9aa6 868 latit=atof(latitude+2); // 文字列を実数に変換．有効数字が足りないので度単位を省略
hayama 0:c2262bac9aa6 869 longit=atof(longitude+3); // 文字列を実数に変換．有効数字が足りないので度単位を省略
hayama 0:c2262bac9aa6 870 kn=atof(knot);
hayama 0:c2262bac9aa6 871 rv=kn*0.51; // ノット単位をメートル単位に変換
hayama 0:c2262bac9aa6 872 rdB=rd; // 一つ前の値を保管
hayama 0:c2262bac9aa6 873 rd=atof(direct); // 方位を数値変換
hayama 0:c2262bac9aa6 874 }
hayama 0:c2262bac9aa6 875
hayama 0:c2262bac9aa6 876 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 877 // 現在地とウェイポイントとの相対関係を計算
hayama 0:c2262bac9aa6 878 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 879 void gps_cal(){
hayama 0:c2262bac9aa6 880 // 緯度経度の数値変換，目標地点との差分を計算
hayama 0:c2262bac9aa6 881 dy=(wy[np] - latit ) *1860;
hayama 0:c2262bac9aa6 882 dx=(wx[np] - longit)*1560;
hayama 0:c2262bac9aa6 883
hayama 0:c2262bac9aa6 884 // 目標地点までのと距離を計算
hayama 0:c2262bac9aa6 885 dr=sqrt(pow(dy,2)+pow(dx,2));
hayama 0:c2262bac9aa6 886
hayama 0:c2262bac9aa6 887 // 目標地点への方向を計算
hayama 0:c2262bac9aa6 888 dd = atan(dx / dy); // 北に対する角度を求める(±π/2範囲)
hayama 0:c2262bac9aa6 889 dd=dd*57; // ラジアン->度に変換 dd*(180/pai)
hayama 0:c2262bac9aa6 890 // 0-360度に変換
hayama 0:c2262bac9aa6 891 if (dx > 0 && dy < 0) dd = 180 + dd;
hayama 0:c2262bac9aa6 892 else if(dx < 0 && dy < 0) dd = 180 + dd;
hayama 0:c2262bac9aa6 893 else if(dx < 0 && dy > 0) dd = 360 + dd;
hayama 0:c2262bac9aa6 894 else;
hayama 0:c2262bac9aa6 895
hayama 0:c2262bac9aa6 896 // GPSが正しい方位を出力しない場合は、前の座標との差分で計算
hayama 0:c2262bac9aa6 897 dy2=(latit -latitB) *1860;
hayama 0:c2262bac9aa6 898 dx2=(longit-longitB)*1560;
hayama 0:c2262bac9aa6 899
hayama 0:c2262bac9aa6 900 if (dy2==0 || dx2==0){ // 緯度または経度のどちらかの変化が０の時
hayama 0:c2262bac9aa6 901 if (dy2==0){ if (dx2<0) rd2= 270; else rd2=90; } // rd2= 0, 90, 180，270 のいずれかを直接与える．
hayama 0:c2262bac9aa6 902 if (dx2==0){ if (dy2<0) rd2=-180; else rd2=0; } // dy2,dx2のどちらも0の時はrd2=0
hayama 0:c2262bac9aa6 903 } else {
hayama 0:c2262bac9aa6 904 // 目標地点への方向を計算
hayama 0:c2262bac9aa6 905 rd2 = atan(dx2 / dy2); // 北に対する角度を求める(±π/2範囲)
hayama 0:c2262bac9aa6 906 rd2=rd2*57; // ラジアン->度に変換 dd*(180/pai)
hayama 0:c2262bac9aa6 907 // 0-360度に変換
hayama 0:c2262bac9aa6 908 if (dx2 > 0 && dy2 < 0) rd2 = 180 + rd2;
hayama 0:c2262bac9aa6 909 else if(dx2 < 0 && dy2 < 0) rd2 = 180 + rd2;
hayama 0:c2262bac9aa6 910 else if(dx2 < 0 && dy2 > 0) rd2 = 360 + rd2;
hayama 0:c2262bac9aa6 911 else;
hayama 0:c2262bac9aa6 912 }
hayama 0:c2262bac9aa6 913
hayama 0:c2262bac9aa6 914 // 移動距離が小さくて使用するGPSが正しい速度と方向を出さない時のみ
hayama 0:c2262bac9aa6 915 // 座標差から計算した値と置き換える（速度=0,方位前の値のまま，GPSの仕様により異なる）
hayama 0:c2262bac9aa6 916 if (kn==0 && dx2!=0 && dy2!=0){
hayama 0:c2262bac9aa6 917 rd=rd2;
hayama 0:c2262bac9aa6 918 }
hayama 0:c2262bac9aa6 919
hayama 0:c2262bac9aa6 920 // compare the dirction by GPS and compus
hayama 0:c2262bac9aa6 921 if (rd>180) rdCpy=rd-360; else rdCpy=rd;
hayama 0:c2262bac9aa6 922 if (dirCompus>180) dirCompusCpy=dirCompus-360; else dirCompusCpy=dirCompus;
hayama 0:c2262bac9aa6 923 // compare the dirction by GPS and compus
hayama 0:c2262bac9aa6 924 rd=(fabs(rdCpy-dirCompusCpy)>45)? dirCompus: rd;
hayama 0:c2262bac9aa6 925
hayama 0:c2262bac9aa6 926 // 方位偏差の計算し，現在の進行方向から±180度の範囲に直す
hayama 0:c2262bac9aa6 927 dd=dd-rd;
hayama 0:c2262bac9aa6 928 if (dd>180) dd=dd-360;
hayama 0:c2262bac9aa6 929 else if (dd<-180) dd=dd+360;
hayama 0:c2262bac9aa6 930 }
hayama 0:c2262bac9aa6 931
hayama 0:c2262bac9aa6 932 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 933 // gps read by serial task
hayama 0:c2262bac9aa6 934 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 935 void serial_task(void const*) {
hayama 0:c2262bac9aa6 936 char c;
hayama 0:c2262bac9aa6 937 int i=0;
hayama 0:c2262bac9aa6 938 //FILE *fp;
hayama 0:c2262bac9aa6 939
hayama 0:c2262bac9aa6 940 USBHostSerial serial;
hayama 0:c2262bac9aa6 941
hayama 0:c2262bac9aa6 942 while(1) {
hayama 0:c2262bac9aa6 943 while(!serial.connect()) Thread::wait(500); // try to connect a serial device
hayama 0:c2262bac9aa6 944
hayama 0:c2262bac9aa6 945 while (1) {
hayama 0:c2262bac9aa6 946 if (!serial.connected()) break; // if device disconnected, try to connect it again
hayama 0:c2262bac9aa6 947
hayama 0:c2262bac9aa6 948 while (serial.available()) { // receive message
hayama 0:c2262bac9aa6 949 c= serial.getc();
hayama 0:c2262bac9aa6 950 str[i]=c;
hayama 0:c2262bac9aa6 951 if (c == '\n') break;
hayama 0:c2262bac9aa6 952 i++;
hayama 0:c2262bac9aa6 953 }
hayama 0:c2262bac9aa6 954 str[i] = '\0';
hayama 0:c2262bac9aa6 955 i=0;
hayama 0:c2262bac9aa6 956 //fp=fopen("/local/log.txt", "a"); // Open "out.txt" on the local file system for writing
hayama 0:c2262bac9aa6 957 //fprintf(fp, str);
hayama 0:c2262bac9aa6 958 //fclose(fp);
hayama 0:c2262bac9aa6 959
hayama 0:c2262bac9aa6 960 header=strtok(str,",");
hayama 0:c2262bac9aa6 961
hayama 0:c2262bac9aa6 962 //if RMC line
hayama 0:c2262bac9aa6 963 if(strcmp(header,"$GNRMC")==0){
hayama 0:c2262bac9aa6 964 strtok(NULL,",");
hayama 0:c2262bac9aa6 965 strtok(NULL,",");
hayama 0:c2262bac9aa6 966 latitude=strtok(NULL,","); //get latitude
hayama 0:c2262bac9aa6 967 strtok(NULL,",");
hayama 0:c2262bac9aa6 968 longitude=strtok(NULL,","); //get longtude
hayama 0:c2262bac9aa6 969 strtok(NULL,","); // E読み飛ばし
hayama 0:c2262bac9aa6 970 knot=strtok(NULL,","); // 速度 get
hayama 0:c2262bac9aa6 971 direct=strtok(NULL,","); // 進行方向 get
hayama 0:c2262bac9aa6 972 gps_val(); // GPS信号の数値返還
hayama 0:c2262bac9aa6 973 gps_cal(); // 現在地とウェイポイントとの関係を計算
hayama 0:c2262bac9aa6 974 recvGPS++; if(recvGPS>2) recvGPS=2;
hayama 0:c2262bac9aa6 975 printf("LA=%f LO=%f kn=%f rv=%f rd=%f recvGPS=%d\n", latit, longit, kn, rv, rd, recvGPS );
hayama 0:c2262bac9aa6 976 }
hayama 0:c2262bac9aa6 977
hayama 0:c2262bac9aa6 978 if(strcmp(header,"$GPRMC")==0){
hayama 0:c2262bac9aa6 979 strtok(NULL,",");
hayama 0:c2262bac9aa6 980 strtok(NULL,",");
hayama 0:c2262bac9aa6 981 latitude=strtok(NULL,","); //get latitude
hayama 0:c2262bac9aa6 982 strtok(NULL,",");
hayama 0:c2262bac9aa6 983 longitude=strtok(NULL,","); //get longtude
hayama 0:c2262bac9aa6 984 strtok(NULL,","); // E読み飛ばし
hayama 0:c2262bac9aa6 985 knot=strtok(NULL,","); // 速度 get
hayama 0:c2262bac9aa6 986 direct=strtok(NULL,","); // 進行方向 get
hayama 0:c2262bac9aa6 987 gps_val(); // GPS信号の数値返還
hayama 0:c2262bac9aa6 988 gps_cal(); // 現在地とウェイポイントとの関係を計算
hayama 0:c2262bac9aa6 989 recvGPS++; if(recvGPS>2) recvGPS=2;
hayama 0:c2262bac9aa6 990 printf("LA=%f LO=%f kn=%f rv=%f rd=%f recvGPS=%d\n", latit, longit, kn, rv, rd, recvGPS );
hayama 0:c2262bac9aa6 991 }
hayama 0:c2262bac9aa6 992
hayama 0:c2262bac9aa6 993 //if QZQSM line
hayama 0:c2262bac9aa6 994 if(strcmp(header,"$QZQSM")==0 && recvQZSS==0){ //if RMC line
hayama 0:c2262bac9aa6 995 strtok(NULL,","); // 55
hayama 0:c2262bac9aa6 996 //strtok(NULL,","); // 53
hayama 0:c2262bac9aa6 997 messages=strtok(NULL,"*");
hayama 0:c2262bac9aa6 998 printf("%s\n",messages+4); // messages
hayama 0:c2262bac9aa6 999 strncpy(strtmp,messages+4,4); strtmp[4]='\0';
hayama 0:c2262bac9aa6 1000 year=strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1001 strncpy(strtmp,messages+8,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1002 teamNum =strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1003 strncpy(strtmp,messages+10,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1004 numA =strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1005 strncpy(strtmp,messages+12,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1006 sizeA=strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1007 strncpy(strtmp,messages+14,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1008 numB =strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1009 strncpy(strtmp,messages+16,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1010 sizeB=strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1011 strncpy(strtmp,messages+18,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1012 numT=strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1013 strncpy(strtmp,messages+20,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1014 startA=strtol(strtmp,NULL,18);
hayama 0:c2262bac9aa6 1015 strncpy(strtmp,messages+22,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1016 startB=strtol(strtmp,NULL,20);
hayama 0:c2262bac9aa6 1017 for(i=0; i<numT; i++){
hayama 0:c2262bac9aa6 1018 strncpy(strtmp,messages+24+i*4,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1019 targetA[i]=strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1020 strncpy(strtmp,messages+26+i*4,2); strtmp[2]='\0';
hayama 0:c2262bac9aa6 1021 targetB[i]=strtol(strtmp,NULL,16);
hayama 0:c2262bac9aa6 1022 }
hayama 0:c2262bac9aa6 1023 if (teamNum==tNum) recvQZSS=1;
hayama 0:c2262bac9aa6 1024
hayama 0:c2262bac9aa6 1025 printf("year=%d\n",year);
hayama 0:c2262bac9aa6 1026 printf("teamNum=%d\n",teamNum);
hayama 0:c2262bac9aa6 1027 printf("numA=%d\n",numA);
hayama 0:c2262bac9aa6 1028 printf("sizeA=%d\n",sizeA);
hayama 0:c2262bac9aa6 1029 printf("numB=%d\n",numB);
hayama 0:c2262bac9aa6 1030 printf("sizeB=%d\n",sizeB);
hayama 0:c2262bac9aa6 1031 printf("tNum=%d\n",numT);
hayama 0:c2262bac9aa6 1032 printf("startA=%d\n",startA);
hayama 0:c2262bac9aa6 1033 printf("startB=%d\n",startB);
hayama 0:c2262bac9aa6 1034 for(i=0; i<numT; i++){
hayama 0:c2262bac9aa6 1035 printf("targetA[%d]=%d\n",i,targetA[i]);
hayama 0:c2262bac9aa6 1036 printf("targetB[%d]=%d\n",i,targetB[i]);
hayama 0:c2262bac9aa6 1037 }
hayama 0:c2262bac9aa6 1038 printf("recvQZSS=%d\n",recvQZSS);
hayama 0:c2262bac9aa6 1039 }
hayama 0:c2262bac9aa6 1040 Thread::wait(50);
hayama 0:c2262bac9aa6 1041 }
hayama 0:c2262bac9aa6 1042 }
hayama 0:c2262bac9aa6 1043 }
hayama 0:c2262bac9aa6 1044
hayama 0:c2262bac9aa6 1045
hayama 0:c2262bac9aa6 1046 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1047 // GPSによる走行
hayama 0:c2262bac9aa6 1048 // runMode=0:GPS run without Gyro, normal rule( stop waypoint)
hayama 0:c2262bac9aa6 1049 // runMode=1:GPS run without Gyro, Double pylon race( loop waypoint without stop)
hayama 0:c2262bac9aa6 1050 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1051 void runGPS(int runMode){
hayama 0:c2262bac9aa6 1052 lcd.cls(); lcd.locate(0,1); lcd.printf("Start!"); // 目標値までの距離をLEDに表示
hayama 0:c2262bac9aa6 1053 Thread::wait(1000);
hayama 0:c2262bac9aa6 1054
hayama 0:c2262bac9aa6 1055 np=0;
hayama 0:c2262bac9aa6 1056 motorStart();
hayama 0:c2262bac9aa6 1057
hayama 0:c2262bac9aa6 1058 while (resetSW==1){ // STARTスイッチが押されたら抜ける
hayama 0:c2262bac9aa6 1059 // speed control
hayama 0:c2262bac9aa6 1060 if (fabs(dr-wr)>8){ // ステアリングが真っ直ぐで距離が十分あるとき
hayama 0:c2262bac9aa6 1061 if (cntC>spdH) motorAmp=POSC-posL; else motorAmp=POSC-posH;
hayama 0:c2262bac9aa6 1062 } else {
hayama 0:c2262bac9aa6 1063 if (cntC>spdL) motorAmp=POSC-posL; else motorAmp=POSC-posH;
hayama 0:c2262bac9aa6 1064 }
hayama 0:c2262bac9aa6 1065
hayama 0:c2262bac9aa6 1066 while (recvGPS==0) Thread::wait(15); // GPS受信待ち
hayama 0:c2262bac9aa6 1067 recvGPS=0; // reset after GPS recv
hayama 0:c2262bac9aa6 1068 lcd.locate(0,0); lcd.printf("dd=%f \n", dd); // 目標値までの距離をLEDに表示
hayama 0:c2262bac9aa6 1069 lcd.locate(0,1); lcd.printf("dr=%f \n", dr); // 目標値までの距離をLEDに表示
hayama 0:c2262bac9aa6 1070
hayama 0:c2262bac9aa6 1071 // 角度差に応じて方向にステアリングを切る．
hayama 0:c2262bac9aa6 1072 posSteer=strn - dd*ks;
hayama 0:c2262bac9aa6 1073 posSteer=(posSteer<strmin)? strmin: (posSteer>strmax)? strmax: posSteer; // 切れ幅を制限する
hayama 0:c2262bac9aa6 1074 if (kn==0) posSteer=strn; // GPSで方位が得られなかった時（速度＝０）ニュートラルに戻す
hayama 0:c2262bac9aa6 1075 strServo=posSteer;
hayama 0:c2262bac9aa6 1076 Thread::wait(15);
hayama 0:c2262bac9aa6 1077
hayama 0:c2262bac9aa6 1078 // ウェイポイントとの距離を求め，ポイント更新または走行終了判断
hayama 0:c2262bac9aa6 1079 if (dr < wr){
hayama 0:c2262bac9aa6 1080 steerNeutral();
hayama 0:c2262bac9aa6 1081 if(runMode==0) motorStop();
hayama 0:c2262bac9aa6 1082 np++;
hayama 0:c2262bac9aa6 1083 if (np>=wp){
hayama 0:c2262bac9aa6 1084 if(runMode==0) break; // when normal robot car rule,
hayama 0:c2262bac9aa6 1085 else np=0;
hayama 0:c2262bac9aa6 1086 }
hayama 0:c2262bac9aa6 1087 // 次のポイント数を表示
hayama 0:c2262bac9aa6 1088 lcd.locate(0,1); lcd.printf("Next WP:%d ",np);
hayama 0:c2262bac9aa6 1089 if(runMode==0){ // normal rule, wait 6sec
hayama 0:c2262bac9aa6 1090 Thread::wait(6000);
hayama 0:c2262bac9aa6 1091 motorStart();
hayama 0:c2262bac9aa6 1092 }
hayama 0:c2262bac9aa6 1093 }
hayama 0:c2262bac9aa6 1094 }
hayama 0:c2262bac9aa6 1095 }
hayama 0:c2262bac9aa6 1096
hayama 0:c2262bac9aa6 1097 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1098 // GPSによる走行, with gyro
hayama 0:c2262bac9aa6 1099 // runMode=0:GPS run with Gyro, normal rule( stop waypoint)
hayama 0:c2262bac9aa6 1100 // runMode=1:GPS run with Gyro, Double pylon race( loop waypoint without stop)
hayama 0:c2262bac9aa6 1101 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1102 void runGPSandGyro(int runMode){
hayama 0:c2262bac9aa6 1103 lcd.cls(); lcd.locate(0,1); lcd.printf("Start!"); // 目標値までの距離をLEDに表示
hayama 0:c2262bac9aa6 1104 Thread::wait(1000);
hayama 0:c2262bac9aa6 1105
hayama 0:c2262bac9aa6 1106 np=0;
hayama 0:c2262bac9aa6 1107 motorStart();
hayama 0:c2262bac9aa6 1108
hayama 0:c2262bac9aa6 1109 while (resetSW==1){ // STARTスイッチが押されたら抜ける
hayama 0:c2262bac9aa6 1110 // speed control
hayama 0:c2262bac9aa6 1111 if (fabs(dr-wr)>8){ // ステアリングが真っ直ぐで距離が十分あるとき
hayama 0:c2262bac9aa6 1112 if (cntC>spdH) motorAmp=POSC-posL; else motorAmp=POSC-posH;
hayama 0:c2262bac9aa6 1113 } else {
hayama 0:c2262bac9aa6 1114 if (cntC>spdL) motorAmp=POSC-posL; else motorAmp=POSC-posH;
hayama 0:c2262bac9aa6 1115 }
hayama 0:c2262bac9aa6 1116
hayama 0:c2262bac9aa6 1117 //if (recvGPS==2){
hayama 0:c2262bac9aa6 1118 if (recvGPS>=1){
hayama 0:c2262bac9aa6 1119 recvGPS=0; // reset after GPS recv
hayama 0:c2262bac9aa6 1120 ddCpy=dd; // GPSを受信した時の角度差を記録
hayama 0:c2262bac9aa6 1121 lcd.locate(0,0); lcd.printf("dd=%f \n", dd); // 目標値までの距離をLEDに表示
hayama 0:c2262bac9aa6 1122 lcd.locate(0,1); lcd.printf("dr=%f \n", dr); // 目標値までの距離をLEDに表示
hayama 0:c2262bac9aa6 1123
hayama 0:c2262bac9aa6 1124 // 角度差に応じて方向にステアリングを切る．
hayama 0:c2262bac9aa6 1125 posSteer=strn - dd*ksg;
hayama 0:c2262bac9aa6 1126
hayama 0:c2262bac9aa6 1127 posSteer=(posSteer<strmin)? strmin: (posSteer>strmax)? strmax: posSteer; // 切れ幅を制限する
hayama 0:c2262bac9aa6 1128 if (kn==0) posSteer=strn; // GPSで方位が得られなかった時（速度＝０）ニュートラルに戻す
hayama 0:c2262bac9aa6 1129 strServo=posSteer;
hayama 0:c2262bac9aa6 1130 Thread::wait(15);
hayama 0:c2262bac9aa6 1131 gyro_reset();
hayama 0:c2262bac9aa6 1132 }
hayama 0:c2262bac9aa6 1133 // ジャイロで角度モニタ，指定角度でステアリング戻す,ddはターン中も更新されるので更新されないddCpyでターン
hayama 0:c2262bac9aa6 1134 if (fabs(angle)>fabs(ddCpy)){posSteer=strn; strServo=posSteer;}
hayama 0:c2262bac9aa6 1135 // ターン途中かつターン角度が大きい時，GPS受信しても数値を更新させない,recvGPS=0から次のターンまで２秒.待ちすぎか？
hayama 0:c2262bac9aa6 1136 if (posSteer!=strn && fabs(ddCpy)>60) recvGPS=0;
hayama 0:c2262bac9aa6 1137
hayama 0:c2262bac9aa6 1138 // ウェイポイントとの距離を求め，ポイント更新または走行終了判断
hayama 0:c2262bac9aa6 1139 if (dr < wr){
hayama 0:c2262bac9aa6 1140 steerNeutral();
hayama 0:c2262bac9aa6 1141 if(runMode==0) motorStop();
hayama 0:c2262bac9aa6 1142 np++;
hayama 0:c2262bac9aa6 1143 if (np>=wp){
hayama 0:c2262bac9aa6 1144 if(runMode==0) break; // when normal robot car rule,
hayama 0:c2262bac9aa6 1145 else np=0;
hayama 0:c2262bac9aa6 1146 }
hayama 0:c2262bac9aa6 1147 // 次のポイント数を表示
hayama 0:c2262bac9aa6 1148 lcd.locate(0,1); lcd.printf("Next WP:%d ",np);
hayama 0:c2262bac9aa6 1149 if(runMode==0){ // normal rule, wait 6.5sec
hayama 0:c2262bac9aa6 1150 Thread::wait(6500);
hayama 0:c2262bac9aa6 1151 motorStart(); // run straignt for 1 sec.
hayama 0:c2262bac9aa6 1152 }
hayama 0:c2262bac9aa6 1153 }
hayama 0:c2262bac9aa6 1154 Thread::wait(15);
hayama 0:c2262bac9aa6 1155 }
hayama 0:c2262bac9aa6 1156 }
hayama 0:c2262bac9aa6 1157
hayama 0:c2262bac9aa6 1158
hayama 0:c2262bac9aa6 1159 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1160 // QZSSによる走行,
hayama 0:c2262bac9aa6 1161 // runMode=0: without Gyro, normal rule( stop waypoint)
hayama 0:c2262bac9aa6 1162 // runMode=1: with Gyro, normal rule( stop waypoint)
hayama 0:c2262bac9aa6 1163 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1164 void runQZSS(int runMode){
hayama 0:c2262bac9aa6 1165
hayama 0:c2262bac9aa6 1166 lcd.cls();
hayama 0:c2262bac9aa6 1167 lcd.printf("Waiting QZSS\n");
hayama 0:c2262bac9aa6 1168 lcd.locate(0,1);
hayama 0:c2262bac9aa6 1169 lcd.printf("massage: %d \n", tNum);
hayama 0:c2262bac9aa6 1170 while (recvQZSS==0){
hayama 0:c2262bac9aa6 1171 Thread::wait(500);
hayama 0:c2262bac9aa6 1172 if (resetSW==0) break;
hayama 0:c2262bac9aa6 1173 if (startSW==0){tNum++; if (tNum>25) tNum=0;}
hayama 0:c2262bac9aa6 1174 if (setSW==0) {tNum--; if (tNum<0) tNum=25;}
hayama 0:c2262bac9aa6 1175 lcd.locate(0,1); lcd.printf("massage: %d \n", tNum);
hayama 0:c2262bac9aa6 1176 }
hayama 0:c2262bac9aa6 1177
hayama 0:c2262bac9aa6 1178 lcd.cls();
hayama 0:c2262bac9aa6 1179 lcd.printf("QZSS massage \n");
hayama 0:c2262bac9aa6 1180 lcd.printf("Recieved !! \n");
hayama 0:c2262bac9aa6 1181 Thread::wait(3000);
hayama 0:c2262bac9aa6 1182
hayama 0:c2262bac9aa6 1183 // set way point
hayama 0:c2262bac9aa6 1184 wp=0;
hayama 0:c2262bac9aa6 1185 //wy[wp]=wyQ[startA][startB];
hayama 0:c2262bac9aa6 1186 //wx[wp]=wxQ[startA][startB];
hayama 0:c2262bac9aa6 1187 //wp++;
hayama 0:c2262bac9aa6 1188 for(int i=0; i<numT; i++){
hayama 0:c2262bac9aa6 1189 wy[wp]=wyQ[targetA[i]][targetB[i]];
hayama 0:c2262bac9aa6 1190 wx[wp]=wxQ[targetA[i]][targetB[i]];
hayama 0:c2262bac9aa6 1191 wp++;
hayama 0:c2262bac9aa6 1192 }
hayama 0:c2262bac9aa6 1193
hayama 0:c2262bac9aa6 1194 if (runMode==0) runGPS(0); runGPSandGyro(0);
hayama 0:c2262bac9aa6 1195 }
hayama 0:c2262bac9aa6 1196
hayama 0:c2262bac9aa6 1197 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1198 // セットアップ以外の初期値設定
hayama 0:c2262bac9aa6 1199 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1200 void initPara(){
hayama 0:c2262bac9aa6 1201 steerNeutral();
hayama 0:c2262bac9aa6 1202 motorStop();
hayama 0:c2262bac9aa6 1203 }
hayama 0:c2262bac9aa6 1204
hayama 0:c2262bac9aa6 1205 //--------------------------------------------------------------------------
hayama 0:c2262bac9aa6 1206 int main() {
hayama 0:c2262bac9aa6 1207 strServo=POSC; motorAmp=POSC;
hayama 0:c2262bac9aa6 1208 resetSW.mode(PullUp); setSW.mode(PullUp); startSW.mode(PullUp);
hayama 0:c2262bac9aa6 1209 Thread serialTask(serial_task, NULL, osPriorityNormal, 256 * 4);
hayama 0:c2262bac9aa6 1210 timerS.attach_us(&sensor, 1000); // set timer to 1ms for timer interrupt
hayama 0:c2262bac9aa6 1211 ptR.rise(&countR);
hayama 0:c2262bac9aa6 1212 ptL.rise(&countL);
hayama 0:c2262bac9aa6 1213 timer.start();
hayama 0:c2262bac9aa6 1214 gyro_offset();
hayama 0:c2262bac9aa6 1215 readParam();
hayama 0:c2262bac9aa6 1216
hayama 0:c2262bac9aa6 1217 lcd.cls();
hayama 0:c2262bac9aa6 1218 lcd.printf("GPS robocar v1\n");
hayama 0:c2262bac9aa6 1219 lcd.printf("sw:ret set start\n");
hayama 0:c2262bac9aa6 1220 while(resetSW==1 && setSW==1 && startSW==1);
hayama 0:c2262bac9aa6 1221 lcd.locate(0,1); lcd.printf(menuS[pmode]);
hayama 0:c2262bac9aa6 1222
hayama 0:c2262bac9aa6 1223 while(1) {
hayama 0:c2262bac9aa6 1224
hayama 0:c2262bac9aa6 1225 // initialize parameters
hayama 0:c2262bac9aa6 1226 initPara();
hayama 0:c2262bac9aa6 1227 lcd.cls();
hayama 0:c2262bac9aa6 1228 lcd.printf("GPS robocar v1\n");
hayama 0:c2262bac9aa6 1229 lcd.locate(0,1); lcd.printf(menuS[pmode]);
hayama 0:c2262bac9aa6 1230
hayama 0:c2262bac9aa6 1231 while (startSW==1) {
hayama 0:c2262bac9aa6 1232 if (resetSW==0) {
hayama 0:c2262bac9aa6 1233 Thread::wait(10);
hayama 0:c2262bac9aa6 1234 while (resetSW==0);
hayama 0:c2262bac9aa6 1235 Thread::wait(10);
hayama 0:c2262bac9aa6 1236 pmode--;
hayama 0:c2262bac9aa6 1237 if (pmode<0) pmode=nmenu;
hayama 0:c2262bac9aa6 1238 lcd.locate(0,1);
hayama 0:c2262bac9aa6 1239 lcd.printf(menuS[pmode]);
hayama 0:c2262bac9aa6 1240 }
hayama 0:c2262bac9aa6 1241 if (setSW==0) {
hayama 0:c2262bac9aa6 1242 Thread::wait(10);
hayama 0:c2262bac9aa6 1243 while (setSW==0);
hayama 0:c2262bac9aa6 1244 Thread::wait(10);
hayama 0:c2262bac9aa6 1245 pmode++;
hayama 0:c2262bac9aa6 1246 if (pmode>nmenu) pmode=0;
hayama 0:c2262bac9aa6 1247 lcd.locate(0,1);
hayama 0:c2262bac9aa6 1248 lcd.printf(menuS[pmode]);
hayama 0:c2262bac9aa6 1249 }
hayama 0:c2262bac9aa6 1250
hayama 0:c2262bac9aa6 1251 }
hayama 0:c2262bac9aa6 1252 Thread::wait(500);
hayama 0:c2262bac9aa6 1253
hayama 0:c2262bac9aa6 1254 // go selected functions
hayama 0:c2262bac9aa6 1255 switch(pmode){
hayama 0:c2262bac9aa6 1256 case 0: dispGPS(); break; // GPSのテスト
hayama 0:c2262bac9aa6 1257 case 1: setWp(); break; // ウェイポイントのセット
hayama 0:c2262bac9aa6 1258 case 2: dispWp(); break; // display way point
hayama 0:c2262bac9aa6 1259 case 3: clearWp(); break; // ウェイポイントのクリア
hayama 0:c2262bac9aa6 1260 case 4: readWp(); break; // ウェイポイントのread
hayama 0:c2262bac9aa6 1261 case 5: writeWp(); break; // ウェイポイントのread
hayama 0:c2262bac9aa6 1262 case 6: setTeamNum(); break; // set team number for QZSS
hayama 0:c2262bac9aa6 1263 case 7: setQZWp(); break; // (1,1)(1,7)(7,1)(7,7)の４点だけ読み書き、他の点は計算
hayama 0:c2262bac9aa6 1264 case 8: readQZWp4(); break; // (1,1)(1,7)(7,1)(7,7)の４点だけ読み書き、他の点は計算
hayama 0:c2262bac9aa6 1265 case 9: writeQZWp4(); break; // (1,1)(1,7)(7,1)(7,7)の４点だけ読み書き、他の点は計算
hayama 0:c2262bac9aa6 1266 case 10: dispQmsg(); break; // ウェイポイントのクリア
hayama 0:c2262bac9aa6 1267 case 11: clearQmsg(); break; // ウェイポイントのread
hayama 0:c2262bac9aa6 1268 case 12: readQmsg(); break; // ウェイポイントのread
hayama 0:c2262bac9aa6 1269 case 13: writeQmsg(); break; // ウェイポイントのread
hayama 0:c2262bac9aa6 1270 case 14: runGPS(1); break; // GPS走行, 0：WPで止まる、１：WPで止まらない
hayama 0:c2262bac9aa6 1271 case 15: runGPSandGyro(1); break; // ジャイロを使ったGPS走行,０：WPで止まる、１：WPで止まらない
hayama 0:c2262bac9aa6 1272 case 16: runQZSS(0); break; // QZSS-GPS走行(WPで止まる)
hayama 0:c2262bac9aa6 1273 case 17: runQZSS(1); break; // QZSS-ジャイロを使ったGPS走行(WPで止まる)
hayama 0:c2262bac9aa6 1274 case 18: testServo(); break; // サーボの動作テスト
hayama 0:c2262bac9aa6 1275 case 19: testMotor(); break; // モーターの動作テスト
hayama 0:c2262bac9aa6 1276 case 20: initNeutral(); break; // ステアリングニュートラル位置の設定
hayama 0:c2262bac9aa6 1277 case 21: initStrmin(); break; // ステアリングの最小値の設定
hayama 0:c2262bac9aa6 1278 case 22: initStrmax(); break; // ステアリングの最大値の設定
hayama 0:c2262bac9aa6 1279 case 23: initWr(); break; // ウェイポイントの半径設定
hayama 0:c2262bac9aa6 1280 case 24: initKs(); break; // 舵角計算の比例値設定
hayama 0:c2262bac9aa6 1281 case 25: initKsG(); break; // 舵角計算の比例値設定
hayama 0:c2262bac9aa6 1282 case 26: initPosH(); break; // amp pos for high speed
hayama 0:c2262bac9aa6 1283 case 27: initPosL(); break; // amp pos for low speed
hayama 0:c2262bac9aa6 1284 case 28: initSpdH(); break; // 走行スピードhighの設定
hayama 0:c2262bac9aa6 1285 case 29: initSpdL(); break; // 走行スピードlowの設定
hayama 0:c2262bac9aa6 1286 case 30: testRun(); break; // test run
hayama 0:c2262bac9aa6 1287 case 31: writeParam(); break; // ウェイポイントのシリアル出力
hayama 0:c2262bac9aa6 1288 case 32: readParam(); break; // パラメータのシリアル出力
hayama 0:c2262bac9aa6 1289 case 33: testEncoder(); break;
hayama 0:c2262bac9aa6 1290 case 34: testGyro(); break;
hayama 0:c2262bac9aa6 1291 case 35: testCompus(); break;
hayama 0:c2262bac9aa6 1292 }
hayama 0:c2262bac9aa6 1293 Thread::wait(500);
hayama 0:c2262bac9aa6 1294 }
hayama 0:c2262bac9aa6 1295 }
hayama 0:c2262bac9aa6 1296