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.
Diff: main.cpp
- Revision:
- 0:09287ca178ff
- Child:
- 1:038eca93a8f4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Sep 10 12:37:29 2021 +0000
@@ -0,0 +1,847 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h> //距離を求める際に逆三角関数を使うため必要
+#include "mbed.h"
+#include "BME280.h"
+#include "GPS.h"
+#include "hcsr04.h"
+//#include "SDFileSystem.h" //<==これをつけるとコンパイルできない
+#include "Servo.h"
+#include "BMX_055.h"
+#include "Moter_drive.h"
+
+//地磁気センサの補正用の値
+#define mag_cari_x 18.75
+#define mag_cari_y -26.875
+#define acc_cari_y 0
+#define Max_t 110
+
+//モーター関係
+#define M_time_180 3.6 //180度その場回転する時に必要な秒数
+#define M_time_90 1.8 //90度その場回転する時に必要な秒数
+#define M_time_45 0.9 //45度その場回転する時に必要な秒数
+
+
+
+//通信関係
+Serial pc(USBTX, USBRX, 115200);
+Serial twe(A7, A2, 115200);
+//Serial USBSerial(PA_2,PA_15);
+
+
+BME280 sensor(D4, D5);
+GPS michibiki(GPSTX,GPSRX);
+HCSR04 usensor(D6, D3); //それぞれ Trig, Echo につなぐ
+//SDFileSystem sd(A6, A5, A4, D8, "sd"); // the pinout on the mbed Cool Components workshop board //<== これをつけるとコンパイルできない
+Servo myservo(A3); //パラシュート切り離し機構のサーボ
+BMX055 bmx;
+DigitalOut myled(LED1); //goal検知
+
+
+const double goal_x = 139.987503; //ゴールの経度(初期値は適当,35.657941,139.542798
+const double goal_y = 40.142727; //ゴールの緯度
+const double R = 6371000.0; //地球の半径 [m]
+double delta_x, delta_y, now_x, now_y, sita, goal_y_rad,temp1,temp2; //ゴールまでの直線と機体の位置とのなす角度
+double r = 10; //ゴールまでの残り距離
+double r_first[5]; //ただし、[0]は使わない
+float ax, ay, az, gx, gy, gz, mx, my, mz; //9軸センサの値を入れる変数
+int heading_int, sita_int;
+float Ele_angle; //仰角(水平面と基板がなす角度)
+float nx, ny, nz; //地磁気の補正用
+double X,Y; //ゴールがどの象限にいるかを区別するための変数
+double mode; //ゴールの位置によってモードを変える
+int i; //超音波でのループ用変数
+int d[25];
+
+#include "Flight_Pin.h" //フライトピンについての定義及び処理内容(処理内容により、このヘッダーはtwe,now_x,now_yの宣言の後におく必要がある！)
+#include "Parachute_detach.h"
+
+
+//9軸センサからデータを取得する関数
+void Getdata(){
+ bmx.getMotion9(&ax,&ay,&az, &gx,&gy,&gz, &mx,&my,&mz);
+}
+
+#include "landing_detect.h"
+
+
+int main() {
+
+ /*
+ while(1){ //テスト用、実際は実装しない
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+
+ twe.printf("lat_now : %f,long_now : %f\n",now_y, now_x);
+
+ delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む
+ delta_y = goal_y - now_y;
+
+ 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));
+
+ r = abs(r); //絶対値にする
+
+ twe.printf("r : %f\n",r);
+
+ //方角を求める
+ delta_x = delta_x * 3.14/180;
+ now_x = now_x * 3.14/180;
+ goal_y_rad = goal_y * 3.14/180;
+ now_y = now_y * 3.14/180;
+
+ //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));
+ temp1 = cos(now_y)*tan(goal_y_rad);
+ temp2 = sin(now_y)*cos(delta_x);
+
+ twe.printf("temp1 : %f, temp2 : %f\n",temp1,temp2);
+
+ //sita = atan2(sin(delta_x),cos(now_x)*tan(goal_y_rad)-sin(now_y)*cos(delta_x));
+ sita = atan2(sin(delta_x),(temp1-temp2));
+ sita = sita * 180/3.14; //ラジアンからdegreeに戻す
+
+ twe.printf("sita : %f\n",sita);
+
+ sita_int = (int)sita;
+ sita_int = sita_int % 360;
+ twe.printf("sita_int : %d\n",sita_int);
+
+ wait(1.0);
+ }
+ */
+
+ bmx.init(); //bmx055(9軸センサの初期化)
+
+/*
+// PCとの通信
+ int a, b, c, d;
+ pc.printf("Hello World!\r\n");
+ mkdir("/sd/mydir", 0777);
+ FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
+ if(fp == NULL) {
+ error("Could not open file for write\r\n");
+ }
+ fprintf(fp, "Hello fun SD Card World!");
+ fclose(fp);
+
+ // CSVファイル作成
+ fp = fopen("/sd/mydir/data.csv", "w");
+ if(fp == NULL) {
+ error("Could not open file for write\r\n");
+ }
+ fprintf(fp, "10,11,12,13\n");
+ fprintf(fp, "20,21,22,23\n");
+ fprintf(fp, "30,31,32,33\n");
+ fclose(fp);
+
+ //CSVファイル読み込み
+ fp = fopen("/sd/mydir/data.csv", "r");
+ if(fp == NULL) {
+ error("Could not open file for write\r\n");
+ }
+ while( fscanf( fp, "%d,%d,%d,%d",&a, &b, &c, &d ) != EOF ){
+ pc.printf("%d,%d,%d,%d\r\n",a, b, c, d);
+ }
+ fclose(fp);
+
+ printf("Goodbye World!\r\n"); //microsd
+ */ //これをつけるとコンパイルできない
+
+ /*
+ pc.printf("%2.2f degC, %04.2f hPa, %2.2f %%\r\n", sensor.getTemperature(), sensor.getPressure(), sensor.getHumidity()); //BME280
+
+ pc.printf("%f %f\r\n",michibiki.latitude,michibiki.longtitude); //GPS
+
+ usensor.start();
+ wait_ms(500);
+ unsigned int dist = usensor.get_dist_cm();
+ pc.printf("%ld cm\r\n",dist ); //hcsr04
+
+ float ax, ay, az, gx, gy, gz, mx, my, mz;
+ bmx.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
+ 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); //これをつけるとコンパイルできない
+ */
+
+ //-----------------------------------------------------------------------------------------------
+
+ //フライトピンの接続状況を確認する
+ flight_pin_loop(); //Flight_Pin.hで定義された関数
+
+ //着地状況を確認する
+ landing_detect(); //landing_detect.hで定義された関数
+
+ //パラシュートを切り離す
+ parachute_detach(); //Parachute_detach.hで定義された関数
+
+
+ //--------------------------------------------------------------------------------------ここから以前の処理方法(採用無し)
+ //パラシュートを切り離したあと、GPSと地磁気の情報を元にゴールまで進むプログラム
+ /*
+ while(1){ //とりあえず無限ループ(終了条件は内部で定義)
+
+ if(r > 4){ //ゴールまで4m以上ある時
+ Getdata(); //9軸センサからデータを取得する
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+
+ twe.printf("-----------------------------\n");
+ twe.printf(",lat_now,%f,long_now,%f\n",now_y, now_x);
+
+ delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む
+ delta_y = goal_y - now_y;
+
+ //以前までの、距離を求める式(多分使わない)
+ //delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む
+ //delta_y = goal_y - now_y;
+ //r = sqrt(delta_x * delta_x + delta_y * delta_y); //ゴールまでの距離を求める(3平方の定理)
+
+
+
+
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+
+ r = abs(r); //絶対値にする
+
+ twe.printf("r : %f\n",r);
+
+ //sita = atan2l(delta_y, delta_x)*180/3.14; //現在地点からゴールまでの角度を求める(同時に角度の単位をラジアンからdegreeにも変換している)
+
+ //方角を求める(degree ==> rad)
+ delta_x = delta_x * 3.14/180;
+ now_x = now_x * 3.14/180;
+ goal_y_rad = goal_y * 3.14/180;
+ now_y = now_y * 3.14/180;
+
+ temp1 = cos(now_y)*tan(goal_y_rad);
+ temp2 = sin(now_y)*cos(delta_x);
+
+ sita = atan2(sin(delta_x),(temp1-temp2));
+ sita = sita * 180/3.14; //ラジアンからdegreeに戻す
+
+ twe.printf("sita : %f\n",sita);
+
+ sita_int = (int)sita;
+ sita_int = sita_int % 360;
+ //twe.printf("sita_int : %d\n",sita_int);
+ //座標変換
+ if(sita_int > -270 && sita_int < -180){
+ sita_int = sita_int + 360;
+ }
+ twe.printf("sita_int : %d\n",sita_int);
+
+
+ //twe.printf("my,mx : %f,%f\n",my,mx);
+ //ここから地磁気の値の補正
+ ay = ay - acc_cari_y;
+ //USBSerial.printf("%d,%05.3f,",seq, ay);
+ Ele_angle = asin(ay);
+ Ele_angle = Ele_angle*180/3.14;
+
+ mx = mx + mag_cari_x;
+ my = my + mag_cari_y;
+
+ nx = (-1)*ay / sqrt(ax*ax+ay*ay);
+ ny = ax / sqrt(ax*ax+ay*ay);
+ nz = 0;
+
+ Ele_angle = (-1)*Ele_angle;
+
+ mx = my * (ny*nx*(1-cos(Ele_angle)))+mx*(nx*nx*(1-cos(Ele_angle))+cos(Ele_angle))-mz*ny*sin(Ele_angle);
+ my = my*(ny*ny*(1-cos(Ele_angle))+cos(Ele_angle))+mx*(nx*ny*(1-cos(Ele_angle)))+mz*nx*sin(Ele_angle);
+
+
+
+ float heading = atan2(my, mx)*180/3.14; //地磁気センサのx,y軸の値を使って、方位を計算(同時に角度の単位をラジアンからdegreeにも変換している)
+ //twe.printf("heading : %f\n",heading);
+ heading_int = (int)heading;
+ //twe.printf("heading_int : %d\n",heading_int);
+ heading_int = heading_int%360; //角度を0 <= heading < 360 にする
+
+ //座標変換
+ heading_int = (-1)*heading_int + 90;
+ if(heading_int >= 180){
+ heading_int = heading_int - 360;
+ }
+ twe.printf("heading_int : %d\n",heading_int);
+
+
+
+
+ int sita_new = heading_int - sita_int; //方位とゴールの方角の差分を計算する
+
+ twe.printf("sita_new = %d \r\n",sita_new);
+
+ //この差分sita_newを0にする(つまり、ゴールの方を向くようにする)
+ sita_new = abs(sita_new);
+ while(sita_new > 35){
+ twe.printf("while -----------------------------\n");
+ //右だけタイヤを回して、機体をその場回転させる
+ seiten(R_Motor, 1.0);
+ wait(0.05);
+ stop(R_Motor);
+
+ Getdata(); //9軸センサからデータを取得する
+ //twe.printf("my,mx : %f,%f\n",my,mx);
+ //ここから地磁気の値の補正
+ ay = ay - acc_cari_y;
+ //USBSerial.printf("%d,%05.3f,",seq, ay);
+ Ele_angle = asin(ay);
+ Ele_angle = Ele_angle*180/3.14;
+
+ mx = mx + mag_cari_x;
+ my = my + mag_cari_y;
+
+ nx = (-1)*ay / sqrt(ax*ax+ay*ay);
+ ny = ax / sqrt(ax*ax+ay*ay);
+ nz = 0;
+
+ Ele_angle = (-1)*Ele_angle;
+
+ mx = my * (ny*nx*(1-cos(Ele_angle)))+mx*(nx*nx*(1-cos(Ele_angle))+cos(Ele_angle))-mz*ny*sin(Ele_angle);
+ my = my*(ny*ny*(1-cos( Ele_angle))+cos(Ele_angle))+mx*(nx*ny*(1-cos(Ele_angle)))+mz*nx*sin(Ele_angle);
+
+
+ float heading = atan2(my, mx)*180/3.14; //地磁気センサのx,y軸の値を使って、方位を計算(同時に角度の単位をラジアンからdegreeにも変換している)
+ //twe.printf("heading : %f\n",heading);
+ heading_int = (int)heading;
+ //twe.printf("heading_int : %d\n",heading_int);
+ heading_int = heading_int%360; //角度を0 <= heading < 360 にする
+ //座標変換
+ heading_int = (-1)*heading_int + 90;
+ if(heading_int >= 180){
+ heading_int = heading_int - 360;
+ }
+ twe.printf("heading_int : %d\n",heading_int);
+ twe.printf("sita_int = %d \r\n",sita_int);
+
+ sita_new = heading_int - sita_int; //方位とゴールの方角の差分を計算する
+ sita_new = abs(sita_new);
+ twe.printf("sita_new = %d \r\n",sita_new);
+ wait(0.2);
+ }
+
+
+ twe.printf("moving Straight 4seconds\r\n");
+ stop(R_Motor);
+ stop(L_Motor);
+ wait(1.0); //角度の差分が15度以内の時は進む
+ seiten(R_Motor, 1.0); //ゴールまで4秒進む
+ seiten(L_Motor, 1.0);
+ wait(4.0);
+ stop(R_Motor);
+ stop(L_Motor);
+
+ //twe.printf("moved Straight 4seconds\r\n");
+
+ }
+
+ else{ //r<=4mになったときにループを抜ける
+ break;
+ }
+ }
+
+ twe.printf("moving Straight 6seconds\r\n");
+ seiten(R_Motor, 1.0); //ゴールまで6秒進む
+ seiten(L_Motor, 1.0);
+ wait(6.0);
+
+ */
+ //--------------------------------------------------------------------------------ここまで、以前の処理方法
+
+ //ここから、現場で追加した処理
+ //最初は現在地からゴールまでの距離計算
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r); //絶対値にする
+ twe.printf("now_0,%f,%f,%f\n", now_y, now_x, r);
+
+
+ //距離が5m以上ある時、十字に動いて、向かう方向をざっくり決める
+ if(r>4){
+
+ //4秒前進
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(4.0);
+ stop(R_Motor);
+ stop(L_Motor);
+
+ wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r_first[1] = abs(r_first[1]); //絶対値にする
+
+ twe.printf("now_1,%f,%f,%f\n", now_y, now_x, r_first[1]);
+
+
+ //動いた結果近づいたら、90度曲がる
+ if(r_first[1] < r){
+ //90度左回転
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_90); //90度その場左回転
+ stop(R_Motor);
+ stop(L_Motor);
+
+ r = r_first[1]; //rに再代入する(そこを初期位置扱いにする)
+ X = 1; //Xは+1
+ }
+
+ //動いた結果遠ざかったら、Uターンして8秒進む
+ //180度回転
+ else{
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_180); //180度その場回転
+ stop(R_Motor);
+ stop(L_Motor);
+
+ //8秒前進
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(8.0);
+ stop(R_Motor);
+ stop(L_Motor);
+
+ wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ 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));
+ r_first[2] = abs(r_first[2]); //絶対値にする
+ r = r_first[2]; //rに再代入(そこを初期位置扱いにする)
+
+ twe.printf("now_2,%f,%f,%f\n", now_y, now_x, r_first[2]);
+
+
+ X = -1; //Xは-1
+
+ //90度右回転
+ hanten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(M_time_90); //90度その場右回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+
+ //最初と90度ずれた軸方向について調べる
+ //4秒前進
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(4.0);
+ stop(R_Motor);
+ stop(L_Motor);
+
+ wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ 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));
+ r_first[3] = abs(r_first[3]); //絶対値にする
+
+ twe.printf("now_3,%f,%f,%f", now_y, now_x, r_first[3]);
+
+
+ if(r_first[3] < r){
+ Y = 1;
+ }
+ else{
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_180); //180度その場回転
+ stop(R_Motor);
+ stop(L_Motor);
+
+ //8秒前進
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(8.0);
+ stop(R_Motor);
+ stop(L_Motor);
+
+ wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r_first[4] = abs(r_first[4]);
+ twe.printf("now_4,%f,%f,%f\n", now_y, now_x, r);
+
+
+ Y = -1;
+ }
+
+ }
+ //-------------ここまでif文
+
+ //モード分けをする
+ if(X == 1 && Y == 1){
+ mode = 1;
+ }
+ if(X == -1 && Y == 1){
+ mode = 2;
+ }
+ if(X == -1 && Y == -1){
+ mode = 3;
+ }
+ if(X == 1 && Y == -1){
+ mode = 4;
+ }
+
+ //---------------------------------------ここからモード番号によって処理が変わる
+
+ if(mode == 1){ //ゴールが第一象限にある場合
+ twe.printf("mode1\n");
+
+ //右に45度回転する
+ hanten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(M_time_45); //45度右回転
+ stop(R_Motor);
+ stop(L_Motor);
+
+ //距離が10m以上の時に、45度の方向で進む
+ while(r > 10){
+ //ずっと前進
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(1.0);
+
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r);
+ twe.printf("mode1,r>10,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r);
+ }
+
+ //ループを出たら、一旦止める
+ stop(R_Motor);
+ stop(L_Motor);
+
+ while(r > 4){ //4 < r < 10 [m]になったら、緯度と経度で差の大きい方を縮めるように動く
+ wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r);
+ twe.printf("mode1,r>4,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r);
+
+ delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む
+ delta_y = goal_y - now_y;
+ delta_x = abs(delta_x);
+ delta_y = abs(delta_y);
+
+ if(delta_x > delta_y){
+ //右に45度回転する
+ hanten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(M_time_45); //45度右回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+
+ else if(delta_y > delta_x){
+ //左に45度回転する
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_45); //45度左回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+ else{
+ //この場合は緯度と経度の差が等しいので、そのまま進んでOK
+ }
+ //条件に当てはまる限り進む
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(1.0);
+
+ }
+ stop(R_Motor);
+ stop(L_Motor); //止めて終わり
+ }
+
+
+ //------------------------------------------
+ if(mode == 2){ //ゴールが第二象限にある場合
+ twe.printf("mode2\n");
+
+ //左に45度回転する
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_45); //45度左回転
+ stop(R_Motor);
+ stop(L_Motor);
+
+ //距離が10m以上の時に、45度の方向で進む
+ while(r > 10){
+ //ずっと前進
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(1.0);
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r);
+ twe.printf("mode2,r>10,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r);
+
+ }
+
+ //ループを出たら、一旦止める
+ stop(R_Motor);
+ stop(L_Motor);
+
+ while(r > 4){ //4 < r < 10 [m]になったら、緯度と経度で差の大きい方を縮めるように動く
+ wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r);
+ twe.printf("mode2,r>4,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r);
+
+
+ delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む
+ delta_y = goal_y - now_y;
+ delta_x = abs(delta_x);
+ delta_y = abs(delta_y);
+
+ if(delta_x > delta_y){
+ //左に45度回転する
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_45); //45度左回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+
+ else if(delta_y > delta_x){
+ //右に45度回転する
+ hanten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(M_time_45); //45度右回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+ else{
+ //この場合は緯度と経度の差が等しいので、そのまま進んでOK
+ }
+ //条件に当てはまる限り進む
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(1.0);
+ }
+
+ stop(R_Motor);
+ stop(L_Motor); //止めて終わり
+ }
+
+
+
+ //------------------------------------------
+ if(mode == 3){ //ゴールが第三象限にある場合
+ twe.printf("mode3\n");
+
+ //右に45度回転する
+ hanten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(M_time_45); //45度右回転
+ stop(R_Motor);
+ stop(L_Motor);
+
+ //距離が10m以上の時に、45度の方向で進む
+ while(r > 10){
+ //ずっと前進
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(1.0);
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r);
+ twe.printf("mode3,r>10,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r);
+
+ }
+
+ //ループを出たら、一旦止める
+ stop(R_Motor);
+ stop(L_Motor);
+
+ while(r > 4){ //4 < r < 10 [m]になったら、緯度と経度で差の大きい方を縮めるように動く
+ wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r);
+ twe.printf("mode3,r>4,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r);
+
+
+ delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む
+ delta_y = goal_y - now_y;
+ delta_x = abs(delta_x);
+ delta_y = abs(delta_y);
+
+ if(delta_x > delta_y){
+ //右に45度回転する
+ hanten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(M_time_45); //45度右回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+
+ else if(delta_y > delta_x){
+ //左に45度回転する
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_45); //45度左回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+ else{
+ //この場合は緯度と経度の差が等しいので、そのまま進んでOK
+ }
+ //条件に当てはまる限り進む
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(1.0);
+ }
+
+ stop(R_Motor);
+ stop(L_Motor); //止めて終わり
+ }
+
+
+ //------------------------------------------
+ if(mode == 4){ //ゴールが第四象限にある場合
+ twe.printf("mode4\n");
+
+ //左に45度回転する
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_45); //45度左回転
+ stop(R_Motor);
+ stop(L_Motor);
+
+ //距離が10m以上の時に、45度の方向で進む
+ while(r > 10){
+ //ずっと前進
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(1.0);
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r);
+ twe.printf("mode4,r>10,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r);
+
+ }
+
+ //ループを出たら、一旦止める
+ stop(R_Motor);
+ stop(L_Motor);
+
+ while(r > 4){ //4 < r < 10 [m]になったら、緯度と経度で差の大きい方を縮めるように動く
+ wait(1.0); //GPSは1秒置きにデータを取得するため、データ更新時には1秒待つ
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ //地球を完全な球と仮定して、距離を求める[球面三角法](少しの誤差あり)、緯度経度は共にdegreeで与えられているので、三角関数に値を入れる為にラジアンに戻す
+ 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));
+ r = abs(r);
+ twe.printf("mode4,r>4,lat_now,%f,long_now,%f,r,%f\n",now_y, now_x, r);
+
+ delta_x = goal_x - now_x; //ゴールまでのx,y座標の差分を読み込む
+ delta_y = goal_y - now_y;
+ delta_x = abs(delta_x);
+ delta_y = abs(delta_y);
+
+ if(delta_x > delta_y){
+ //左に45度回転する
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ wait(M_time_45); //45度左回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+
+ else if(delta_y > delta_x){
+ //右に45度回転する
+ hanten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(M_time_45); //45度右回転
+ stop(R_Motor);
+ stop(L_Motor);
+ }
+ else{
+ //この場合は緯度と経度の差が等しいので、そのまま進んでOK
+ }
+ //条件に当てはまる限り進む
+ seiten(R_Motor, 1.0);
+ seiten(L_Motor, 1.0);
+ wait(1.0);
+ }
+
+ stop(R_Motor);
+ stop(L_Motor); //止めて終わり
+ }
+
+
+ twe.printf("UltraSound!!\n");
+
+ //ここから超音波
+ while(1){
+
+ for(i=0;i<24;i++){ //15dgree
+
+ stop(R_Motor);
+ stop(L_Motor);
+ wait_ms(3000);
+ usensor.start();
+ wait_ms(500);
+ d[i]=usensor.get_dist_cm();
+ twe.printf("%d cmA\r\n",d[i]);
+
+ //0問題解決用
+ if(d[i]<=1){
+ wait(5);
+ usensor.start();
+ wait_ms(500);
+ d[i]=usensor.get_dist_cm();
+ twe.printf("Re:%d cmA\r\n",d[i]);
+ }
+
+ if(d[i]<3){
+ pc.printf("END"); //3cm以下になったらEND
+ while(1){}} //無限ループで終わり
+
+ if(0<d[i]&&d[i]<10){break;} //0 < d < 10[cm]になったら抜ける
+
+ seiten(R_Motor, 1.0);
+ hanten(L_Motor, 1.0);
+ }
+
+ stop(R_Motor);
+ stop(L_Motor);
+ wait(3.6);
+ twe.printf("SEITEN");
+ seiten(R_Motor,1.0);
+ seiten(L_Motor,1.0);
+ wait(2.0);
+ now_y = michibiki.latitude; //GPSを取得(タイマー割り込みで取得される)緯度
+ now_x = michibiki.longtitude; //経度
+ twe.printf("Ultra,lat_now,%f,long_now,%f\n",now_y, now_x);
+
+ }
+
+
+}
+
\ No newline at end of file