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