![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
とりあえず、中間報告
Dependencies: mbed
Revision 0:1b7e9aa47a80, committed 2018-07-13
- Comitter:
- ponpoko1939
- Date:
- Fri Jul 13 15:02:55 2018 +0000
- Commit message:
- ver1???;
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
mbed.bld | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jul 13 15:02:55 2018 +0000 @@ -0,0 +1,178 @@ +#include "mbed.h" +#include "math.h" + +#define PI 3.14159265359 + +//ピンの初期化 +DigitalOut motor1(p21); +DigitalOut motor2(p22); +DigitalOut motor3(p23); +DigitalOut motor4(p24); +Serial gps(p28,p27); // tx, rx…GPSとの通信に使用 +Serial pc(USBTX, USBRX); // tx, rx +AnalogIn sw1(p15); + +//グローバル変数群 +int i,rlock,mode; +char gps_data[256]; +char ns,ew; +float x=135.585800, y=34.648525; //実験用初期位地 +float w_time,hokui,tokei; +float g_hokui,g_tokei,old_hokui, old_tokei; //oldとg_を比べることで距離と角度を出す +float d_hokui,m_hokui,d_tokei,m_tokei; +unsigned char c; +float dis; + +//プロトタイプ宣言 +void Brake(); +void Turn(); +void Return(); +void turnL(); +void turnR(); +void getGPS(); +float distance(float ,float); +void arg(float, float); + +int main() { + +} + +void Brake(){ + motor1 = 0; + motor2 = 0; + motor3 = 0; + motor4 = 0; +} + +void Turn(){ + motor1 = 1; + motor2 = 0; + motor3 = 1; + motor4 = 0; +} + +void Return(){ + motor1 = 0; + motor2 = 1; + motor3 = 0; + motor4 = 1; +} + +void turnL(){ + motor1 = 1; + motor2 = 0; + motor3 = 0; + motor4 = 1; +} + +void turnR(){ + motor1 = 0; + motor2 = 1; + motor3 = 1; + motor4 = 0; +} + + //ヒューベニの公式で距離出す +float distance(float g_hokui, float g_tokei){ + float rlat1 = y*PI/180, rlon1 = x*PI/180; + float rlat2 = g_hokui*PI/180, rlon2 = g_tokei*PI/180; + float a=6378137.000, a2=pow(a,2), e=sqrt((a2-pow(6356752.314,2))/a2); + float lat_diff = rlat1-rlat2; + float lon_diff = rlon1-rlon2; + float lat_ave = (rlat1+rlat2)/2; + float w = sqrt(1-pow(e,2)*pow(sin(lat_ave),2)); + float meridian = a*(1-pow(e,2))/pow(w,3); + float n=a/w; + float distance2 = sqrt(pow(lat_diff*meridian,2)+pow(lon_diff*n*cos(lat_ave),2)); + pc.printf("dis2 : %fm\n\r",distance2); + return distance2; +} + +//編集中、pdfの方と比べて検討 +void arg(float g_hokui, float g_tokei){ + + int turn; + float arg,radt,ido,keido; + + //緯度と経度をそれぞれの差を算出している + ido = x - g_hokui; + keido = y - g_tokei; + + arg = atan2(ido,keido); //arctanの値を算出 + + if(ido >= 0){ + //第一象限 + if(arg >= 0){ + radt = (arg * 180.0) / PI; + turn = 0; //右回り + } + //第四象限 + else if(arg < 0){ + radt = ((arg + PI) * 180) /PI; + turn = 0; + } + }else { + //第二象限 + if(arg >= 0){ + radt = (arg * 180) / PI; + turn = 1; //左回り + } + //第三象限 + else if(arg < 0){ + radt = ((arg + PI) * 180) /PI; + turn = 1; + } + } +} + +void getGPS() { + c = gps.getc(); + if( c=='$' || i == 256){ + mode = 0; + i = 0; + for(int j=0; j<256; j++){ + gps_data[j]=NULL; + } + } + + if(mode==0){ + if((gps_data[i]=c) != '\r'){ + i++; + }else{ + gps_data[i]='\0'; + + if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){ + if(rlock==1){ + pc.printf("Status:Lock(%d)\n\r",rlock); + + //logitude + d_tokei= int(tokei/100); + m_tokei= (tokei-d_tokei*100)/60; + g_tokei= d_tokei+m_tokei; + //Latitude + d_hokui=int(hokui/100); + m_hokui=(hokui-d_hokui*100)/60; + g_hokui=d_hokui+m_hokui; + pc.printf("Lat:%.6f, Lon:%.6f\n\r",g_hokui, g_tokei); //現在地のgpsデータ + pc.printf("ch_Lat:%.6f, ch_Lon:%.6f\n\r",old_hokui-g_hokui, old_tokei-g_tokei); //前回計測してからの移動(gps) + //pc.printf("dis : %fm\n\r", 1000*6378.137*acos(sin(y*PI/180)*sin(g_hokui*PI/180)+cos(y*PI/180)*cos(g_hokui*PI/180)*cos(x*PI/180-g_tokei*PI/180))); //別の距離の出し方 + dis = distance(g_hokui, g_tokei); //ここでヒュベニの公式を利用 + old_hokui=g_hokui; + old_tokei=g_tokei; + + //スイッチを押すことで目標のgpsデータ更新 + if(sw1>=0.5){ + x=g_tokei; + y=g_hokui; + pc.printf("\n\tchange destination\n\n\r"); + } + } + else{//gpsデータ未受信時 + pc.printf("\n\rStatus:unLock(%d)\n\r",rlock); + //pc.printf("%s",gps_data); + } + sprintf(gps_data, ""); + }//if + } + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Jul 13 15:02:55 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/a7c7b631e539 \ No newline at end of file