electronics-lab / Mbed 2 deprecated Cansat2018_1

とりあえず、中間報告

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?

UserRevisionLine numberNew 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 }