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
- Committer:
- hirokikanda
- Date:
- 2022-02-14
- Revision:
- 1:038eca93a8f4
- Parent:
- 0:09287ca178ff
File content as of revision 1:038eca93a8f4:
#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; //ゴールの位置によってモードを変える
//float temp1,pre1,h1;
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() {
//temp1= sensor.getTemperature();//電源を入れた時の標高を求める
//pre1=sensor.getPressure();
//h1=( pow((pre1/1013.25),0.1902225f))*(temp1+273.15f)/(0.065f);
/*
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 = atan2(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);
}
}