走行試験用のプログラムです。
Dependencies: mbed TB6612FNG HMC6352 US015 getGPS
main.cpp
- Committer:
- ushiroji
- Date:
- 2021-11-06
- Revision:
- 0:0454d5b5905b
File content as of revision 0:0454d5b5905b:
#include "mbed.h" #include "Function.h" int main() { //直進 Move('2', 1); // 前進 wait(10); Move('1', 0); // 停止 //回転 Move('4', 0.5); // 時計回りに回転 wait(10); Move('1', 0); // 停止 //FrontGetの距離調整 while(1) { if (FrontGet()) { Move('1', 0); //停止 Move('4', 0.5); //回転 wait(1); Move('1', 0); //停止 } else { Move('2', 1); } } /* main double GPS_x, GPS_y; // 現在地の座標 double direction; // 次CPへの向き double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796}; //CPリスト(x座標) double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758}; // CPリスト(y座標) double next_CP_x, next_CP_y; int cp_max = sizeof(CPs_x); while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定 for (int i = 0; i<=cp_max-1 ; i++) { // 移動 next_CP_x = CPs_x[i]; next_CP_y = CPs_y[i]; pc.printf("i=%d\r\n", i); pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); while (1) { // 角度調整 direction = AngleGet(); pc.printf("direction=%f\n", direction); while(1) { if(direction < 5 || direction > 355) { //角度判定 Move('2', 1); break; } else { Move('1', 0); Move('4', 0.5); } } //障害物判定 if (FrontGet()) { Move('1', 0); //停止 Move('4', 0.5); //回転 wait(1); Move('1', 0); //回転停止 continue; } else { Move('2', 0.5); } //到着判定 catchGPS(); pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); double lati = 111132.8715; //1度あたりの緯度の距離(m) double longi = 91535.79099; //1度あたりの経度の距離(m) if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x)*lati*lati + (next_CP_y = GPS_y)*(next_CP_y = GPS_y)*longi*longi < 100) { // CP到着判定 //試験で調整 pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y); break; } } } // 行動フロー終了 pc.printf("End\r\n"); Move('1', 0); //停止 return 0; } */ }