とりあえず、中間報告
Dependencies: mbed
main.cpp@0:1b7e9aa47a80, 2018-07-13 (annotated)
- Committer:
- ponpoko1939
- Date:
- Fri Jul 13 15:02:55 2018 +0000
- Revision:
- 0:1b7e9aa47a80
ver1???;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ponpoko1939 | 0:1b7e9aa47a80 | 1 | #include "mbed.h" |
ponpoko1939 | 0:1b7e9aa47a80 | 2 | #include "math.h" |
ponpoko1939 | 0:1b7e9aa47a80 | 3 | |
ponpoko1939 | 0:1b7e9aa47a80 | 4 | #define PI 3.14159265359 |
ponpoko1939 | 0:1b7e9aa47a80 | 5 | |
ponpoko1939 | 0:1b7e9aa47a80 | 6 | //ピンの初期化 |
ponpoko1939 | 0:1b7e9aa47a80 | 7 | DigitalOut motor1(p21); |
ponpoko1939 | 0:1b7e9aa47a80 | 8 | DigitalOut motor2(p22); |
ponpoko1939 | 0:1b7e9aa47a80 | 9 | DigitalOut motor3(p23); |
ponpoko1939 | 0:1b7e9aa47a80 | 10 | DigitalOut motor4(p24); |
ponpoko1939 | 0:1b7e9aa47a80 | 11 | Serial gps(p28,p27); // tx, rx…GPSとの通信に使用 |
ponpoko1939 | 0:1b7e9aa47a80 | 12 | Serial pc(USBTX, USBRX); // tx, rx |
ponpoko1939 | 0:1b7e9aa47a80 | 13 | AnalogIn sw1(p15); |
ponpoko1939 | 0:1b7e9aa47a80 | 14 | |
ponpoko1939 | 0:1b7e9aa47a80 | 15 | //グローバル変数群 |
ponpoko1939 | 0:1b7e9aa47a80 | 16 | int i,rlock,mode; |
ponpoko1939 | 0:1b7e9aa47a80 | 17 | char gps_data[256]; |
ponpoko1939 | 0:1b7e9aa47a80 | 18 | char ns,ew; |
ponpoko1939 | 0:1b7e9aa47a80 | 19 | float x=135.585800, y=34.648525; //実験用初期位地 |
ponpoko1939 | 0:1b7e9aa47a80 | 20 | float w_time,hokui,tokei; |
ponpoko1939 | 0:1b7e9aa47a80 | 21 | float g_hokui,g_tokei,old_hokui, old_tokei; //oldとg_を比べることで距離と角度を出す |
ponpoko1939 | 0:1b7e9aa47a80 | 22 | float d_hokui,m_hokui,d_tokei,m_tokei; |
ponpoko1939 | 0:1b7e9aa47a80 | 23 | unsigned char c; |
ponpoko1939 | 0:1b7e9aa47a80 | 24 | float dis; |
ponpoko1939 | 0:1b7e9aa47a80 | 25 | |
ponpoko1939 | 0:1b7e9aa47a80 | 26 | //プロトタイプ宣言 |
ponpoko1939 | 0:1b7e9aa47a80 | 27 | void Brake(); |
ponpoko1939 | 0:1b7e9aa47a80 | 28 | void Turn(); |
ponpoko1939 | 0:1b7e9aa47a80 | 29 | void Return(); |
ponpoko1939 | 0:1b7e9aa47a80 | 30 | void turnL(); |
ponpoko1939 | 0:1b7e9aa47a80 | 31 | void turnR(); |
ponpoko1939 | 0:1b7e9aa47a80 | 32 | void getGPS(); |
ponpoko1939 | 0:1b7e9aa47a80 | 33 | float distance(float ,float); |
ponpoko1939 | 0:1b7e9aa47a80 | 34 | void arg(float, float); |
ponpoko1939 | 0:1b7e9aa47a80 | 35 | |
ponpoko1939 | 0:1b7e9aa47a80 | 36 | int main() { |
ponpoko1939 | 0:1b7e9aa47a80 | 37 | |
ponpoko1939 | 0:1b7e9aa47a80 | 38 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 39 | |
ponpoko1939 | 0:1b7e9aa47a80 | 40 | void Brake(){ |
ponpoko1939 | 0:1b7e9aa47a80 | 41 | motor1 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 42 | motor2 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 43 | motor3 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 44 | motor4 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 45 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 46 | |
ponpoko1939 | 0:1b7e9aa47a80 | 47 | void Turn(){ |
ponpoko1939 | 0:1b7e9aa47a80 | 48 | motor1 = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 49 | motor2 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 50 | motor3 = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 51 | motor4 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 52 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 53 | |
ponpoko1939 | 0:1b7e9aa47a80 | 54 | void Return(){ |
ponpoko1939 | 0:1b7e9aa47a80 | 55 | motor1 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 56 | motor2 = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 57 | motor3 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 58 | motor4 = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 59 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 60 | |
ponpoko1939 | 0:1b7e9aa47a80 | 61 | void turnL(){ |
ponpoko1939 | 0:1b7e9aa47a80 | 62 | motor1 = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 63 | motor2 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 64 | motor3 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 65 | motor4 = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 66 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 67 | |
ponpoko1939 | 0:1b7e9aa47a80 | 68 | void turnR(){ |
ponpoko1939 | 0:1b7e9aa47a80 | 69 | motor1 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 70 | motor2 = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 71 | motor3 = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 72 | motor4 = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 73 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 74 | |
ponpoko1939 | 0:1b7e9aa47a80 | 75 | //ヒューベニの公式で距離出す |
ponpoko1939 | 0:1b7e9aa47a80 | 76 | float distance(float g_hokui, float g_tokei){ |
ponpoko1939 | 0:1b7e9aa47a80 | 77 | float rlat1 = y*PI/180, rlon1 = x*PI/180; |
ponpoko1939 | 0:1b7e9aa47a80 | 78 | float rlat2 = g_hokui*PI/180, rlon2 = g_tokei*PI/180; |
ponpoko1939 | 0:1b7e9aa47a80 | 79 | float a=6378137.000, a2=pow(a,2), e=sqrt((a2-pow(6356752.314,2))/a2); |
ponpoko1939 | 0:1b7e9aa47a80 | 80 | float lat_diff = rlat1-rlat2; |
ponpoko1939 | 0:1b7e9aa47a80 | 81 | float lon_diff = rlon1-rlon2; |
ponpoko1939 | 0:1b7e9aa47a80 | 82 | float lat_ave = (rlat1+rlat2)/2; |
ponpoko1939 | 0:1b7e9aa47a80 | 83 | float w = sqrt(1-pow(e,2)*pow(sin(lat_ave),2)); |
ponpoko1939 | 0:1b7e9aa47a80 | 84 | float meridian = a*(1-pow(e,2))/pow(w,3); |
ponpoko1939 | 0:1b7e9aa47a80 | 85 | float n=a/w; |
ponpoko1939 | 0:1b7e9aa47a80 | 86 | float distance2 = sqrt(pow(lat_diff*meridian,2)+pow(lon_diff*n*cos(lat_ave),2)); |
ponpoko1939 | 0:1b7e9aa47a80 | 87 | pc.printf("dis2 : %fm\n\r",distance2); |
ponpoko1939 | 0:1b7e9aa47a80 | 88 | return distance2; |
ponpoko1939 | 0:1b7e9aa47a80 | 89 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 90 | |
ponpoko1939 | 0:1b7e9aa47a80 | 91 | //編集中、pdfの方と比べて検討 |
ponpoko1939 | 0:1b7e9aa47a80 | 92 | void arg(float g_hokui, float g_tokei){ |
ponpoko1939 | 0:1b7e9aa47a80 | 93 | |
ponpoko1939 | 0:1b7e9aa47a80 | 94 | int turn; |
ponpoko1939 | 0:1b7e9aa47a80 | 95 | float arg,radt,ido,keido; |
ponpoko1939 | 0:1b7e9aa47a80 | 96 | |
ponpoko1939 | 0:1b7e9aa47a80 | 97 | //緯度と経度をそれぞれの差を算出している |
ponpoko1939 | 0:1b7e9aa47a80 | 98 | ido = x - g_hokui; |
ponpoko1939 | 0:1b7e9aa47a80 | 99 | keido = y - g_tokei; |
ponpoko1939 | 0:1b7e9aa47a80 | 100 | |
ponpoko1939 | 0:1b7e9aa47a80 | 101 | arg = atan2(ido,keido); //arctanの値を算出 |
ponpoko1939 | 0:1b7e9aa47a80 | 102 | |
ponpoko1939 | 0:1b7e9aa47a80 | 103 | if(ido >= 0){ |
ponpoko1939 | 0:1b7e9aa47a80 | 104 | //第一象限 |
ponpoko1939 | 0:1b7e9aa47a80 | 105 | if(arg >= 0){ |
ponpoko1939 | 0:1b7e9aa47a80 | 106 | radt = (arg * 180.0) / PI; |
ponpoko1939 | 0:1b7e9aa47a80 | 107 | turn = 0; //右回り |
ponpoko1939 | 0:1b7e9aa47a80 | 108 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 109 | //第四象限 |
ponpoko1939 | 0:1b7e9aa47a80 | 110 | else if(arg < 0){ |
ponpoko1939 | 0:1b7e9aa47a80 | 111 | radt = ((arg + PI) * 180) /PI; |
ponpoko1939 | 0:1b7e9aa47a80 | 112 | turn = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 113 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 114 | }else { |
ponpoko1939 | 0:1b7e9aa47a80 | 115 | //第二象限 |
ponpoko1939 | 0:1b7e9aa47a80 | 116 | if(arg >= 0){ |
ponpoko1939 | 0:1b7e9aa47a80 | 117 | radt = (arg * 180) / PI; |
ponpoko1939 | 0:1b7e9aa47a80 | 118 | turn = 1; //左回り |
ponpoko1939 | 0:1b7e9aa47a80 | 119 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 120 | //第三象限 |
ponpoko1939 | 0:1b7e9aa47a80 | 121 | else if(arg < 0){ |
ponpoko1939 | 0:1b7e9aa47a80 | 122 | radt = ((arg + PI) * 180) /PI; |
ponpoko1939 | 0:1b7e9aa47a80 | 123 | turn = 1; |
ponpoko1939 | 0:1b7e9aa47a80 | 124 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 125 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 126 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 127 | |
ponpoko1939 | 0:1b7e9aa47a80 | 128 | void getGPS() { |
ponpoko1939 | 0:1b7e9aa47a80 | 129 | c = gps.getc(); |
ponpoko1939 | 0:1b7e9aa47a80 | 130 | if( c=='$' || i == 256){ |
ponpoko1939 | 0:1b7e9aa47a80 | 131 | mode = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 132 | i = 0; |
ponpoko1939 | 0:1b7e9aa47a80 | 133 | for(int j=0; j<256; j++){ |
ponpoko1939 | 0:1b7e9aa47a80 | 134 | gps_data[j]=NULL; |
ponpoko1939 | 0:1b7e9aa47a80 | 135 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 136 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 137 | |
ponpoko1939 | 0:1b7e9aa47a80 | 138 | if(mode==0){ |
ponpoko1939 | 0:1b7e9aa47a80 | 139 | if((gps_data[i]=c) != '\r'){ |
ponpoko1939 | 0:1b7e9aa47a80 | 140 | i++; |
ponpoko1939 | 0:1b7e9aa47a80 | 141 | }else{ |
ponpoko1939 | 0:1b7e9aa47a80 | 142 | gps_data[i]='\0'; |
ponpoko1939 | 0:1b7e9aa47a80 | 143 | |
ponpoko1939 | 0:1b7e9aa47a80 | 144 | if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){ |
ponpoko1939 | 0:1b7e9aa47a80 | 145 | if(rlock==1){ |
ponpoko1939 | 0:1b7e9aa47a80 | 146 | pc.printf("Status:Lock(%d)\n\r",rlock); |
ponpoko1939 | 0:1b7e9aa47a80 | 147 | |
ponpoko1939 | 0:1b7e9aa47a80 | 148 | //logitude |
ponpoko1939 | 0:1b7e9aa47a80 | 149 | d_tokei= int(tokei/100); |
ponpoko1939 | 0:1b7e9aa47a80 | 150 | m_tokei= (tokei-d_tokei*100)/60; |
ponpoko1939 | 0:1b7e9aa47a80 | 151 | g_tokei= d_tokei+m_tokei; |
ponpoko1939 | 0:1b7e9aa47a80 | 152 | //Latitude |
ponpoko1939 | 0:1b7e9aa47a80 | 153 | d_hokui=int(hokui/100); |
ponpoko1939 | 0:1b7e9aa47a80 | 154 | m_hokui=(hokui-d_hokui*100)/60; |
ponpoko1939 | 0:1b7e9aa47a80 | 155 | g_hokui=d_hokui+m_hokui; |
ponpoko1939 | 0:1b7e9aa47a80 | 156 | pc.printf("Lat:%.6f, Lon:%.6f\n\r",g_hokui, g_tokei); //現在地のgpsデータ |
ponpoko1939 | 0:1b7e9aa47a80 | 157 | pc.printf("ch_Lat:%.6f, ch_Lon:%.6f\n\r",old_hokui-g_hokui, old_tokei-g_tokei); //前回計測してからの移動(gps) |
ponpoko1939 | 0:1b7e9aa47a80 | 158 | //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))); //別の距離の出し方 |
ponpoko1939 | 0:1b7e9aa47a80 | 159 | dis = distance(g_hokui, g_tokei); //ここでヒュベニの公式を利用 |
ponpoko1939 | 0:1b7e9aa47a80 | 160 | old_hokui=g_hokui; |
ponpoko1939 | 0:1b7e9aa47a80 | 161 | old_tokei=g_tokei; |
ponpoko1939 | 0:1b7e9aa47a80 | 162 | |
ponpoko1939 | 0:1b7e9aa47a80 | 163 | //スイッチを押すことで目標のgpsデータ更新 |
ponpoko1939 | 0:1b7e9aa47a80 | 164 | if(sw1>=0.5){ |
ponpoko1939 | 0:1b7e9aa47a80 | 165 | x=g_tokei; |
ponpoko1939 | 0:1b7e9aa47a80 | 166 | y=g_hokui; |
ponpoko1939 | 0:1b7e9aa47a80 | 167 | pc.printf("\n\tchange destination\n\n\r"); |
ponpoko1939 | 0:1b7e9aa47a80 | 168 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 169 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 170 | else{//gpsデータ未受信時 |
ponpoko1939 | 0:1b7e9aa47a80 | 171 | pc.printf("\n\rStatus:unLock(%d)\n\r",rlock); |
ponpoko1939 | 0:1b7e9aa47a80 | 172 | //pc.printf("%s",gps_data); |
ponpoko1939 | 0:1b7e9aa47a80 | 173 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 174 | sprintf(gps_data, ""); |
ponpoko1939 | 0:1b7e9aa47a80 | 175 | }//if |
ponpoko1939 | 0:1b7e9aa47a80 | 176 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 177 | } |
ponpoko1939 | 0:1b7e9aa47a80 | 178 | } |