Kiyoteru Hayama
This program is made for the GPS Robot Car contest 2014
Dependencies: Servo TextLCD USBHost mbed
Presentation for Geospatial EXPO 2014. /media/uploads/hayama/presen.pdf
Diff: main.cpp
- Revision:
- 0:c2262bac9aa6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Nov 13 15:33:47 2014 +0000
@@ -0,0 +1,1296 @@
+#include "mbed.h"
+#include "TextLCD.h"
+#include "Servo.h"
+#include "USBHostSerial.h"
+
+#define POSC 0.5
+#define KANG 5 // factor of gyro output to angle rot right + val
+#define GYROADJ (-0.0001)
+#define FSMTHS 1 // smoothing factor for sensors
+
+// 各種定数または初期値の設定
+#define PI 3.14159 // pai
+#define DEF_TNUM 1
+
+// メニュー関係
+int nmenu=36;
+const char *menuS[36]={ // メニューの文字列
+ "Disp GPS ","Set way point ","Disp way point ","Clear way point ","Read way point ",
+ "Write way point ","Set team number ","Set QZSS Wp ","Read QZSS Wp ","Write QZSS Wp ",
+ "Disp QZSS Msg ","Clear QZSS Msg ","Read QZSS Msg ","Write QZSS Msg ","Run GPS no Gyro ",
+ "Run GPS use Gyro","Run QZSS no Gyro","Run QZSS use Gyr","Test servo ","Test motor ",
+ "Init neutral ","Init steer min ","Set steer max ","Init WP radius ","Init Ks ",
+ "Init KsGyro ","Init PosH ","Init PosL ","Init Speed H ","Init Speed L ",
+ "Test Run ","Write parameter ","Read Parameter ","Test encoder ","Test Gyro ",
+ "Test Compus "
+};
+
+const double arrayCompus[]={180,225,45,0,135,270,90,315};
+volatile double dirCompus;
+double rdCpy, dirCompusCpy;
+
+int pmode=0;
+volatile int recvGPS=0;
+volatile int recvQZSS=0;
+volatile long encR=0, encRB=0;
+volatile long encL=0, encLB=0;
+volatile int cntR=0;
+volatile int cntL=0;
+volatile int cntC=0; // center both cntR and cntL
+volatile int cntE=0; // timing counter for cntR/L read
+
+// gyro sensor
+volatile float gyro, gyItg, gyOff;
+volatile float angle;
+
+// ウェイポイント設定(基準点からのN,E方向への座標)、最高16ポイント
+int np=0; // soukoutyuu目標ポイントナンバー
+int wp=0; // number of waypoint, initial value read from file of "waypoint.txt"
+double wy[16]={0}; // latitude of waypoints
+double wx[16]={0}; // longitude of waypoints
+
+// QZSS-ウェイポイント設定(基準点からのN,E方向への座標)、最高16ポイント
+int npQ=0; // 目標ポイントナンバー
+int wpQ=0; // wp:0-15まで,ウェイポイントの数 -1
+double wyQ11,wxQ11;
+double wyQ[8][8]={0}; // index 1 to 7, wyQ[A],[B]
+double wxQ[8][8]={0}; // index 1 to 7, wxQ[A],[B]
+
+double xQ11,yQ11,xQ71,yQ71,xQ17,yQ17,xQ77,yQ77,dxQ71,dyQ71,dxQ17,dyQ17;
+double theta;
+double magx,magy;
+
+
+// 走行処理関係変数
+double gy, gx; // 北緯，東経を原点からの距離（メートル）に数値化
+double ry, rx, rv, rd; // 北緯（正規化），東経（正規化），速度（ノット），方向（単位：度）の数値化
+double dy, dx, dr, dd; // ウェイポイントに対する北緯，東経の差分(m)，距離の差分，方位の差分
+double ddCpy; // 走行中にddの保存
+
+double wr; // ウェイポイントの半径
+double ks; // ステアリング切れ角比例係数
+double ksg; // ステアリング切れ角比例係数, with gyro
+
+float strn; // ニュートラルの位置
+float posSteer; // ステアリングの位置
+float strmin; // ステアリングの最小値
+float strmax; // ステアリングの最大値
+float posH, posL; // servo position for high/low speed of motor
+int spdH,spdL; // スピード, count of encoder per 100ms
+
+
+// GPS受信用変数，ポインタ
+char str[200]; // GPSほか読み取りのための文字列バッファ
+char *header;
+char *latitude, *longitude, *knot, *direct;
+double latit, longit, kn; // 緯度，経度，ノット保存用の変数
+double latitB, longitB, rdB; // 差分で方位計算する時に前の座標保持用, 方位保管用
+double dy2, dx2, rd2; // 一つ前の値との北緯，東経の差分(m)，差分で求めた方位
+
+// QZSS
+char strtmp[10];
+char *messages;
+long teamNum,year,numA,sizeA,numB,sizeB,numT;
+long startA, startB;
+long targetA[20];
+long targetB[20];
+long tNum=0;
+
+InterruptIn ptR(p5);
+InterruptIn ptL(p6);
+//DigitalIn S3(p7)-S4(p8);
+
+DigitalIn resetSW(p13);
+DigitalIn setSW(p14);
+DigitalIn startSW(p9);
+DigitalOut led(p21);
+
+AnalogIn gyroOut(p15);
+//AnalogIn A2(p16)-(p20);
+
+Servo strServo(p26);
+Servo motorAmp(p25);
+//Servo servoC3(p24)-(p21);
+
+BusIn compus(p18, p19, p20); // compus, 3bit 8 direction
+BusOut leds( LED3, LED2, LED1); // for LED display
+TextLCD lcd(p11, p12, p27, p28, p29, p30); // rs, e, d4-d7
+Ticker timerS; // defince interval timer
+Timer timer;
+Serial pc(USBTX, USBRX);
+LocalFileSystem local("local");
+
+
+//--------------------------------------------------------------------------
+// 走行パラメータを読み書き
+//--------------------------------------------------------------------------
+void writeParam(){
+ FILE *fp = fopen("/local/param.txt","w");
+ fprintf(fp, "%f\n",wr);
+ fprintf(fp, "%f\n",ks);
+ fprintf(fp, "%f\n",ksg);
+ fprintf(fp, "%f\n",strmin);
+ fprintf(fp, "%f\n",strmax);
+ fprintf(fp, "%f\n",strn);
+ fprintf(fp, "%f\n",posH);
+ fprintf(fp, "%f\n",posL);
+ fprintf(fp, "%d\n",spdH);
+ fprintf(fp, "%d\n",spdL);
+ fclose(fp);
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+void readParam(){
+ char st[20];
+ FILE *fp = fopen("/local/param.txt","r");
+ fgets(st,20,fp); wr=atof(st);
+ fgets(st,20,fp); ks=atof(st);
+ fgets(st,20,fp); ksg=atof(st);
+ fgets(st,20,fp); strmin=atof(st);
+ fgets(st,20,fp); strmax=atof(st);
+ fgets(st,20,fp); strn=atof(st);
+ fgets(st,20,fp); posH=atof(st);
+ fgets(st,20,fp); posL=atof(st);
+ fgets(st,20,fp); spdH=atoi(st);
+ fgets(st,20,fp); spdL=atoi(st);
+ fclose(fp);
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+//--------------------------------------------------------------------------
+// ウェイポイントの保管と読み出し
+//--------------------------------------------------------------------------
+void writeWp(){
+ FILE *fp = fopen("/local/waypoint.txt","w");
+ fprintf(fp, "%d\n",wp);
+ for(int i=0;i<wp;i++){
+ fprintf(fp,"%f\n",wy[i]);
+ fprintf(fp,"%f\n",wx[i]);
+ }
+ fclose(fp);
+
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+void readWp(){
+ char st[20];
+ FILE *fp = fopen("/local/waypoint.txt","r");
+ fgets(st,20,fp); wp=atof(st);
+ for(int i=0;i<wp;i++){
+ fgets(st,20,fp); wy[i]=atof(st);
+ fgets(st,20,fp); wx[i]=atof(st);
+ }
+ fclose(fp);
+
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+// QZSS message file out
+void writeQmsg(){
+ FILE *fp = fopen("/local/QZmsg.txt","w");
+ fprintf(fp, "%d\n",year);
+ fprintf(fp, "%d\n",teamNum);
+ fprintf(fp, "%d\n",numA);
+ fprintf(fp, "%d\n",sizeA);
+ fprintf(fp, "%d\n",numB);
+ fprintf(fp, "%d\n",sizeB);
+ fprintf(fp, "%d\n",numT);
+ fprintf(fp, "%d\n",startA);
+ fprintf(fp, "%d\n",startB);
+ for(int i=0; i<numT; i++){
+ fprintf(fp,"%d\n",targetA[i]); // 座標のインデックスは１から、配列のインデックスは０から
+ fprintf(fp,"%d\n",targetB[i]);
+ }
+ fprintf(fp, "%d\n",recvQZSS);
+ fclose(fp);
+
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+// QZSS message file in
+void readQmsg(){
+ char st[20];
+ FILE *fp = fopen("/local/QZmsg.txt","r");
+ fgets(st,20,fp); year=atoi(st);
+ fgets(st,20,fp); teamNum=atoi(st);
+ fgets(st,20,fp); numA=atoi(st);
+ fgets(st,20,fp); sizeA=atoi(st);
+ fgets(st,20,fp); numB=atoi(st);
+ fgets(st,20,fp); sizeB=atoi(st);
+ fgets(st,20,fp); numT=atoi(st);
+ fgets(st,20,fp); startA=atoi(st);
+ fgets(st,20,fp); startB=atoi(st);
+ for(int i=0; i<numT; i++){
+ fgets(st,20,fp); targetA[i]=atof(st); // 座標のインデックスは１から、配列のインデックスは０から
+ fgets(st,20,fp); targetB[i]=atof(st);
+ }
+ fgets(st,20,fp); recvQZSS=atoi(st);
+ if (recvQZSS==1) tNum=teamNum;
+ fclose(fp);
+
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+//--------------------------------------------------------------------------
+// ウェイポイントの設定
+// (GPSが受信できてない時はポイント追加できない)
+//--------------------------------------------------------------------------
+void setWp(){
+ lcd.locate(0,1); lcd.printf("present WP: %d ",wp); // ウェイポイント番号を進表示
+ while (resetSW==1){ // STARTスイッチが押されたら抜ける
+
+ // start-swが押されたらウェイポイントを記録
+ if (startSW==0) {
+ Thread::wait(10);
+ while(startSW==0); // チャタリング防止
+ Thread::wait(10);
+
+ // GPRMCを受信できて値が正常であるまで待つ
+ while(recvGPS==0 || (latit==0) || (longit==0));
+
+ wy[wp] = latit;
+ wx[wp] = longit;
+ lcd.locate(0,1); lcd.printf("set WP: %d ",wp);
+ Thread::wait(2000);
+ lcd.locate(0,0); lcd.printf("wy[%d]=%f \n", wp, wy[wp]);
+ lcd.locate(0,1); lcd.printf("wx[%d]=%f \n", wp, wx[wp]);
+ Thread::wait(2000);
+ wp++; if (wp>15) wp=15; // 次のウェイポイント入力状態にするウェイポイントの追加は１５まで、０からで計１６個
+ // ウェイポイント追加
+ }
+
+ // set-swが押されたらウェイポイントdecrement
+ if (setSW==0) {
+ Thread::wait(10);
+ while(setSW==0); // チャタリング防止
+ Thread::wait(10);
+ wp--; if (wp<0) wp=0;
+ wy[wp] =0;
+ wx[wp] =0;
+ lcd.locate(0,1); lcd.printf("delete WP: %d ",wp);
+ Thread::wait(2000);
+ lcd.locate(0,0); lcd.printf("wy[%d]=%f \n", wp, wy[wp]);
+ lcd.locate(0,1); lcd.printf("wx[%d]=%f \n", wp, wx[wp]);
+ Thread::wait(2000);
+ }
+
+ lcd.cls();
+ lcd.printf("LA=%f \n", latit);
+ lcd.printf("LO=%f \n", longit);
+ Thread::wait(200);
+ }
+}
+
+//--------------------------------------------------------------------------
+// display way point
+//--------------------------------------------------------------------------
+void dispWp(){
+ if(wp==0){
+ lcd.locate(0,1); lcd.printf("no way point !! ");
+ return;
+ }
+ int i=0;
+ lcd.cls();
+ while(resetSW==1){
+ if (setSW==0){
+ Thread::wait(10);
+ while (setSW==0);
+ Thread::wait(10);
+ i--; if (i<0) i=wp-1;
+ }
+ if (startSW==0){
+ Thread::wait(10);
+ while (startSW==0);
+ Thread::wait(10);
+ i++; if (i==wp) i=0;
+ }
+ lcd.locate(0,0); lcd.printf("wy[%d]=%f \n", i, wy[i]);
+ lcd.locate(0,1); lcd.printf("wx[%d]=%f \n", i, wx[i]);
+ Thread::wait(200);
+ }
+ Thread::wait(200);
+}
+//--------------------------------------------------------------------------
+// ウェイポイントの消去
+//--------------------------------------------------------------------------
+void clearWp(){
+ wp=0;
+ for(int i=0;i<16;i++){ wy[i]=0; wx[i]=0; }
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+//--------------------------------------------------------------------------
+// QZSS display way point
+//--------------------------------------------------------------------------
+void dispQmsg(){
+ int i=0;
+
+ if (recvQZSS==1){
+ lcd.locate(0,1); lcd.printf("QZSS message OK!");
+ } else {
+ lcd.locate(0,1); lcd.printf("QZSS no message ");
+ }
+ Thread::wait(3000);
+
+ while(resetSW==1){
+ if (setSW==0){
+ Thread::wait(10);
+ while (setSW==0);
+ Thread::wait(10);
+ i--; if (i<0) i=24;
+ }
+ if (startSW==0){
+ Thread::wait(10);
+ while (startSW==0);
+ Thread::wait(10);
+ i++; if (i>24) i=0;
+ }
+ switch (i){
+ case 0: lcd.locate(0,1); lcd.printf("year=%d \n", year); break;
+ case 1: lcd.locate(0,1); lcd.printf("teamNum=%d \n", teamNum); break;
+ case 2: lcd.locate(0,1); lcd.printf("numA=%d \n", numA); break;
+ case 3: lcd.locate(0,1); lcd.printf("sizeA=%d \n", sizeA); break;
+ case 4: lcd.locate(0,1); lcd.printf("numB=%d \n", numB); break;
+ case 5: lcd.locate(0,1); lcd.printf("sizeB=%d \n", sizeB); break;
+ case 6: lcd.locate(0,1); lcd.printf("numT=%d \n", numT); break;
+ case 7: lcd.locate(0,1); lcd.printf("startA=%d \n", startA); break;
+ case 8: lcd.locate(0,1); lcd.printf("startB=%d \n", startB); break;
+ case 9: lcd.locate(0,1); lcd.printf("targetA[1]=%d \n", targetA[0]); break; // 座標のインデックスは１から、配列のインデックスは０から
+ case 10: lcd.locate(0,1); lcd.printf("targetB[1]=%d \n", targetB[0]); break;
+ case 11: lcd.locate(0,1); lcd.printf("targetA[2]=%d \n", targetA[1]); break;
+ case 12: lcd.locate(0,1); lcd.printf("targetB[2]=%d \n", targetB[1]); break;
+ case 13: lcd.locate(0,1); lcd.printf("targetA[3]=%d \n", targetA[2]); break;
+ case 14: lcd.locate(0,1); lcd.printf("targetB[3]=%d \n", targetB[2]); break;
+ case 15: lcd.locate(0,1); lcd.printf("targetA[4]=%d \n", targetA[3]); break;
+ case 16: lcd.locate(0,1); lcd.printf("targetB[4]=%d \n", targetB[3]); break;
+ case 17: lcd.locate(0,1); lcd.printf("targetA[5]=%d \n", targetA[4]); break;
+ case 18: lcd.locate(0,1); lcd.printf("targetB[5]=%d \n", targetB[4]); break;
+ case 19: lcd.locate(0,1); lcd.printf("targetA[6]=%d \n", targetA[5]); break;
+ case 20: lcd.locate(0,1); lcd.printf("targetB[6]=%d \n", targetB[5]); break;
+ case 21: lcd.locate(0,1); lcd.printf("targetA[7]=%d \n", targetA[6]); break;
+ case 22: lcd.locate(0,1); lcd.printf("targetB[7]=%d \n", targetB[6]); break;
+ case 23: lcd.locate(0,1); lcd.printf("targetA[8]=%d \n", targetA[7]); break;
+ case 24: lcd.locate(0,1); lcd.printf("targetB[8]=%d \n", targetB[7]); break;
+ }
+ Thread::wait(100);
+ }
+ Thread::wait(200);
+}
+
+//--------------------------------------------------------------------------
+// QZSS ウェイポイントの消去
+//--------------------------------------------------------------------------
+void clearQmsg(){
+ for(int i=0;i<numT;i++){ targetA[i]=targetB[i]=0; }
+ year=teamNum=numA=sizeA=numB=sizeB=numT=startA=startB=0;
+ recvQZSS=0;
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+
+//--------------------------------------------------------------------------
+// set team number, inc by startSW, dec by setSW, max 25
+// compare to the teamNum from QZSS massage
+//--------------------------------------------------------------------------
+void setTeamNum(){
+ while(resetSW==1){
+ if (startSW==0){tNum++; if (tNum>25) tNum=0;}
+ if (setSW==0) {tNum--; if (tNum<0) tNum=25;}
+ lcd.locate(0,1); lcd.printf("team number=%d \n", tNum);
+ Thread::wait(200);
+ }
+}
+
+
+// calculation of all QZSS waypoint from 4 point
+void calcQZWp(){
+
+ wxQ11=wxQ[1][1]; wyQ11=wyQ[1][1];
+ xQ11=wxQ[1][1]-wxQ11; yQ11=wyQ[1][1]-wyQ11; // (A1,B1)
+ xQ71=wxQ[7][1]-wxQ11; yQ71=wyQ[7][1]-wyQ11; // (A7,B1)
+ xQ17=wxQ[1][7]-wxQ11; yQ17=wyQ[1][7]-wyQ11; // (A1,B7)
+ xQ77=wxQ[7][7]-wxQ11; yQ77=wyQ[7][7]-wyQ11; // (A7,B7)
+
+ dxQ71=xQ71-xQ11; dyQ71=yQ71-yQ11;
+ dxQ17=xQ17-xQ11; dyQ17=yQ17-yQ11;
+
+ theta=atan(dxQ71/dyQ71);
+ magx=sqrt(dxQ71*dxQ71+dyQ71*dyQ71)/6;
+ magy=sqrt(dxQ17*dxQ17+dyQ17*dyQ17)/6;
+
+ for (int i=1;i<8;i++){
+ for(int j=1;j<8;j++){
+ wxQ[i][j]=magx*(j-1)*cos(theta)-magy*(i-1)*sin(theta)+wxQ11;
+ wyQ[i][j]=magx*(j-1)*sin(theta)+magy*(i-1)*cos(theta)+wyQ11;
+ }
+ }
+}
+
+
+// set QZSS way point for (A1,B1)(A7,B1)(A1,B7)(A7,B7)
+void setQZWp(){
+ int qa=1,qb=1;
+ int qn=3;
+
+ while (resetSW==1){ // STARTスイッチが押されたら抜ける
+
+ // set-swが押されたらchange A,B axis
+ if (setSW==0) {
+ Thread::wait(10);
+ while(setSW==0); // チャタリング防止
+ Thread::wait(10);
+
+ qn++; if (qn>3) qn=0; // 次のウェイポイント入力状態にするウェイポイントの追加は１５まで、０からで計１６個
+ switch(qn){
+ case 0: qa=1; qb=1; break;
+ case 1: qa=7; qb=1; break;
+ case 2: qa=1; qb=7; break;
+ case 3: qa=7; qb=7; break;
+ }
+ lcd.cls();
+ lcd.locate(0,1); lcd.printf("(A%d,B%d)", qa,qb);
+ Thread::wait(2000);
+ lcd.cls();
+ lcd.locate(0,0); lcd.printf("wyQ[%d][%d]=%f \n", qa,qb, wyQ[qa][qb]);
+ lcd.locate(0,1); lcd.printf("wxQ[%d][%d]=%f \n", qa,qb, wxQ[qa][qb]);
+ Thread::wait(2000);
+ }
+
+ // start-swが押されたらウェイポイントを記録
+ if (startSW==0) {
+ Thread::wait(10);
+ while(startSW==0); // チャタリング防止
+ Thread::wait(10);
+
+ // GPRMCを受信できて値が正常であるまで待つ
+ while(recvGPS==0 || (latit==0) || (longit==0));
+
+ wyQ[qa][qb] = latit;
+ wxQ[qa][qb] = longit;
+ lcd.cls();
+ lcd.locate(0,0); lcd.printf("wyQ[%d][%d]=%f \n", qa,qb, wyQ[qa][qb]);
+ lcd.locate(0,1); lcd.printf("wxQ[%d][%d]=%f \n", qa,qb, wxQ[qa][qb]);
+ Thread::wait(2000);
+ }
+
+ lcd.cls();
+ lcd.printf("LA=%f \n", latit);
+ lcd.printf("LO=%f \n", longit);
+ Thread::wait(200);
+ }
+ calcQZWp();
+}
+
+// file out QZSS way point, 4 point only
+void writeQZWp4(){
+ FILE *fp = fopen("/local/QZpoint4.txt","w");
+ fprintf(fp, "%f\n",xQ11);
+ fprintf(fp, "%f\n",yQ11);
+ fprintf(fp, "%f\n",xQ71);
+ fprintf(fp, "%f\n",yQ71);
+ fprintf(fp, "%f\n",xQ17);
+ fprintf(fp, "%f\n",yQ17);
+ fprintf(fp, "%f\n",xQ77);
+ fprintf(fp, "%f\n",yQ77);
+ fclose(fp);
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+// file read QZSS way point, 4 point only
+void readQZWp4(){
+ char st[20];
+ FILE *fp = fopen("/local/QZpoint4.txt","r");
+ fgets(st,20,fp); xQ11=atof(st);
+ fgets(st,20,fp); yQ11=atof(st);
+ fgets(st,20,fp); xQ71=atof(st);
+ fgets(st,20,fp); yQ71=atof(st);
+ fgets(st,20,fp); xQ17=atof(st);
+ fgets(st,20,fp); yQ17=atof(st);
+ fgets(st,20,fp); xQ77=atof(st);
+ fgets(st,20,fp); yQ77=atof(st);
+ fclose(fp);
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+ calcQZWp();
+}
+
+
+// file out QZSS way point
+void writeQZWp(){
+ FILE *fp = fopen("/local/QZpoint.txt","w");
+ for(int i=1;i<8;i++){
+ for(int j=1;j<8;j++){
+ fprintf(fp, "%f\n",wyQ[i][j]);
+ fprintf(fp, "%f\n",wxQ[i][j]);
+ }
+ }
+ fclose(fp);
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+// file read QZSS way point
+void readQZWp(){
+ char st[20];
+ FILE *fp = fopen("/local/QZpoint.txt","r");
+ for(int i=1;i<8;i++){
+ for(int j=1;j<8;j++){
+ fgets(st,20,fp); wyQ[i][j]=atof(st);
+ fgets(st,20,fp); wxQ[i][j]=atof(st);
+ }
+ }
+ fclose(fp);
+ lcd.locate(0,1); lcd.printf("Completed! ");
+ Thread::wait(1000);
+}
+
+
+//--------------------------------------------------------------------------
+// スムージング関数(cData:現在のデータ，pData:一つ前のデータ,factor:係数，大きい現在地の割合大きい
+//--------------------------------------------------------------------------
+float smooth(float cData, float pData, float factor){
+ return(cData*factor + pData*(1-factor));
+}
+
+//--------------------------------------------------------------------------
+// count up by encoder
+//--------------------------------------------------------------------------
+void countR(){
+ encR++;
+}
+void countL(){
+ encL++;
+}
+
+//--------------------------------------------------------------------------
+// ジャイロのオフセット取得
+//--------------------------------------------------------------------------
+int gyro_offset(void){
+ int i,tmp;
+
+ lcd.cls();
+ Thread::wait(1000);
+ gyOff=0;
+ for (i=0;i<16;i++){
+ gyOff+=gyroOut; // read gyro sensor
+ Thread::wait(10);
+ }
+ gyOff=gyOff/16+GYROADJ;
+
+ gyItg=0;
+ for (i=0;i<10;i++){
+ tmp=(int)(fabs(angle));
+ lcd.locate(0,1); lcd.printf("angle=%d \n", tmp);
+ Thread::wait(100);
+ }
+ return ((int)fabs(angle)); // スタートスイッチで復帰，ただしジャイロ使えないレベル
+}
+
+//--------------------------------------------------------------------------
+// ジャイロのリセット
+//--------------------------------------------------------------------------
+void gyro_reset(void){
+ gyItg=0;
+ angle=0;
+}
+
+//--------------------------------------------------------------------------
+// interrupt by us timerS
+//--------------------------------------------------------------------------
+void sensor() {
+ // read gyro sensor
+ gyro=-(gyroOut-gyOff);
+ gyItg+=gyro;
+ angle=gyItg*KANG;
+
+ cntE++; if (cntE>200){
+ cntE=0;
+ cntR=encR-encRB;
+ cntL=encL-encLB;
+ encRB=encR;
+ encLB=encL;
+ cntC=(cntR>cntL)? cntR: cntL; // max of cntR/L.
+ }
+ dirCompus=arrayCompus[compus];
+}
+
+//--------------------------------------------------------------------------
+// テスト，パラメータ設定
+//--------------------------------------------------------------------------
+
+// サーボのテスト
+void testServo() {
+ strServo=strmin; // 最初左に曲がる
+ Thread::wait(1000);
+ strServo=strmax; // 次に右に曲がる
+ Thread::wait(1000);
+ strServo=strn; // ニュートラル
+ Thread::wait(1000);
+}
+
+// モータテスト、実行スイッチで正転、セットスイッチで逆転、リセットを押すまでモータ回り続ける
+void testMotor(){
+ while(resetSW==1){
+ if (startSW==0) motorAmp=POSC-posL;
+ else if (setSW==0) motorAmp=POSC+posL;
+ //else motorAmp=POSC;
+ }
+}
+
+// encoderテスト
+void testEncoder(){
+ lcd.cls();
+ while(resetSW==1){
+ lcd.locate(0,0); lcd.printf("eR%d eL%d \n", encR, encL);
+ lcd.locate(0,1); lcd.printf("cR=%d cL=%d cC=%d \n", cntR, cntL, cntC);
+ }
+}
+
+// test gyro sensor
+void testGyro(){
+ lcd.cls();
+ gyro_reset();
+ while(resetSW==1){
+ if (setSW==0) {
+ gyro_offset();
+ gyro_reset();
+ }
+ lcd.locate(0,0); lcd.printf("gyItg=%f \n", gyItg);
+ lcd.locate(0,1); lcd.printf("angle=%f \n", angle);
+ }
+}
+
+//--------------------------------------------------------------------------
+// モータスタート
+//--------------------------------------------------------------------------
+void motorStart(){
+ motorAmp=POSC-posL;
+ Thread::wait(1000);
+}
+
+//--------------------------------------------------------------------------
+// モータストップ
+//--------------------------------------------------------------------------
+void motorStop(){
+ motorAmp=POSC;
+ Thread::wait(500);
+}
+
+//--------------------------------------------------------------------------
+// ステアリングニュートラル
+//--------------------------------------------------------------------------
+void steerNeutral(){
+ strServo=strn;
+}
+
+//--------------------------------------------------------------------------
+// ニュートラルのセット、スタートでプラス、セットでマイナス、リセットで戻る
+//--------------------------------------------------------------------------
+void initNeutral(){
+ while(resetSW==1){
+ if (startSW==0){strn+=0.01; if (strn>strmax) strn=strmax; }
+ if (setSW==0) {strn-=0.01; if (strn<strmin) strn=strmin; }
+ strServo=strn; // ニュートラル
+ lcd.locate(0,1); lcd.printf("str nuetral=%f \n", strn);
+ Thread::wait(100);
+ }
+}
+
+//--------------------------------------------------------------------------
+// ステアリング最小値のセット、スタートでプラス、セットでマイナス、リセットで戻る
+//--------------------------------------------------------------------------
+void initStrmin(){
+ while(resetSW==1){
+ if (startSW==0) if (strmin<0.5) strmin+=0.02; // 可変範囲は45-90度まで
+ if (setSW==0) if (strmin>0.02) strmin-=0.02;
+ strServo=strmin; // ステアリング最小値に動かす
+ lcd.locate(0,1); lcd.printf("strmin=%f \n", strmin);
+ Thread::wait(100);
+ }
+}
+
+//--------------------------------------------------------------------------
+// ステアリング最大値のセット、スタートでプラス、セットでマイナス、リセットで戻る
+//--------------------------------------------------------------------------
+void initStrmax(){
+ while(resetSW==1){
+ if (startSW==0) if (strmax<0.98) strmax+=0.02; // 可変範囲は90-135度まで
+ if (setSW==0) if (strmax>0.5 ) strmax-=0.02;
+ strServo=strmax; // ステアリング最大値に動かす
+ lcd.locate(0,1); lcd.printf("strmax=%f \n", strmax);
+ Thread::wait(200);
+ }
+}
+
+//--------------------------------------------------------------------------
+// ウェイポイントの半径をセット、スタートでプラス、セットでマイナス、リセットで戻る
+// ２進数で半径を表示
+//--------------------------------------------------------------------------
+void initWr(){
+ while(resetSW==1){
+ if (startSW==0){wr+=0.1; if (wr>10) wr=10; }
+ if (setSW==0) {wr-=0.1; if (wr<0.1) wr=0.1; }
+ lcd.locate(0,1); lcd.printf("wr=%f \n", wr);
+ Thread::wait(200);
+ }
+}
+
+//--------------------------------------------------------------------------
+// ステアリング制御の比例係数をセット、スタートでプラス、セットでマイナス、リセットで戻る
+// 比例係数の50倍を２進数で表示
+//--------------------------------------------------------------------------
+void initKs(){
+ while(resetSW==1){
+ if (startSW==0){ks+=0.001; if (ks>1) ks=1;}
+ if (setSW==0) {ks-=0.001; if (ks<-1) ks=-1;}
+ lcd.locate(0,1); lcd.printf("ks=%f \n", ks);
+ Thread::wait(200);
+ }
+}
+
+void initKsG(){
+ while(resetSW==1){
+ if (startSW==0){ksg+=0.001; if (ksg>1) ksg=1;}
+ if (setSW==0) {ksg-=0.001; if (ksg<-1) ksg=-1;}
+ lcd.locate(0,1); lcd.printf("ksg=%f \n", ksg);
+ Thread::wait(200);
+ }
+}
+
+//--------------------------------------------------------------------------
+// 直進スピードをセット、スタートでプラス、セットでマイナス、リセットで戻る
+// スピードの1/16を2進数で表示
+//--------------------------------------------------------------------------
+void initPosH(){
+ while(resetSW==1){
+ motorAmp=POSC-posH; // モータを回転させる
+ if (startSW==0){posH+=0.01; if (posH>0.8) posH=0.8;}
+ if (setSW==0) {posH-=0.01; if (posH<0) posH=0;}
+ lcd.locate(0,1); lcd.printf("PosH=%f \n", posH);
+ Thread::wait(200);
+ }
+}
+
+void initPosL(){
+ while(resetSW==1){
+ motorAmp=POSC-posL; // モータを回転させる
+ if (startSW==0){posL+=0.01; if (posL>0.8) posL=0.8;}
+ if (setSW==0) {posL-=0.01; if (posL<0) posL=0;}
+ lcd.locate(0,1); lcd.printf("PosL=%f \n", posL);
+ Thread::wait(200);
+ }
+}
+
+
+void initSpdH(){ // 走行スピードhighの設定
+ while(resetSW==1){
+ if (startSW==0){spdH++; if (spdH>100) spdH=100;}
+ if (setSW==0) {spdH--; if (spdH<0) spdH=0;}
+ lcd.locate(0,1); lcd.printf("speed High=%d \n", spdH);
+ Thread::wait(200);
+ }
+}
+
+void initSpdL(){ // 走行スピードlowの設定
+ while(resetSW==1){
+ if (startSW==0){spdL++; if (spdL>100) spdL=100;}
+ if (setSW==0) {spdL--; if (spdL<0) spdL=0;}
+ lcd.locate(0,1); lcd.printf("speed Low=%d \n", spdL);
+ Thread::wait(200);
+ }
+}
+
+void testRun(){ // test run
+ int setSpd=0;
+ while(resetSW==1){
+ if (setSW==0) setSpd=spdL;
+ if (startSW==0) setSpd=spdH;
+ lcd.locate(0,1); lcd.printf("set speed=%d \n", setSpd);
+
+ if (setSpd>cntC){
+ motorAmp=POSC-posH;
+ } else {
+ motorAmp=POSC-posL;
+ }
+ Thread::wait(50);
+ }
+ motorAmp=POSC;
+}
+
+
+void testCompus(){
+ lcd.cls();
+ while(resetSW==1){
+ lcd.locate(0,0); lcd.printf("compus=%f \n", arrayCompus[compus]);
+ lcd.locate(0,1); lcd.printf("rd=%f \n", rd);
+ Thread::wait(50);
+ }
+}
+
+
+//--------------------------------------------------------------------------
+// GPS受信テスト，補足状況をLED点灯の数で示す
+//--------------------------------------------------------------------------
+void dispGPS(){
+ while(resetSW==1){
+ lcd.cls();
+ if (latit==0) lcd.printf("GPS Not Found \n");
+ else {
+ lcd.cls();
+ lcd.printf("LA=%f \n", latit);
+ lcd.printf("LO=%f \n", longit);
+ Thread::wait(1500);
+ lcd.cls();
+ lcd.printf("rv=%f3 \n",rv);
+ lcd.printf("rd=%f3 \n",rd);
+ Thread::wait(1500);
+ }
+ Thread::wait(200);
+
+ }
+ Thread::wait(200);
+}
+
+//--------------------------------------------------------------------------
+// GPS信号の数値返還
+//--------------------------------------------------------------------------
+void gps_val(){
+ latitB=latit; longitB=longit; // 一つ前の値を保管
+ latit=atof(latitude+2); // 文字列を実数に変換．有効数字が足りないので度単位を省略
+ longit=atof(longitude+3); // 文字列を実数に変換．有効数字が足りないので度単位を省略
+ kn=atof(knot);
+ rv=kn*0.51; // ノット単位をメートル単位に変換
+ rdB=rd; // 一つ前の値を保管
+ rd=atof(direct); // 方位を数値変換
+}
+
+//--------------------------------------------------------------------------
+// 現在地とウェイポイントとの相対関係を計算
+//--------------------------------------------------------------------------
+void gps_cal(){
+ // 緯度経度の数値変換，目標地点との差分を計算
+ dy=(wy[np] - latit ) *1860;
+ dx=(wx[np] - longit)*1560;
+
+ // 目標地点までのと距離を計算
+ dr=sqrt(pow(dy,2)+pow(dx,2));
+
+ // 目標地点への方向を計算
+ dd = atan(dx / dy); // 北に対する角度を求める(±π/2範囲)
+ dd=dd*57; // ラジアン->度に変換 dd*(180/pai)
+ // 0-360度に変換
+ if (dx > 0 && dy < 0) dd = 180 + dd;
+ else if(dx < 0 && dy < 0) dd = 180 + dd;
+ else if(dx < 0 && dy > 0) dd = 360 + dd;
+ else;
+
+ // GPSが正しい方位を出力しない場合は、前の座標との差分で計算
+ dy2=(latit -latitB) *1860;
+ dx2=(longit-longitB)*1560;
+
+ if (dy2==0 || dx2==0){ // 緯度または経度のどちらかの変化が０の時
+ if (dy2==0){ if (dx2<0) rd2= 270; else rd2=90; } // rd2= 0, 90, 180，270 のいずれかを直接与える．
+ if (dx2==0){ if (dy2<0) rd2=-180; else rd2=0; } // dy2,dx2のどちらも0の時はrd2=0
+ } else {
+ // 目標地点への方向を計算
+ rd2 = atan(dx2 / dy2); // 北に対する角度を求める(±π/2範囲)
+ rd2=rd2*57; // ラジアン->度に変換 dd*(180/pai)
+ // 0-360度に変換
+ if (dx2 > 0 && dy2 < 0) rd2 = 180 + rd2;
+ else if(dx2 < 0 && dy2 < 0) rd2 = 180 + rd2;
+ else if(dx2 < 0 && dy2 > 0) rd2 = 360 + rd2;
+ else;
+ }
+
+ // 移動距離が小さくて使用するGPSが正しい速度と方向を出さない時のみ
+ // 座標差から計算した値と置き換える（速度=0,方位前の値のまま，GPSの仕様により異なる）
+ if (kn==0 && dx2!=0 && dy2!=0){
+ rd=rd2;
+ }
+
+ // compare the dirction by GPS and compus
+ if (rd>180) rdCpy=rd-360; else rdCpy=rd;
+ if (dirCompus>180) dirCompusCpy=dirCompus-360; else dirCompusCpy=dirCompus;
+ // compare the dirction by GPS and compus
+ rd=(fabs(rdCpy-dirCompusCpy)>45)? dirCompus: rd;
+
+ // 方位偏差の計算し，現在の進行方向から±180度の範囲に直す
+ dd=dd-rd;
+ if (dd>180) dd=dd-360;
+ else if (dd<-180) dd=dd+360;
+}
+
+//--------------------------------------------------------------------------
+// gps read by serial task
+//--------------------------------------------------------------------------
+void serial_task(void const*) {
+ char c;
+ int i=0;
+ //FILE *fp;
+
+ USBHostSerial serial;
+
+ while(1) {
+ while(!serial.connect()) Thread::wait(500); // try to connect a serial device
+
+ while (1) {
+ if (!serial.connected()) break; // if device disconnected, try to connect it again
+
+ while (serial.available()) { // receive message
+ c= serial.getc();
+ str[i]=c;
+ if (c == '\n') break;
+ i++;
+ }
+ str[i] = '\0';
+ i=0;
+ //fp=fopen("/local/log.txt", "a"); // Open "out.txt" on the local file system for writing
+ //fprintf(fp, str);
+ //fclose(fp);
+
+ header=strtok(str,",");
+
+ //if RMC line
+ if(strcmp(header,"$GNRMC")==0){
+ strtok(NULL,",");
+ strtok(NULL,",");
+ latitude=strtok(NULL,","); //get latitude
+ strtok(NULL,",");
+ longitude=strtok(NULL,","); //get longtude
+ strtok(NULL,","); // E読み飛ばし
+ knot=strtok(NULL,","); // 速度 get
+ direct=strtok(NULL,","); // 進行方向 get
+ gps_val(); // GPS信号の数値返還
+ gps_cal(); // 現在地とウェイポイントとの関係を計算
+ recvGPS++; if(recvGPS>2) recvGPS=2;
+ printf("LA=%f LO=%f kn=%f rv=%f rd=%f recvGPS=%d\n", latit, longit, kn, rv, rd, recvGPS );
+ }
+
+ if(strcmp(header,"$GPRMC")==0){
+ strtok(NULL,",");
+ strtok(NULL,",");
+ latitude=strtok(NULL,","); //get latitude
+ strtok(NULL,",");
+ longitude=strtok(NULL,","); //get longtude
+ strtok(NULL,","); // E読み飛ばし
+ knot=strtok(NULL,","); // 速度 get
+ direct=strtok(NULL,","); // 進行方向 get
+ gps_val(); // GPS信号の数値返還
+ gps_cal(); // 現在地とウェイポイントとの関係を計算
+ recvGPS++; if(recvGPS>2) recvGPS=2;
+ printf("LA=%f LO=%f kn=%f rv=%f rd=%f recvGPS=%d\n", latit, longit, kn, rv, rd, recvGPS );
+ }
+
+ //if QZQSM line
+ if(strcmp(header,"$QZQSM")==0 && recvQZSS==0){ //if RMC line
+ strtok(NULL,","); // 55
+ //strtok(NULL,","); // 53
+ messages=strtok(NULL,"*");
+ printf("%s\n",messages+4); // messages
+ strncpy(strtmp,messages+4,4); strtmp[4]='\0';
+ year=strtol(strtmp,NULL,16);
+ strncpy(strtmp,messages+8,2); strtmp[2]='\0';
+ teamNum =strtol(strtmp,NULL,16);
+ strncpy(strtmp,messages+10,2); strtmp[2]='\0';
+ numA =strtol(strtmp,NULL,16);
+ strncpy(strtmp,messages+12,2); strtmp[2]='\0';
+ sizeA=strtol(strtmp,NULL,16);
+ strncpy(strtmp,messages+14,2); strtmp[2]='\0';
+ numB =strtol(strtmp,NULL,16);
+ strncpy(strtmp,messages+16,2); strtmp[2]='\0';
+ sizeB=strtol(strtmp,NULL,16);
+ strncpy(strtmp,messages+18,2); strtmp[2]='\0';
+ numT=strtol(strtmp,NULL,16);
+ strncpy(strtmp,messages+20,2); strtmp[2]='\0';
+ startA=strtol(strtmp,NULL,18);
+ strncpy(strtmp,messages+22,2); strtmp[2]='\0';
+ startB=strtol(strtmp,NULL,20);
+ for(i=0; i<numT; i++){
+ strncpy(strtmp,messages+24+i*4,2); strtmp[2]='\0';
+ targetA[i]=strtol(strtmp,NULL,16);
+ strncpy(strtmp,messages+26+i*4,2); strtmp[2]='\0';
+ targetB[i]=strtol(strtmp,NULL,16);
+ }
+ if (teamNum==tNum) recvQZSS=1;
+
+ printf("year=%d\n",year);
+ printf("teamNum=%d\n",teamNum);
+ printf("numA=%d\n",numA);
+ printf("sizeA=%d\n",sizeA);
+ printf("numB=%d\n",numB);
+ printf("sizeB=%d\n",sizeB);
+ printf("tNum=%d\n",numT);
+ printf("startA=%d\n",startA);
+ printf("startB=%d\n",startB);
+ for(i=0; i<numT; i++){
+ printf("targetA[%d]=%d\n",i,targetA[i]);
+ printf("targetB[%d]=%d\n",i,targetB[i]);
+ }
+ printf("recvQZSS=%d\n",recvQZSS);
+ }
+ Thread::wait(50);
+ }
+ }
+}
+
+
+//--------------------------------------------------------------------------
+// GPSによる走行
+// runMode=0:GPS run without Gyro, normal rule( stop waypoint)
+// runMode=1:GPS run without Gyro, Double pylon race( loop waypoint without stop)
+//--------------------------------------------------------------------------
+void runGPS(int runMode){
+ lcd.cls(); lcd.locate(0,1); lcd.printf("Start!"); // 目標値までの距離をLEDに表示
+ Thread::wait(1000);
+
+ np=0;
+ motorStart();
+
+ while (resetSW==1){ // STARTスイッチが押されたら抜ける
+ // speed control
+ if (fabs(dr-wr)>8){ // ステアリングが真っ直ぐで距離が十分あるとき
+ if (cntC>spdH) motorAmp=POSC-posL; else motorAmp=POSC-posH;
+ } else {
+ if (cntC>spdL) motorAmp=POSC-posL; else motorAmp=POSC-posH;
+ }
+
+ while (recvGPS==0) Thread::wait(15); // GPS受信待ち
+ recvGPS=0; // reset after GPS recv
+ lcd.locate(0,0); lcd.printf("dd=%f \n", dd); // 目標値までの距離をLEDに表示
+ lcd.locate(0,1); lcd.printf("dr=%f \n", dr); // 目標値までの距離をLEDに表示
+
+ // 角度差に応じて方向にステアリングを切る．
+ posSteer=strn - dd*ks;
+ posSteer=(posSteer<strmin)? strmin: (posSteer>strmax)? strmax: posSteer; // 切れ幅を制限する
+ if (kn==0) posSteer=strn; // GPSで方位が得られなかった時（速度＝０）ニュートラルに戻す
+ strServo=posSteer;
+ Thread::wait(15);
+
+ // ウェイポイントとの距離を求め，ポイント更新または走行終了判断
+ if (dr < wr){
+ steerNeutral();
+ if(runMode==0) motorStop();
+ np++;
+ if (np>=wp){
+ if(runMode==0) break; // when normal robot car rule,
+ else np=0;
+ }
+ // 次のポイント数を表示
+ lcd.locate(0,1); lcd.printf("Next WP:%d ",np);
+ if(runMode==0){ // normal rule, wait 6sec
+ Thread::wait(6000);
+ motorStart();
+ }
+ }
+ }
+}
+
+//--------------------------------------------------------------------------
+// GPSによる走行, with gyro
+// runMode=0:GPS run with Gyro, normal rule( stop waypoint)
+// runMode=1:GPS run with Gyro, Double pylon race( loop waypoint without stop)
+//--------------------------------------------------------------------------
+void runGPSandGyro(int runMode){
+ lcd.cls(); lcd.locate(0,1); lcd.printf("Start!"); // 目標値までの距離をLEDに表示
+ Thread::wait(1000);
+
+ np=0;
+ motorStart();
+
+ while (resetSW==1){ // STARTスイッチが押されたら抜ける
+ // speed control
+ if (fabs(dr-wr)>8){ // ステアリングが真っ直ぐで距離が十分あるとき
+ if (cntC>spdH) motorAmp=POSC-posL; else motorAmp=POSC-posH;
+ } else {
+ if (cntC>spdL) motorAmp=POSC-posL; else motorAmp=POSC-posH;
+ }
+
+ //if (recvGPS==2){
+ if (recvGPS>=1){
+ recvGPS=0; // reset after GPS recv
+ ddCpy=dd; // GPSを受信した時の角度差を記録
+ lcd.locate(0,0); lcd.printf("dd=%f \n", dd); // 目標値までの距離をLEDに表示
+ lcd.locate(0,1); lcd.printf("dr=%f \n", dr); // 目標値までの距離をLEDに表示
+
+ // 角度差に応じて方向にステアリングを切る．
+ posSteer=strn - dd*ksg;
+
+ posSteer=(posSteer<strmin)? strmin: (posSteer>strmax)? strmax: posSteer; // 切れ幅を制限する
+ if (kn==0) posSteer=strn; // GPSで方位が得られなかった時（速度＝０）ニュートラルに戻す
+ strServo=posSteer;
+ Thread::wait(15);
+ gyro_reset();
+ }
+ // ジャイロで角度モニタ，指定角度でステアリング戻す,ddはターン中も更新されるので更新されないddCpyでターン
+ if (fabs(angle)>fabs(ddCpy)){posSteer=strn; strServo=posSteer;}
+ // ターン途中かつターン角度が大きい時，GPS受信しても数値を更新させない,recvGPS=0から次のターンまで２秒.待ちすぎか？
+ if (posSteer!=strn && fabs(ddCpy)>60) recvGPS=0;
+
+ // ウェイポイントとの距離を求め，ポイント更新または走行終了判断
+ if (dr < wr){
+ steerNeutral();
+ if(runMode==0) motorStop();
+ np++;
+ if (np>=wp){
+ if(runMode==0) break; // when normal robot car rule,
+ else np=0;
+ }
+ // 次のポイント数を表示
+ lcd.locate(0,1); lcd.printf("Next WP:%d ",np);
+ if(runMode==0){ // normal rule, wait 6.5sec
+ Thread::wait(6500);
+ motorStart(); // run straignt for 1 sec.
+ }
+ }
+ Thread::wait(15);
+ }
+}
+
+
+//--------------------------------------------------------------------------
+// QZSSによる走行,
+// runMode=0: without Gyro, normal rule( stop waypoint)
+// runMode=1: with Gyro, normal rule( stop waypoint)
+//--------------------------------------------------------------------------
+void runQZSS(int runMode){
+
+ lcd.cls();
+ lcd.printf("Waiting QZSS\n");
+ lcd.locate(0,1);
+ lcd.printf("massage: %d \n", tNum);
+ while (recvQZSS==0){
+ Thread::wait(500);
+ if (resetSW==0) break;
+ if (startSW==0){tNum++; if (tNum>25) tNum=0;}
+ if (setSW==0) {tNum--; if (tNum<0) tNum=25;}
+ lcd.locate(0,1); lcd.printf("massage: %d \n", tNum);
+ }
+
+ lcd.cls();
+ lcd.printf("QZSS massage \n");
+ lcd.printf("Recieved !! \n");
+ Thread::wait(3000);
+
+ // set way point
+ wp=0;
+ //wy[wp]=wyQ[startA][startB];
+ //wx[wp]=wxQ[startA][startB];
+ //wp++;
+ for(int i=0; i<numT; i++){
+ wy[wp]=wyQ[targetA[i]][targetB[i]];
+ wx[wp]=wxQ[targetA[i]][targetB[i]];
+ wp++;
+ }
+
+ if (runMode==0) runGPS(0); runGPSandGyro(0);
+}
+
+//--------------------------------------------------------------------------
+// セットアップ以外の初期値設定
+//--------------------------------------------------------------------------
+void initPara(){
+ steerNeutral();
+ motorStop();
+}
+
+//--------------------------------------------------------------------------
+int main() {
+ strServo=POSC; motorAmp=POSC;
+ resetSW.mode(PullUp); setSW.mode(PullUp); startSW.mode(PullUp);
+ Thread serialTask(serial_task, NULL, osPriorityNormal, 256 * 4);
+ timerS.attach_us(&sensor, 1000); // set timer to 1ms for timer interrupt
+ ptR.rise(&countR);
+ ptL.rise(&countL);
+ timer.start();
+ gyro_offset();
+ readParam();
+
+ lcd.cls();
+ lcd.printf("GPS robocar v1\n");
+ lcd.printf("sw:ret set start\n");
+ while(resetSW==1 && setSW==1 && startSW==1);
+ lcd.locate(0,1); lcd.printf(menuS[pmode]);
+
+ while(1) {
+
+ // initialize parameters
+ initPara();
+ lcd.cls();
+ lcd.printf("GPS robocar v1\n");
+ lcd.locate(0,1); lcd.printf(menuS[pmode]);
+
+ while (startSW==1) {
+ if (resetSW==0) {
+ Thread::wait(10);
+ while (resetSW==0);
+ Thread::wait(10);
+ pmode--;
+ if (pmode<0) pmode=nmenu;
+ lcd.locate(0,1);
+ lcd.printf(menuS[pmode]);
+ }
+ if (setSW==0) {
+ Thread::wait(10);
+ while (setSW==0);
+ Thread::wait(10);
+ pmode++;
+ if (pmode>nmenu) pmode=0;
+ lcd.locate(0,1);
+ lcd.printf(menuS[pmode]);
+ }
+
+ }
+ Thread::wait(500);
+
+ // go selected functions
+ switch(pmode){
+ case 0: dispGPS(); break; // GPSのテスト
+ case 1: setWp(); break; // ウェイポイントのセット
+ case 2: dispWp(); break; // display way point
+ case 3: clearWp(); break; // ウェイポイントのクリア
+ case 4: readWp(); break; // ウェイポイントのread
+ case 5: writeWp(); break; // ウェイポイントのread
+ case 6: setTeamNum(); break; // set team number for QZSS
+ case 7: setQZWp(); break; // (1,1)(1,7)(7,1)(7,7)の４点だけ読み書き、他の点は計算
+ case 8: readQZWp4(); break; // (1,1)(1,7)(7,1)(7,7)の４点だけ読み書き、他の点は計算
+ case 9: writeQZWp4(); break; // (1,1)(1,7)(7,1)(7,7)の４点だけ読み書き、他の点は計算
+ case 10: dispQmsg(); break; // ウェイポイントのクリア
+ case 11: clearQmsg(); break; // ウェイポイントのread
+ case 12: readQmsg(); break; // ウェイポイントのread
+ case 13: writeQmsg(); break; // ウェイポイントのread
+ case 14: runGPS(1); break; // GPS走行, 0：WPで止まる、１：WPで止まらない
+ case 15: runGPSandGyro(1); break; // ジャイロを使ったGPS走行,０：WPで止まる、１：WPで止まらない
+ case 16: runQZSS(0); break; // QZSS-GPS走行(WPで止まる)
+ case 17: runQZSS(1); break; // QZSS-ジャイロを使ったGPS走行(WPで止まる)
+ case 18: testServo(); break; // サーボの動作テスト
+ case 19: testMotor(); break; // モーターの動作テスト
+ case 20: initNeutral(); break; // ステアリングニュートラル位置の設定
+ case 21: initStrmin(); break; // ステアリングの最小値の設定
+ case 22: initStrmax(); break; // ステアリングの最大値の設定
+ case 23: initWr(); break; // ウェイポイントの半径設定
+ case 24: initKs(); break; // 舵角計算の比例値設定
+ case 25: initKsG(); break; // 舵角計算の比例値設定
+ case 26: initPosH(); break; // amp pos for high speed
+ case 27: initPosL(); break; // amp pos for low speed
+ case 28: initSpdH(); break; // 走行スピードhighの設定
+ case 29: initSpdL(); break; // 走行スピードlowの設定
+ case 30: testRun(); break; // test run
+ case 31: writeParam(); break; // ウェイポイントのシリアル出力
+ case 32: readParam(); break; // パラメータのシリアル出力
+ case 33: testEncoder(); break;
+ case 34: testGyro(); break;
+ case 35: testCompus(); break;
+ }
+ Thread::wait(500);
+ }
+}
+
\ No newline at end of file