hiroki kanda
/
Nucleo_test2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp@0:09287ca178ff, 2021-09-10 (annotated)
- Committer:
- rmf_flight
- Date:
- Fri Sep 10 12:37:29 2021 +0000
- Revision:
- 0:09287ca178ff
- Child:
- 1:038eca93a8f4
initial commit
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| rmf_flight | 0:09287ca178ff | 1 | #include <stdlib.h> |
| rmf_flight | 0:09287ca178ff | 2 | #include <stdio.h> |
| rmf_flight | 0:09287ca178ff | 3 | #include <math.h> //距離を求める際に逆三角関数を使うため必要 |
| rmf_flight | 0:09287ca178ff | 4 | #include "mbed.h" |
| rmf_flight | 0:09287ca178ff | 5 | #include "BME280.h" |
| rmf_flight | 0:09287ca178ff | 6 | #include "GPS.h" |
| rmf_flight | 0:09287ca178ff | 7 | #include "hcsr04.h" |
| rmf_flight | 0:09287ca178ff | 8 | //#include "SDFileSystem.h" //<==これをつけるとコンパイルできない |
| rmf_flight | 0:09287ca178ff | 9 | #include "Servo.h" |
| rmf_flight | 0:09287ca178ff | 10 | #include "BMX_055.h" |
| rmf_flight | 0:09287ca178ff | 11 | #include "Moter_drive.h" |
| rmf_flight | 0:09287ca178ff | 12 | |
| rmf_flight | 0:09287ca178ff | 13 | //地磁気センサの補正用の値 |
| rmf_flight | 0:09287ca178ff | 14 | #define mag_cari_x 18.75 |
| rmf_flight | 0:09287ca178ff | 15 | #define mag_cari_y -26.875 |
| rmf_flight | 0:09287ca178ff | 16 | #define acc_cari_y 0 |
| rmf_flight | 0:09287ca178ff | 17 | #define Max_t 110 |
| rmf_flight | 0:09287ca178ff | 18 | |
| rmf_flight | 0:09287ca178ff | 19 | //モーター関係 |
| rmf_flight | 0:09287ca178ff | 20 | #define M_time_180 3.6 //180度その場回転する時に必要な秒数 |
| rmf_flight | 0:09287ca178ff | 21 | #define M_time_90 1.8 //90度その場回転する時に必要な秒数 |
| rmf_flight | 0:09287ca178ff | 22 | #define M_time_45 0.9 //45度その場回転する時に必要な秒数 |
| rmf_flight | 0:09287ca178ff | 23 | |
| rmf_flight | 0:09287ca178ff | 24 | |
| rmf_flight | 0:09287ca178ff | 25 | |
| rmf_flight | 0:09287ca178ff | 26 | //通信関係 |
| rmf_flight | 0:09287ca178ff | 27 | Serial pc(USBTX, USBRX, 115200); |
| rmf_flight | 0:09287ca178ff | 28 | Serial twe(A7, A2, 115200); |
| rmf_flight | 0:09287ca178ff | 29 | //Serial USBSerial(PA_2,PA_15); |
| rmf_flight | 0:09287ca178ff | 30 | |
| rmf_flight | 0:09287ca178ff | 31 | |
| rmf_flight | 0:09287ca178ff | 32 | BME280 sensor(D4, D5); |
| rmf_flight | 0:09287ca178ff | 33 | GPS michibiki(GPSTX,GPSRX); |
| rmf_flight | 0:09287ca178ff | 34 | HCSR04 usensor(D6, D3); //それぞれ Trig, Echo につなぐ |
| rmf_flight | 0:09287ca178ff | 35 | //SDFileSystem sd(A6, A5, A4, D8, "sd"); // the pinout on the mbed Cool Components workshop board //<== これをつけるとコンパイルできない |
| rmf_flight | 0:09287ca178ff | 36 | Servo myservo(A3); //パラシュート切り離し機構のサーボ |
| rmf_flight | 0:09287ca178ff | 37 | BMX055 bmx; |
| rmf_flight | 0:09287ca178ff | 38 | DigitalOut myled(LED1); //goal検知 |
| rmf_flight | 0:09287ca178ff | 39 | |
| rmf_flight | 0:09287ca178ff | 40 | |
| rmf_flight | 0:09287ca178ff | 41 | const double goal_x = 139.987503; //ゴールの経度(初期値は適当,35.657941,139.542798 |
| rmf_flight | 0:09287ca178ff | 42 | const double goal_y = 40.142727; //ゴールの緯度 |
| rmf_flight | 0:09287ca178ff | 43 | const double R = 6371000.0; //地球の半径 [m] |
| rmf_flight | 0:09287ca178ff | 44 | double delta_x, delta_y, now_x, now_y, sita, goal_y_rad,temp1,temp2; //ゴールまでの直線と機体の位置とのなす角度 |
| rmf_flight | 0:09287ca178ff | 45 | double r = 10; //ゴールまでの残り距離 |
| rmf_flight | 0:09287ca178ff | 46 | double r_first[5]; //ただし、[0]は使わない |
| rmf_flight | 0:09287ca178ff | 47 | float ax, ay, az, gx, gy, gz, mx, my, mz; //9軸センサの値を入れる変数 |
| rmf_flight | 0:09287ca178ff | 48 | int heading_int, sita_int; |
| rmf_flight | 0:09287ca178ff | 49 | float Ele_angle; //仰角(水平面と基板がなす角度) |
| rmf_flight | 0:09287ca178ff | 50 | float nx, ny, nz; //地磁気の補正用 |
| rmf_flight | 0:09287ca178ff | 51 | double X,Y; //ゴールがどの象限にいるかを区別するための変数 |
| rmf_flight | 0:09287ca178ff | 52 | double mode; //ゴールの位置によってモードを変える |
| rmf_flight | 0:09287ca178ff | 53 | int i; //超音波でのループ用変数 |
| rmf_flight | 0:09287ca178ff | 54 | int d[25]; |
| rmf_flight | 0:09287ca178ff | 55 | |
| rmf_flight | 0:09287ca178ff | 56 | #include "Flight_Pin.h" //フライトピンについての定義及び処理内容(処理内容により、このヘッダーはtwe,now_x,now_yの宣言の後におく必要がある！) |
| rmf_flight | 0:09287ca178ff | 57 | #include "Parachute_detach.h" |
| rmf_flight | 0:09287ca178ff | 58 | |
| rmf_flight | 0:09287ca178ff | 59 | |
| rmf_flight | 0:09287ca178ff | 60 | //9軸センサからデータを取得する関数 |
| rmf_flight | 0:09287ca178ff | 61 | void Getdata(){ |
| rmf_flight | 0:09287ca178ff | 62 | bmx.getMotion9(&ax,&ay,&az, &gx,&gy,&gz, &mx,&my,&mz); |
| rmf_flight | 0:09287ca178ff | 63 | } |
| rmf_flight | 0:09287ca178ff | 64 | |
| rmf_flight | 0:09287ca178ff | 65 | #include "landing_detect.h" |
| rmf_flight | 0:09287ca178ff | 66 | |
| rmf_flight | 0:09287ca178ff | 67 | |
| rmf_flight | 0:09287ca178ff | 68 | int main() { |
| rmf_flight | 0:09287ca178ff | 69 | |
| rmf_flight | 0:09287ca178ff | 70 | /* |
| rmf_flight | 0:09287ca178ff | 71 | while(1){ //テスト用、実際は実装しない |
| rmf_flight | 0:09287ca178ff | 72 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 73 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 74 | |
| rmf_flight | 0:09287ca178ff | 75 | twe.printf("lat_now : %f,long_now : %f\n",now_y, now_x); |
| rmf_flight | 0:09287ca178ff | 76 | |
| rmf_flight | 0:09287ca178ff | 77 | delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む |
| rmf_flight | 0:09287ca178ff | 78 | delta_y = goal_y - now_y; |
| rmf_flight | 0:09287ca178ff | 79 | |
| rmf_flight | 0:09287ca178ff | 80 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 81 | |
| rmf_flight | 0:09287ca178ff | 82 | r = abs(r); //絶対値にする |
| rmf_flight | 0:09287ca178ff | 83 | |
| rmf_flight | 0:09287ca178ff | 84 | twe.printf("r : %f\n",r); |
| rmf_flight | 0:09287ca178ff | 85 | |
| rmf_flight | 0:09287ca178ff | 86 | //方角を求める |
| rmf_flight | 0:09287ca178ff | 87 | delta_x = delta_x * 3.14/180; |
| rmf_flight | 0:09287ca178ff | 88 | now_x = now_x * 3.14/180; |
| rmf_flight | 0:09287ca178ff | 89 | goal_y_rad = goal_y * 3.14/180; |
| rmf_flight | 0:09287ca178ff | 90 | now_y = now_y * 3.14/180; |
| rmf_flight | 0:09287ca178ff | 91 | |
| rmf_flight | 0:09287ca178ff | 92 | //sita = atan2(sin(delta_x*3.14/180),cos(now_x*3.14/180)*tan(goal_y*3.14/180)-sin(now_y*3.14/180)*cos(delta_x*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 93 | temp1 = cos(now_y)*tan(goal_y_rad); |
| rmf_flight | 0:09287ca178ff | 94 | temp2 = sin(now_y)*cos(delta_x); |
| rmf_flight | 0:09287ca178ff | 95 | |
| rmf_flight | 0:09287ca178ff | 96 | twe.printf("temp1 : %f, temp2 : %f\n",temp1,temp2); |
| rmf_flight | 0:09287ca178ff | 97 | |
| rmf_flight | 0:09287ca178ff | 98 | //sita = atan2(sin(delta_x),cos(now_x)*tan(goal_y_rad)-sin(now_y)*cos(delta_x)); |
| rmf_flight | 0:09287ca178ff | 99 | sita = atan2(sin(delta_x),(temp1-temp2)); |
| rmf_flight | 0:09287ca178ff | 100 | sita = sita * 180/3.14; //ラジアンからdegreeに戻す |
| rmf_flight | 0:09287ca178ff | 101 | |
| rmf_flight | 0:09287ca178ff | 102 | twe.printf("sita : %f\n",sita); |
| rmf_flight | 0:09287ca178ff | 103 | |
| rmf_flight | 0:09287ca178ff | 104 | sita_int = (int)sita; |
| rmf_flight | 0:09287ca178ff | 105 | sita_int = sita_int % 360; |
| rmf_flight | 0:09287ca178ff | 106 | twe.printf("sita_int : %d\n",sita_int); |
| rmf_flight | 0:09287ca178ff | 107 | |
| rmf_flight | 0:09287ca178ff | 108 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 109 | } |
| rmf_flight | 0:09287ca178ff | 110 | */ |
| rmf_flight | 0:09287ca178ff | 111 | |
| rmf_flight | 0:09287ca178ff | 112 | bmx.init(); //bmx055(9軸センサの初期化) |
| rmf_flight | 0:09287ca178ff | 113 | |
| rmf_flight | 0:09287ca178ff | 114 | /* |
| rmf_flight | 0:09287ca178ff | 115 | // PCとの通信 |
| rmf_flight | 0:09287ca178ff | 116 | int a, b, c, d; |
| rmf_flight | 0:09287ca178ff | 117 | pc.printf("Hello World!\r\n"); |
| rmf_flight | 0:09287ca178ff | 118 | mkdir("/sd/mydir", 0777); |
| rmf_flight | 0:09287ca178ff | 119 | FILE *fp = fopen("/sd/mydir/sdtest.txt", "w"); |
| rmf_flight | 0:09287ca178ff | 120 | if(fp == NULL) { |
| rmf_flight | 0:09287ca178ff | 121 | error("Could not open file for write\r\n"); |
| rmf_flight | 0:09287ca178ff | 122 | } |
| rmf_flight | 0:09287ca178ff | 123 | fprintf(fp, "Hello fun SD Card World!"); |
| rmf_flight | 0:09287ca178ff | 124 | fclose(fp); |
| rmf_flight | 0:09287ca178ff | 125 | |
| rmf_flight | 0:09287ca178ff | 126 | // CSVファイル作成 |
| rmf_flight | 0:09287ca178ff | 127 | fp = fopen("/sd/mydir/data.csv", "w"); |
| rmf_flight | 0:09287ca178ff | 128 | if(fp == NULL) { |
| rmf_flight | 0:09287ca178ff | 129 | error("Could not open file for write\r\n"); |
| rmf_flight | 0:09287ca178ff | 130 | } |
| rmf_flight | 0:09287ca178ff | 131 | fprintf(fp, "10,11,12,13\n"); |
| rmf_flight | 0:09287ca178ff | 132 | fprintf(fp, "20,21,22,23\n"); |
| rmf_flight | 0:09287ca178ff | 133 | fprintf(fp, "30,31,32,33\n"); |
| rmf_flight | 0:09287ca178ff | 134 | fclose(fp); |
| rmf_flight | 0:09287ca178ff | 135 | |
| rmf_flight | 0:09287ca178ff | 136 | //CSVファイル読み込み |
| rmf_flight | 0:09287ca178ff | 137 | fp = fopen("/sd/mydir/data.csv", "r"); |
| rmf_flight | 0:09287ca178ff | 138 | if(fp == NULL) { |
| rmf_flight | 0:09287ca178ff | 139 | error("Could not open file for write\r\n"); |
| rmf_flight | 0:09287ca178ff | 140 | } |
| rmf_flight | 0:09287ca178ff | 141 | while( fscanf( fp, "%d,%d,%d,%d",&a, &b, &c, &d ) != EOF ){ |
| rmf_flight | 0:09287ca178ff | 142 | pc.printf("%d,%d,%d,%d\r\n",a, b, c, d); |
| rmf_flight | 0:09287ca178ff | 143 | } |
| rmf_flight | 0:09287ca178ff | 144 | fclose(fp); |
| rmf_flight | 0:09287ca178ff | 145 | |
| rmf_flight | 0:09287ca178ff | 146 | printf("Goodbye World!\r\n"); //microsd |
| rmf_flight | 0:09287ca178ff | 147 | */ //これをつけるとコンパイルできない |
| rmf_flight | 0:09287ca178ff | 148 | |
| rmf_flight | 0:09287ca178ff | 149 | /* |
| rmf_flight | 0:09287ca178ff | 150 | pc.printf("%2.2f degC, %04.2f hPa, %2.2f %%\r\n", sensor.getTemperature(), sensor.getPressure(), sensor.getHumidity()); //BME280 |
| rmf_flight | 0:09287ca178ff | 151 | |
| rmf_flight | 0:09287ca178ff | 152 | pc.printf("%f %f\r\n",michibiki.latitude,michibiki.longtitude); //GPS |
| rmf_flight | 0:09287ca178ff | 153 | |
| rmf_flight | 0:09287ca178ff | 154 | usensor.start(); |
| rmf_flight | 0:09287ca178ff | 155 | wait_ms(500); |
| rmf_flight | 0:09287ca178ff | 156 | unsigned int dist = usensor.get_dist_cm(); |
| rmf_flight | 0:09287ca178ff | 157 | pc.printf("%ld cm\r\n",dist ); //hcsr04 |
| rmf_flight | 0:09287ca178ff | 158 | |
| rmf_flight | 0:09287ca178ff | 159 | float ax, ay, az, gx, gy, gz, mx, my, mz; |
| rmf_flight | 0:09287ca178ff | 160 | bmx.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz); |
| rmf_flight | 0:09287ca178ff | 161 | pc.printf("%05.3f,%05.3f,%05.3f,%05.3f,%05.3f,%05.3f,%05.3f,%05.3f,%05.3f\r\n", ax, ay, az, gx, gy, gz, mx, my, mz); //これをつけるとコンパイルできない |
| rmf_flight | 0:09287ca178ff | 162 | */ |
| rmf_flight | 0:09287ca178ff | 163 | |
| rmf_flight | 0:09287ca178ff | 164 | //----------------------------------------------------------------------------------------------- |
| rmf_flight | 0:09287ca178ff | 165 | |
| rmf_flight | 0:09287ca178ff | 166 | //フライトピンの接続状況を確認する |
| rmf_flight | 0:09287ca178ff | 167 | flight_pin_loop(); //Flight_Pin.hで定義された関数 |
| rmf_flight | 0:09287ca178ff | 168 | |
| rmf_flight | 0:09287ca178ff | 169 | //着地状況を確認する |
| rmf_flight | 0:09287ca178ff | 170 | landing_detect(); //landing_detect.hで定義された関数 |
| rmf_flight | 0:09287ca178ff | 171 | |
| rmf_flight | 0:09287ca178ff | 172 | //パラシュートを切り離す |
| rmf_flight | 0:09287ca178ff | 173 | parachute_detach(); //Parachute_detach.hで定義された関数 |
| rmf_flight | 0:09287ca178ff | 174 | |
| rmf_flight | 0:09287ca178ff | 175 | |
| rmf_flight | 0:09287ca178ff | 176 | //--------------------------------------------------------------------------------------ここから以前の処理方法(採用無し) |
| rmf_flight | 0:09287ca178ff | 177 | //パラシュートを切り離したあと、GPSと地磁気の情報を元にゴールまで進むプログラム |
| rmf_flight | 0:09287ca178ff | 178 | /* |
| rmf_flight | 0:09287ca178ff | 179 | while(1){ //とりあえず無限ループ(終了条件は内部で定義) |
| rmf_flight | 0:09287ca178ff | 180 | |
| rmf_flight | 0:09287ca178ff | 181 | if(r > 4){ //ゴールまで4m以上ある時 |
| rmf_flight | 0:09287ca178ff | 182 | Getdata(); //9軸センサからデータを取得する |
| rmf_flight | 0:09287ca178ff | 183 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 184 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 185 | |
| rmf_flight | 0:09287ca178ff | 186 | twe.printf("-----------------------------\n"); |
| rmf_flight | 0:09287ca178ff | 187 | twe.printf(",lat_now,%f,long_now,%f\n",now_y, now_x); |
| rmf_flight | 0:09287ca178ff | 188 | |
| rmf_flight | 0:09287ca178ff | 189 | delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む |
| rmf_flight | 0:09287ca178ff | 190 | delta_y = goal_y - now_y; |
| rmf_flight | 0:09287ca178ff | 191 | |
| rmf_flight | 0:09287ca178ff | 192 | //以前までの、距離を求める式(多分使わない) |
| rmf_flight | 0:09287ca178ff | 193 | //delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む |
| rmf_flight | 0:09287ca178ff | 194 | //delta_y = goal_y - now_y; |
| rmf_flight | 0:09287ca178ff | 195 | //r = sqrt(delta_x * delta_x + delta_y * delta_y); //ゴールまでの距離を求める(3平方の定理) |
| rmf_flight | 0:09287ca178ff | 196 | |
| rmf_flight | 0:09287ca178ff | 197 | |
| rmf_flight | 0:09287ca178ff | 198 | |
| rmf_flight | 0:09287ca178ff | 199 | |
| rmf_flight | 0:09287ca178ff | 200 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 201 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 202 | |
| rmf_flight | 0:09287ca178ff | 203 | r = abs(r); //絶対値にする |
| rmf_flight | 0:09287ca178ff | 204 | |
| rmf_flight | 0:09287ca178ff | 205 | twe.printf("r : %f\n",r); |
| rmf_flight | 0:09287ca178ff | 206 | |
| rmf_flight | 0:09287ca178ff | 207 | //sita = atan2l(delta_y, delta_x)*180/3.14; //現在地点からゴールまでの角度を求める(同時に角度の単位をラジアンからdegreeにも変換している) |
| rmf_flight | 0:09287ca178ff | 208 | |
| rmf_flight | 0:09287ca178ff | 209 | //方角を求める(degree ==> rad) |
| rmf_flight | 0:09287ca178ff | 210 | delta_x = delta_x * 3.14/180; |
| rmf_flight | 0:09287ca178ff | 211 | now_x = now_x * 3.14/180; |
| rmf_flight | 0:09287ca178ff | 212 | goal_y_rad = goal_y * 3.14/180; |
| rmf_flight | 0:09287ca178ff | 213 | now_y = now_y * 3.14/180; |
| rmf_flight | 0:09287ca178ff | 214 | |
| rmf_flight | 0:09287ca178ff | 215 | temp1 = cos(now_y)*tan(goal_y_rad); |
| rmf_flight | 0:09287ca178ff | 216 | temp2 = sin(now_y)*cos(delta_x); |
| rmf_flight | 0:09287ca178ff | 217 | |
| rmf_flight | 0:09287ca178ff | 218 | sita = atan2(sin(delta_x),(temp1-temp2)); |
| rmf_flight | 0:09287ca178ff | 219 | sita = sita * 180/3.14; //ラジアンからdegreeに戻す |
| rmf_flight | 0:09287ca178ff | 220 | |
| rmf_flight | 0:09287ca178ff | 221 | twe.printf("sita : %f\n",sita); |
| rmf_flight | 0:09287ca178ff | 222 | |
| rmf_flight | 0:09287ca178ff | 223 | sita_int = (int)sita; |
| rmf_flight | 0:09287ca178ff | 224 | sita_int = sita_int % 360; |
| rmf_flight | 0:09287ca178ff | 225 | //twe.printf("sita_int : %d\n",sita_int); |
| rmf_flight | 0:09287ca178ff | 226 | //座標変換 |
| rmf_flight | 0:09287ca178ff | 227 | if(sita_int > -270 && sita_int < -180){ |
| rmf_flight | 0:09287ca178ff | 228 | sita_int = sita_int + 360; |
| rmf_flight | 0:09287ca178ff | 229 | } |
| rmf_flight | 0:09287ca178ff | 230 | twe.printf("sita_int : %d\n",sita_int); |
| rmf_flight | 0:09287ca178ff | 231 | |
| rmf_flight | 0:09287ca178ff | 232 | |
| rmf_flight | 0:09287ca178ff | 233 | //twe.printf("my,mx : %f,%f\n",my,mx); |
| rmf_flight | 0:09287ca178ff | 234 | //ここから地磁気の値の補正 |
| rmf_flight | 0:09287ca178ff | 235 | ay = ay - acc_cari_y; |
| rmf_flight | 0:09287ca178ff | 236 | //USBSerial.printf("%d,%05.3f,",seq, ay); |
| rmf_flight | 0:09287ca178ff | 237 | Ele_angle = asin(ay); |
| rmf_flight | 0:09287ca178ff | 238 | Ele_angle = Ele_angle*180/3.14; |
| rmf_flight | 0:09287ca178ff | 239 | |
| rmf_flight | 0:09287ca178ff | 240 | mx = mx + mag_cari_x; |
| rmf_flight | 0:09287ca178ff | 241 | my = my + mag_cari_y; |
| rmf_flight | 0:09287ca178ff | 242 | |
| rmf_flight | 0:09287ca178ff | 243 | nx = (-1)*ay / sqrt(ax*ax+ay*ay); |
| rmf_flight | 0:09287ca178ff | 244 | ny = ax / sqrt(ax*ax+ay*ay); |
| rmf_flight | 0:09287ca178ff | 245 | nz = 0; |
| rmf_flight | 0:09287ca178ff | 246 | |
| rmf_flight | 0:09287ca178ff | 247 | Ele_angle = (-1)*Ele_angle; |
| rmf_flight | 0:09287ca178ff | 248 | |
| rmf_flight | 0:09287ca178ff | 249 | mx = my * (ny*nx*(1-cos(Ele_angle)))+mx*(nx*nx*(1-cos(Ele_angle))+cos(Ele_angle))-mz*ny*sin(Ele_angle); |
| rmf_flight | 0:09287ca178ff | 250 | my = my*(ny*ny*(1-cos(Ele_angle))+cos(Ele_angle))+mx*(nx*ny*(1-cos(Ele_angle)))+mz*nx*sin(Ele_angle); |
| rmf_flight | 0:09287ca178ff | 251 | |
| rmf_flight | 0:09287ca178ff | 252 | |
| rmf_flight | 0:09287ca178ff | 253 | |
| rmf_flight | 0:09287ca178ff | 254 | float heading = atan2(my, mx)*180/3.14; //地磁気センサのx,y軸の値を使って、方位を計算(同時に角度の単位をラジアンからdegreeにも変換している) |
| rmf_flight | 0:09287ca178ff | 255 | //twe.printf("heading : %f\n",heading); |
| rmf_flight | 0:09287ca178ff | 256 | heading_int = (int)heading; |
| rmf_flight | 0:09287ca178ff | 257 | //twe.printf("heading_int : %d\n",heading_int); |
| rmf_flight | 0:09287ca178ff | 258 | heading_int = heading_int%360; //角度を0 <= heading < 360 にする |
| rmf_flight | 0:09287ca178ff | 259 | |
| rmf_flight | 0:09287ca178ff | 260 | //座標変換 |
| rmf_flight | 0:09287ca178ff | 261 | heading_int = (-1)*heading_int + 90; |
| rmf_flight | 0:09287ca178ff | 262 | if(heading_int >= 180){ |
| rmf_flight | 0:09287ca178ff | 263 | heading_int = heading_int - 360; |
| rmf_flight | 0:09287ca178ff | 264 | } |
| rmf_flight | 0:09287ca178ff | 265 | twe.printf("heading_int : %d\n",heading_int); |
| rmf_flight | 0:09287ca178ff | 266 | |
| rmf_flight | 0:09287ca178ff | 267 | |
| rmf_flight | 0:09287ca178ff | 268 | |
| rmf_flight | 0:09287ca178ff | 269 | |
| rmf_flight | 0:09287ca178ff | 270 | int sita_new = heading_int - sita_int; //方位とゴールの方角の差分を計算する |
| rmf_flight | 0:09287ca178ff | 271 | |
| rmf_flight | 0:09287ca178ff | 272 | twe.printf("sita_new = %d \r\n",sita_new); |
| rmf_flight | 0:09287ca178ff | 273 | |
| rmf_flight | 0:09287ca178ff | 274 | //この差分sita_newを0にする(つまり、ゴールの方を向くようにする) |
| rmf_flight | 0:09287ca178ff | 275 | sita_new = abs(sita_new); |
| rmf_flight | 0:09287ca178ff | 276 | while(sita_new > 35){ |
| rmf_flight | 0:09287ca178ff | 277 | twe.printf("while -----------------------------\n"); |
| rmf_flight | 0:09287ca178ff | 278 | //右だけタイヤを回して、機体をその場回転させる |
| rmf_flight | 0:09287ca178ff | 279 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 280 | wait(0.05); |
| rmf_flight | 0:09287ca178ff | 281 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 282 | |
| rmf_flight | 0:09287ca178ff | 283 | Getdata(); //9軸センサからデータを取得する |
| rmf_flight | 0:09287ca178ff | 284 | //twe.printf("my,mx : %f,%f\n",my,mx); |
| rmf_flight | 0:09287ca178ff | 285 | //ここから地磁気の値の補正 |
| rmf_flight | 0:09287ca178ff | 286 | ay = ay - acc_cari_y; |
| rmf_flight | 0:09287ca178ff | 287 | //USBSerial.printf("%d,%05.3f,",seq, ay); |
| rmf_flight | 0:09287ca178ff | 288 | Ele_angle = asin(ay); |
| rmf_flight | 0:09287ca178ff | 289 | Ele_angle = Ele_angle*180/3.14; |
| rmf_flight | 0:09287ca178ff | 290 | |
| rmf_flight | 0:09287ca178ff | 291 | mx = mx + mag_cari_x; |
| rmf_flight | 0:09287ca178ff | 292 | my = my + mag_cari_y; |
| rmf_flight | 0:09287ca178ff | 293 | |
| rmf_flight | 0:09287ca178ff | 294 | nx = (-1)*ay / sqrt(ax*ax+ay*ay); |
| rmf_flight | 0:09287ca178ff | 295 | ny = ax / sqrt(ax*ax+ay*ay); |
| rmf_flight | 0:09287ca178ff | 296 | nz = 0; |
| rmf_flight | 0:09287ca178ff | 297 | |
| rmf_flight | 0:09287ca178ff | 298 | Ele_angle = (-1)*Ele_angle; |
| rmf_flight | 0:09287ca178ff | 299 | |
| rmf_flight | 0:09287ca178ff | 300 | mx = my * (ny*nx*(1-cos(Ele_angle)))+mx*(nx*nx*(1-cos(Ele_angle))+cos(Ele_angle))-mz*ny*sin(Ele_angle); |
| rmf_flight | 0:09287ca178ff | 301 | my = my*(ny*ny*(1-cos( Ele_angle))+cos(Ele_angle))+mx*(nx*ny*(1-cos(Ele_angle)))+mz*nx*sin(Ele_angle); |
| rmf_flight | 0:09287ca178ff | 302 | |
| rmf_flight | 0:09287ca178ff | 303 | |
| rmf_flight | 0:09287ca178ff | 304 | float heading = atan2(my, mx)*180/3.14; //地磁気センサのx,y軸の値を使って、方位を計算(同時に角度の単位をラジアンからdegreeにも変換している) |
| rmf_flight | 0:09287ca178ff | 305 | //twe.printf("heading : %f\n",heading); |
| rmf_flight | 0:09287ca178ff | 306 | heading_int = (int)heading; |
| rmf_flight | 0:09287ca178ff | 307 | //twe.printf("heading_int : %d\n",heading_int); |
| rmf_flight | 0:09287ca178ff | 308 | heading_int = heading_int%360; //角度を0 <= heading < 360 にする |
| rmf_flight | 0:09287ca178ff | 309 | //座標変換 |
| rmf_flight | 0:09287ca178ff | 310 | heading_int = (-1)*heading_int + 90; |
| rmf_flight | 0:09287ca178ff | 311 | if(heading_int >= 180){ |
| rmf_flight | 0:09287ca178ff | 312 | heading_int = heading_int - 360; |
| rmf_flight | 0:09287ca178ff | 313 | } |
| rmf_flight | 0:09287ca178ff | 314 | twe.printf("heading_int : %d\n",heading_int); |
| rmf_flight | 0:09287ca178ff | 315 | twe.printf("sita_int = %d \r\n",sita_int); |
| rmf_flight | 0:09287ca178ff | 316 | |
| rmf_flight | 0:09287ca178ff | 317 | sita_new = heading_int - sita_int; //方位とゴールの方角の差分を計算する |
| rmf_flight | 0:09287ca178ff | 318 | sita_new = abs(sita_new); |
| rmf_flight | 0:09287ca178ff | 319 | twe.printf("sita_new = %d \r\n",sita_new); |
| rmf_flight | 0:09287ca178ff | 320 | wait(0.2); |
| rmf_flight | 0:09287ca178ff | 321 | } |
| rmf_flight | 0:09287ca178ff | 322 | |
| rmf_flight | 0:09287ca178ff | 323 | |
| rmf_flight | 0:09287ca178ff | 324 | twe.printf("moving Straight 4seconds\r\n"); |
| rmf_flight | 0:09287ca178ff | 325 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 326 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 327 | wait(1.0); //角度の差分が15度以内の時は進む |
| rmf_flight | 0:09287ca178ff | 328 | seiten(R_Motor, 1.0); //ゴールまで4秒進む |
| rmf_flight | 0:09287ca178ff | 329 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 330 | wait(4.0); |
| rmf_flight | 0:09287ca178ff | 331 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 332 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 333 | |
| rmf_flight | 0:09287ca178ff | 334 | //twe.printf("moved Straight 4seconds\r\n"); |
| rmf_flight | 0:09287ca178ff | 335 | |
| rmf_flight | 0:09287ca178ff | 336 | } |
| rmf_flight | 0:09287ca178ff | 337 | |
| rmf_flight | 0:09287ca178ff | 338 | else{ //r<=4mになったときにループを抜ける |
| rmf_flight | 0:09287ca178ff | 339 | break; |
| rmf_flight | 0:09287ca178ff | 340 | } |
| rmf_flight | 0:09287ca178ff | 341 | } |
| rmf_flight | 0:09287ca178ff | 342 | |
| rmf_flight | 0:09287ca178ff | 343 | twe.printf("moving Straight 6seconds\r\n"); |
| rmf_flight | 0:09287ca178ff | 344 | seiten(R_Motor, 1.0); //ゴールまで6秒進む |
| rmf_flight | 0:09287ca178ff | 345 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 346 | wait(6.0); |
| rmf_flight | 0:09287ca178ff | 347 | |
| rmf_flight | 0:09287ca178ff | 348 | */ |
| rmf_flight | 0:09287ca178ff | 349 | //--------------------------------------------------------------------------------ここまで、以前の処理方法 |
| rmf_flight | 0:09287ca178ff | 350 | |
| rmf_flight | 0:09287ca178ff | 351 | //ここから、現場で追加した処理 |
| rmf_flight | 0:09287ca178ff | 352 | //最初は現在地からゴールまでの距離計算 |
| rmf_flight | 0:09287ca178ff | 353 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 354 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 355 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 356 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 357 | r = abs(r); //絶対値にする |
| rmf_flight | 0:09287ca178ff | 358 | twe.printf("now_0,%f,%f,%f\n", now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 359 | |
| rmf_flight | 0:09287ca178ff | 360 | |
| rmf_flight | 0:09287ca178ff | 361 | //距離が5m以上ある時、十字に動いて、向かう方向をざっくり決める |
| rmf_flight | 0:09287ca178ff | 362 | if(r>4){ |
| rmf_flight | 0:09287ca178ff | 363 | |
| rmf_flight | 0:09287ca178ff | 364 | //4秒前進 |
| rmf_flight | 0:09287ca178ff | 365 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 366 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 367 | wait(4.0); |
| rmf_flight | 0:09287ca178ff | 368 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 369 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 370 | |
| rmf_flight | 0:09287ca178ff | 371 | wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ |
| rmf_flight | 0:09287ca178ff | 372 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 373 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 374 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 375 | r_first[1] = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 376 | r_first[1] = abs(r_first[1]); //絶対値にする |
| rmf_flight | 0:09287ca178ff | 377 | |
| rmf_flight | 0:09287ca178ff | 378 | twe.printf("now_1,%f,%f,%f\n", now_y, now_x, r_first[1]); |
| rmf_flight | 0:09287ca178ff | 379 | |
| rmf_flight | 0:09287ca178ff | 380 | |
| rmf_flight | 0:09287ca178ff | 381 | //動いた結果近づいたら、90度曲がる |
| rmf_flight | 0:09287ca178ff | 382 | if(r_first[1] < r){ |
| rmf_flight | 0:09287ca178ff | 383 | //90度左回転 |
| rmf_flight | 0:09287ca178ff | 384 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 385 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 386 | wait(M_time_90); //90度その場左回転 |
| rmf_flight | 0:09287ca178ff | 387 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 388 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 389 | |
| rmf_flight | 0:09287ca178ff | 390 | r = r_first[1]; //rに再代入する(そこを初期位置扱いにする) |
| rmf_flight | 0:09287ca178ff | 391 | X = 1; //Xは+1 |
| rmf_flight | 0:09287ca178ff | 392 | } |
| rmf_flight | 0:09287ca178ff | 393 | |
| rmf_flight | 0:09287ca178ff | 394 | //動いた結果遠ざかったら、Uターンして8秒進む |
| rmf_flight | 0:09287ca178ff | 395 | //180度回転 |
| rmf_flight | 0:09287ca178ff | 396 | else{ |
| rmf_flight | 0:09287ca178ff | 397 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 398 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 399 | wait(M_time_180); //180度その場回転 |
| rmf_flight | 0:09287ca178ff | 400 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 401 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 402 | |
| rmf_flight | 0:09287ca178ff | 403 | //8秒前進 |
| rmf_flight | 0:09287ca178ff | 404 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 405 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 406 | wait(8.0); |
| rmf_flight | 0:09287ca178ff | 407 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 408 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 409 | |
| rmf_flight | 0:09287ca178ff | 410 | wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ |
| rmf_flight | 0:09287ca178ff | 411 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 412 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 413 | r_first[2] = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 414 | r_first[2] = abs(r_first[2]); //絶対値にする |
| rmf_flight | 0:09287ca178ff | 415 | r = r_first[2]; //rに再代入(そこを初期位置扱いにする) |
| rmf_flight | 0:09287ca178ff | 416 | |
| rmf_flight | 0:09287ca178ff | 417 | twe.printf("now_2,%f,%f,%f\n", now_y, now_x, r_first[2]); |
| rmf_flight | 0:09287ca178ff | 418 | |
| rmf_flight | 0:09287ca178ff | 419 | |
| rmf_flight | 0:09287ca178ff | 420 | X = -1; //Xは-1 |
| rmf_flight | 0:09287ca178ff | 421 | |
| rmf_flight | 0:09287ca178ff | 422 | //90度右回転 |
| rmf_flight | 0:09287ca178ff | 423 | hanten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 424 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 425 | wait(M_time_90); //90度その場右回転 |
| rmf_flight | 0:09287ca178ff | 426 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 427 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 428 | } |
| rmf_flight | 0:09287ca178ff | 429 | |
| rmf_flight | 0:09287ca178ff | 430 | //最初と90度ずれた軸方向について調べる |
| rmf_flight | 0:09287ca178ff | 431 | //4秒前進 |
| rmf_flight | 0:09287ca178ff | 432 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 433 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 434 | wait(4.0); |
| rmf_flight | 0:09287ca178ff | 435 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 436 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 437 | |
| rmf_flight | 0:09287ca178ff | 438 | wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ |
| rmf_flight | 0:09287ca178ff | 439 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 440 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 441 | r_first[3] = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 442 | r_first[3] = abs(r_first[3]); //絶対値にする |
| rmf_flight | 0:09287ca178ff | 443 | |
| rmf_flight | 0:09287ca178ff | 444 | twe.printf("now_3,%f,%f,%f", now_y, now_x, r_first[3]); |
| rmf_flight | 0:09287ca178ff | 445 | |
| rmf_flight | 0:09287ca178ff | 446 | |
| rmf_flight | 0:09287ca178ff | 447 | if(r_first[3] < r){ |
| rmf_flight | 0:09287ca178ff | 448 | Y = 1; |
| rmf_flight | 0:09287ca178ff | 449 | } |
| rmf_flight | 0:09287ca178ff | 450 | else{ |
| rmf_flight | 0:09287ca178ff | 451 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 452 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 453 | wait(M_time_180); //180度その場回転 |
| rmf_flight | 0:09287ca178ff | 454 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 455 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 456 | |
| rmf_flight | 0:09287ca178ff | 457 | //8秒前進 |
| rmf_flight | 0:09287ca178ff | 458 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 459 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 460 | wait(8.0); |
| rmf_flight | 0:09287ca178ff | 461 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 462 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 463 | |
| rmf_flight | 0:09287ca178ff | 464 | wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ |
| rmf_flight | 0:09287ca178ff | 465 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 466 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 467 | |
| rmf_flight | 0:09287ca178ff | 468 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 469 | r_first[4] = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 470 | r_first[4] = abs(r_first[4]); |
| rmf_flight | 0:09287ca178ff | 471 | twe.printf("now_4,%f,%f,%f\n", now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 472 | |
| rmf_flight | 0:09287ca178ff | 473 | |
| rmf_flight | 0:09287ca178ff | 474 | Y = -1; |
| rmf_flight | 0:09287ca178ff | 475 | } |
| rmf_flight | 0:09287ca178ff | 476 | |
| rmf_flight | 0:09287ca178ff | 477 | } |
| rmf_flight | 0:09287ca178ff | 478 | //-------------ここまでif文 |
| rmf_flight | 0:09287ca178ff | 479 | |
| rmf_flight | 0:09287ca178ff | 480 | //モード分けをする |
| rmf_flight | 0:09287ca178ff | 481 | if(X == 1 && Y == 1){ |
| rmf_flight | 0:09287ca178ff | 482 | mode = 1; |
| rmf_flight | 0:09287ca178ff | 483 | } |
| rmf_flight | 0:09287ca178ff | 484 | if(X == -1 && Y == 1){ |
| rmf_flight | 0:09287ca178ff | 485 | mode = 2; |
| rmf_flight | 0:09287ca178ff | 486 | } |
| rmf_flight | 0:09287ca178ff | 487 | if(X == -1 && Y == -1){ |
| rmf_flight | 0:09287ca178ff | 488 | mode = 3; |
| rmf_flight | 0:09287ca178ff | 489 | } |
| rmf_flight | 0:09287ca178ff | 490 | if(X == 1 && Y == -1){ |
| rmf_flight | 0:09287ca178ff | 491 | mode = 4; |
| rmf_flight | 0:09287ca178ff | 492 | } |
| rmf_flight | 0:09287ca178ff | 493 | |
| rmf_flight | 0:09287ca178ff | 494 | //---------------------------------------ここからモード番号によって処理が変わる |
| rmf_flight | 0:09287ca178ff | 495 | |
| rmf_flight | 0:09287ca178ff | 496 | if(mode == 1){ //ゴールが第一象限にある場合 |
| rmf_flight | 0:09287ca178ff | 497 | twe.printf("mode1\n"); |
| rmf_flight | 0:09287ca178ff | 498 | |
| rmf_flight | 0:09287ca178ff | 499 | //右に45度回転する |
| rmf_flight | 0:09287ca178ff | 500 | hanten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 501 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 502 | wait(M_time_45); //45度右回転 |
| rmf_flight | 0:09287ca178ff | 503 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 504 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 505 | |
| rmf_flight | 0:09287ca178ff | 506 | //距離が10m以上の時に、45度の方向で進む |
| rmf_flight | 0:09287ca178ff | 507 | while(r > 10){ |
| rmf_flight | 0:09287ca178ff | 508 | //ずっと前進 |
| rmf_flight | 0:09287ca178ff | 509 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 510 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 511 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 512 | |
| rmf_flight | 0:09287ca178ff | 513 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 514 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 515 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 516 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 517 | r = abs(r); |
| rmf_flight | 0:09287ca178ff | 518 | twe.printf("mode1,r>10,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 519 | } |
| rmf_flight | 0:09287ca178ff | 520 | |
| rmf_flight | 0:09287ca178ff | 521 | //ループを出たら、一旦止める |
| rmf_flight | 0:09287ca178ff | 522 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 523 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 524 | |
| rmf_flight | 0:09287ca178ff | 525 | while(r > 4){ //4 < r < 10 [m]になったら、緯度と経度で差の大きい方を縮めるように動く |
| rmf_flight | 0:09287ca178ff | 526 | wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ |
| rmf_flight | 0:09287ca178ff | 527 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 528 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 529 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 530 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 531 | r = abs(r); |
| rmf_flight | 0:09287ca178ff | 532 | twe.printf("mode1,r>4,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 533 | |
| rmf_flight | 0:09287ca178ff | 534 | delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む |
| rmf_flight | 0:09287ca178ff | 535 | delta_y = goal_y - now_y; |
| rmf_flight | 0:09287ca178ff | 536 | delta_x = abs(delta_x); |
| rmf_flight | 0:09287ca178ff | 537 | delta_y = abs(delta_y); |
| rmf_flight | 0:09287ca178ff | 538 | |
| rmf_flight | 0:09287ca178ff | 539 | if(delta_x > delta_y){ |
| rmf_flight | 0:09287ca178ff | 540 | //右に45度回転する |
| rmf_flight | 0:09287ca178ff | 541 | hanten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 542 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 543 | wait(M_time_45); //45度右回転 |
| rmf_flight | 0:09287ca178ff | 544 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 545 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 546 | } |
| rmf_flight | 0:09287ca178ff | 547 | |
| rmf_flight | 0:09287ca178ff | 548 | else if(delta_y > delta_x){ |
| rmf_flight | 0:09287ca178ff | 549 | //左に45度回転する |
| rmf_flight | 0:09287ca178ff | 550 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 551 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 552 | wait(M_time_45); //45度左回転 |
| rmf_flight | 0:09287ca178ff | 553 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 554 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 555 | } |
| rmf_flight | 0:09287ca178ff | 556 | else{ |
| rmf_flight | 0:09287ca178ff | 557 | //この場合は緯度と経度の差が等しいので、そのまま進んでOK |
| rmf_flight | 0:09287ca178ff | 558 | } |
| rmf_flight | 0:09287ca178ff | 559 | //条件に当てはまる限り進む |
| rmf_flight | 0:09287ca178ff | 560 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 561 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 562 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 563 | |
| rmf_flight | 0:09287ca178ff | 564 | } |
| rmf_flight | 0:09287ca178ff | 565 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 566 | stop(L_Motor); //止めて終わり |
| rmf_flight | 0:09287ca178ff | 567 | } |
| rmf_flight | 0:09287ca178ff | 568 | |
| rmf_flight | 0:09287ca178ff | 569 | |
| rmf_flight | 0:09287ca178ff | 570 | //------------------------------------------ |
| rmf_flight | 0:09287ca178ff | 571 | if(mode == 2){ //ゴールが第二象限にある場合 |
| rmf_flight | 0:09287ca178ff | 572 | twe.printf("mode2\n"); |
| rmf_flight | 0:09287ca178ff | 573 | |
| rmf_flight | 0:09287ca178ff | 574 | //左に45度回転する |
| rmf_flight | 0:09287ca178ff | 575 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 576 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 577 | wait(M_time_45); //45度左回転 |
| rmf_flight | 0:09287ca178ff | 578 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 579 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 580 | |
| rmf_flight | 0:09287ca178ff | 581 | //距離が10m以上の時に、45度の方向で進む |
| rmf_flight | 0:09287ca178ff | 582 | while(r > 10){ |
| rmf_flight | 0:09287ca178ff | 583 | //ずっと前進 |
| rmf_flight | 0:09287ca178ff | 584 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 585 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 586 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 587 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 588 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 589 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 590 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 591 | r = abs(r); |
| rmf_flight | 0:09287ca178ff | 592 | twe.printf("mode2,r>10,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 593 | |
| rmf_flight | 0:09287ca178ff | 594 | } |
| rmf_flight | 0:09287ca178ff | 595 | |
| rmf_flight | 0:09287ca178ff | 596 | //ループを出たら、一旦止める |
| rmf_flight | 0:09287ca178ff | 597 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 598 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 599 | |
| rmf_flight | 0:09287ca178ff | 600 | while(r > 4){ //4 < r < 10 [m]になったら、緯度と経度で差の大きい方を縮めるように動く |
| rmf_flight | 0:09287ca178ff | 601 | wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ |
| rmf_flight | 0:09287ca178ff | 602 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 603 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 604 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 605 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 606 | r = abs(r); |
| rmf_flight | 0:09287ca178ff | 607 | twe.printf("mode2,r>4,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 608 | |
| rmf_flight | 0:09287ca178ff | 609 | |
| rmf_flight | 0:09287ca178ff | 610 | delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む |
| rmf_flight | 0:09287ca178ff | 611 | delta_y = goal_y - now_y; |
| rmf_flight | 0:09287ca178ff | 612 | delta_x = abs(delta_x); |
| rmf_flight | 0:09287ca178ff | 613 | delta_y = abs(delta_y); |
| rmf_flight | 0:09287ca178ff | 614 | |
| rmf_flight | 0:09287ca178ff | 615 | if(delta_x > delta_y){ |
| rmf_flight | 0:09287ca178ff | 616 | //左に45度回転する |
| rmf_flight | 0:09287ca178ff | 617 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 618 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 619 | wait(M_time_45); //45度左回転 |
| rmf_flight | 0:09287ca178ff | 620 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 621 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 622 | } |
| rmf_flight | 0:09287ca178ff | 623 | |
| rmf_flight | 0:09287ca178ff | 624 | else if(delta_y > delta_x){ |
| rmf_flight | 0:09287ca178ff | 625 | //右に45度回転する |
| rmf_flight | 0:09287ca178ff | 626 | hanten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 627 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 628 | wait(M_time_45); //45度右回転 |
| rmf_flight | 0:09287ca178ff | 629 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 630 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 631 | } |
| rmf_flight | 0:09287ca178ff | 632 | else{ |
| rmf_flight | 0:09287ca178ff | 633 | //この場合は緯度と経度の差が等しいので、そのまま進んでOK |
| rmf_flight | 0:09287ca178ff | 634 | } |
| rmf_flight | 0:09287ca178ff | 635 | //条件に当てはまる限り進む |
| rmf_flight | 0:09287ca178ff | 636 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 637 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 638 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 639 | } |
| rmf_flight | 0:09287ca178ff | 640 | |
| rmf_flight | 0:09287ca178ff | 641 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 642 | stop(L_Motor); //止めて終わり |
| rmf_flight | 0:09287ca178ff | 643 | } |
| rmf_flight | 0:09287ca178ff | 644 | |
| rmf_flight | 0:09287ca178ff | 645 | |
| rmf_flight | 0:09287ca178ff | 646 | |
| rmf_flight | 0:09287ca178ff | 647 | //------------------------------------------ |
| rmf_flight | 0:09287ca178ff | 648 | if(mode == 3){ //ゴールが第三象限にある場合 |
| rmf_flight | 0:09287ca178ff | 649 | twe.printf("mode3\n"); |
| rmf_flight | 0:09287ca178ff | 650 | |
| rmf_flight | 0:09287ca178ff | 651 | //右に45度回転する |
| rmf_flight | 0:09287ca178ff | 652 | hanten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 653 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 654 | wait(M_time_45); //45度右回転 |
| rmf_flight | 0:09287ca178ff | 655 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 656 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 657 | |
| rmf_flight | 0:09287ca178ff | 658 | //距離が10m以上の時に、45度の方向で進む |
| rmf_flight | 0:09287ca178ff | 659 | while(r > 10){ |
| rmf_flight | 0:09287ca178ff | 660 | //ずっと前進 |
| rmf_flight | 0:09287ca178ff | 661 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 662 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 663 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 664 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 665 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 666 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 667 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 668 | r = abs(r); |
| rmf_flight | 0:09287ca178ff | 669 | twe.printf("mode3,r>10,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 670 | |
| rmf_flight | 0:09287ca178ff | 671 | } |
| rmf_flight | 0:09287ca178ff | 672 | |
| rmf_flight | 0:09287ca178ff | 673 | //ループを出たら、一旦止める |
| rmf_flight | 0:09287ca178ff | 674 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 675 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 676 | |
| rmf_flight | 0:09287ca178ff | 677 | while(r > 4){ //4 < r < 10 [m]になったら、緯度と経度で差の大きい方を縮めるように動く |
| rmf_flight | 0:09287ca178ff | 678 | wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ |
| rmf_flight | 0:09287ca178ff | 679 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 680 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 681 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 682 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 683 | r = abs(r); |
| rmf_flight | 0:09287ca178ff | 684 | twe.printf("mode3,r>4,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 685 | |
| rmf_flight | 0:09287ca178ff | 686 | |
| rmf_flight | 0:09287ca178ff | 687 | delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む |
| rmf_flight | 0:09287ca178ff | 688 | delta_y = goal_y - now_y; |
| rmf_flight | 0:09287ca178ff | 689 | delta_x = abs(delta_x); |
| rmf_flight | 0:09287ca178ff | 690 | delta_y = abs(delta_y); |
| rmf_flight | 0:09287ca178ff | 691 | |
| rmf_flight | 0:09287ca178ff | 692 | if(delta_x > delta_y){ |
| rmf_flight | 0:09287ca178ff | 693 | //右に45度回転する |
| rmf_flight | 0:09287ca178ff | 694 | hanten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 695 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 696 | wait(M_time_45); //45度右回転 |
| rmf_flight | 0:09287ca178ff | 697 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 698 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 699 | } |
| rmf_flight | 0:09287ca178ff | 700 | |
| rmf_flight | 0:09287ca178ff | 701 | else if(delta_y > delta_x){ |
| rmf_flight | 0:09287ca178ff | 702 | //左に45度回転する |
| rmf_flight | 0:09287ca178ff | 703 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 704 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 705 | wait(M_time_45); //45度左回転 |
| rmf_flight | 0:09287ca178ff | 706 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 707 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 708 | } |
| rmf_flight | 0:09287ca178ff | 709 | else{ |
| rmf_flight | 0:09287ca178ff | 710 | //この場合は緯度と経度の差が等しいので、そのまま進んでOK |
| rmf_flight | 0:09287ca178ff | 711 | } |
| rmf_flight | 0:09287ca178ff | 712 | //条件に当てはまる限り進む |
| rmf_flight | 0:09287ca178ff | 713 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 714 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 715 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 716 | } |
| rmf_flight | 0:09287ca178ff | 717 | |
| rmf_flight | 0:09287ca178ff | 718 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 719 | stop(L_Motor); //止めて終わり |
| rmf_flight | 0:09287ca178ff | 720 | } |
| rmf_flight | 0:09287ca178ff | 721 | |
| rmf_flight | 0:09287ca178ff | 722 | |
| rmf_flight | 0:09287ca178ff | 723 | //------------------------------------------ |
| rmf_flight | 0:09287ca178ff | 724 | if(mode == 4){ //ゴールが第四象限にある場合 |
| rmf_flight | 0:09287ca178ff | 725 | twe.printf("mode4\n"); |
| rmf_flight | 0:09287ca178ff | 726 | |
| rmf_flight | 0:09287ca178ff | 727 | //左に45度回転する |
| rmf_flight | 0:09287ca178ff | 728 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 729 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 730 | wait(M_time_45); //45度左回転 |
| rmf_flight | 0:09287ca178ff | 731 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 732 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 733 | |
| rmf_flight | 0:09287ca178ff | 734 | //距離が10m以上の時に、45度の方向で進む |
| rmf_flight | 0:09287ca178ff | 735 | while(r > 10){ |
| rmf_flight | 0:09287ca178ff | 736 | //ずっと前進 |
| rmf_flight | 0:09287ca178ff | 737 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 738 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 739 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 740 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 741 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 742 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 743 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 744 | r = abs(r); |
| rmf_flight | 0:09287ca178ff | 745 | twe.printf("mode4,r>10,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 746 | |
| rmf_flight | 0:09287ca178ff | 747 | } |
| rmf_flight | 0:09287ca178ff | 748 | |
| rmf_flight | 0:09287ca178ff | 749 | //ループを出たら、一旦止める |
| rmf_flight | 0:09287ca178ff | 750 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 751 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 752 | |
| rmf_flight | 0:09287ca178ff | 753 | while(r > 4){ //4 < r < 10 [m]になったら、緯度と経度で差の大きい方を縮めるように動く |
| rmf_flight | 0:09287ca178ff | 754 | wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ |
| rmf_flight | 0:09287ca178ff | 755 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 756 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 757 | //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す |
| rmf_flight | 0:09287ca178ff | 758 | r = R * acos(sin(goal_y*3.14/180) * sin(now_y*3.14/180)+cos(now_y*3.14/180) * cos(goal_y*3.14/180)*cos((goal_x - now_x)*3.14/180)); |
| rmf_flight | 0:09287ca178ff | 759 | r = abs(r); |
| rmf_flight | 0:09287ca178ff | 760 | twe.printf("mode4,r>4,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r); |
| rmf_flight | 0:09287ca178ff | 761 | |
| rmf_flight | 0:09287ca178ff | 762 | delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む |
| rmf_flight | 0:09287ca178ff | 763 | delta_y = goal_y - now_y; |
| rmf_flight | 0:09287ca178ff | 764 | delta_x = abs(delta_x); |
| rmf_flight | 0:09287ca178ff | 765 | delta_y = abs(delta_y); |
| rmf_flight | 0:09287ca178ff | 766 | |
| rmf_flight | 0:09287ca178ff | 767 | if(delta_x > delta_y){ |
| rmf_flight | 0:09287ca178ff | 768 | //左に45度回転する |
| rmf_flight | 0:09287ca178ff | 769 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 770 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 771 | wait(M_time_45); //45度左回転 |
| rmf_flight | 0:09287ca178ff | 772 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 773 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 774 | } |
| rmf_flight | 0:09287ca178ff | 775 | |
| rmf_flight | 0:09287ca178ff | 776 | else if(delta_y > delta_x){ |
| rmf_flight | 0:09287ca178ff | 777 | //右に45度回転する |
| rmf_flight | 0:09287ca178ff | 778 | hanten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 779 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 780 | wait(M_time_45); //45度右回転 |
| rmf_flight | 0:09287ca178ff | 781 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 782 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 783 | } |
| rmf_flight | 0:09287ca178ff | 784 | else{ |
| rmf_flight | 0:09287ca178ff | 785 | //この場合は緯度と経度の差が等しいので、そのまま進んでOK |
| rmf_flight | 0:09287ca178ff | 786 | } |
| rmf_flight | 0:09287ca178ff | 787 | //条件に当てはまる限り進む |
| rmf_flight | 0:09287ca178ff | 788 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 789 | seiten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 790 | wait(1.0); |
| rmf_flight | 0:09287ca178ff | 791 | } |
| rmf_flight | 0:09287ca178ff | 792 | |
| rmf_flight | 0:09287ca178ff | 793 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 794 | stop(L_Motor); //止めて終わり |
| rmf_flight | 0:09287ca178ff | 795 | } |
| rmf_flight | 0:09287ca178ff | 796 | |
| rmf_flight | 0:09287ca178ff | 797 | |
| rmf_flight | 0:09287ca178ff | 798 | twe.printf("UltraSound!!\n"); |
| rmf_flight | 0:09287ca178ff | 799 | |
| rmf_flight | 0:09287ca178ff | 800 | //ここから超音波 |
| rmf_flight | 0:09287ca178ff | 801 | while(1){ |
| rmf_flight | 0:09287ca178ff | 802 | |
| rmf_flight | 0:09287ca178ff | 803 | for(i=0;i<24;i++){ //15dgree |
| rmf_flight | 0:09287ca178ff | 804 | |
| rmf_flight | 0:09287ca178ff | 805 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 806 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 807 | wait_ms(3000); |
| rmf_flight | 0:09287ca178ff | 808 | usensor.start(); |
| rmf_flight | 0:09287ca178ff | 809 | wait_ms(500); |
| rmf_flight | 0:09287ca178ff | 810 | d[i]=usensor.get_dist_cm(); |
| rmf_flight | 0:09287ca178ff | 811 | twe.printf("%d cmA\r\n",d[i]); |
| rmf_flight | 0:09287ca178ff | 812 | |
| rmf_flight | 0:09287ca178ff | 813 | //0問題解決用 |
| rmf_flight | 0:09287ca178ff | 814 | if(d[i]<=1){ |
| rmf_flight | 0:09287ca178ff | 815 | wait(5); |
| rmf_flight | 0:09287ca178ff | 816 | usensor.start(); |
| rmf_flight | 0:09287ca178ff | 817 | wait_ms(500); |
| rmf_flight | 0:09287ca178ff | 818 | d[i]=usensor.get_dist_cm(); |
| rmf_flight | 0:09287ca178ff | 819 | twe.printf("Re:%d cmA\r\n",d[i]); |
| rmf_flight | 0:09287ca178ff | 820 | } |
| rmf_flight | 0:09287ca178ff | 821 | |
| rmf_flight | 0:09287ca178ff | 822 | if(d[i]<3){ |
| rmf_flight | 0:09287ca178ff | 823 | pc.printf("END"); //3cm以下になったらEND |
| rmf_flight | 0:09287ca178ff | 824 | while(1){}} //無限ループで終わり |
| rmf_flight | 0:09287ca178ff | 825 | |
| rmf_flight | 0:09287ca178ff | 826 | if(0<d[i]&&d[i]<10){break;} //0 < d < 10[cm]になったら抜ける |
| rmf_flight | 0:09287ca178ff | 827 | |
| rmf_flight | 0:09287ca178ff | 828 | seiten(R_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 829 | hanten(L_Motor, 1.0); |
| rmf_flight | 0:09287ca178ff | 830 | } |
| rmf_flight | 0:09287ca178ff | 831 | |
| rmf_flight | 0:09287ca178ff | 832 | stop(R_Motor); |
| rmf_flight | 0:09287ca178ff | 833 | stop(L_Motor); |
| rmf_flight | 0:09287ca178ff | 834 | wait(3.6); |
| rmf_flight | 0:09287ca178ff | 835 | twe.printf("SEITEN"); |
| rmf_flight | 0:09287ca178ff | 836 | seiten(R_Motor,1.0); |
| rmf_flight | 0:09287ca178ff | 837 | seiten(L_Motor,1.0); |
| rmf_flight | 0:09287ca178ff | 838 | wait(2.0); |
| rmf_flight | 0:09287ca178ff | 839 | now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度 |
| rmf_flight | 0:09287ca178ff | 840 | now_x = michibiki.longtitude; //経度 |
| rmf_flight | 0:09287ca178ff | 841 | twe.printf("Ultra,lat_now,%f,long_now,%f\n",now_y, now_x); |
| rmf_flight | 0:09287ca178ff | 842 | |
| rmf_flight | 0:09287ca178ff | 843 | } |
| rmf_flight | 0:09287ca178ff | 844 | |
| rmf_flight | 0:09287ca178ff | 845 | |
| rmf_flight | 0:09287ca178ff | 846 | } |
| rmf_flight | 0:09287ca178ff | 847 |