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
main.cpp
- Committer:
- hayama
- Date:
- 2014-11-13
- Revision:
- 0:c2262bac9aa6
File content as of revision 0:c2262bac9aa6:
#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);
}
}