走行試験用のプログラムです。
Dependencies: mbed TB6612FNG HMC6352 US015 getGPS
main.cpp@0:0454d5b5905b, 2021-11-06 (annotated)
- Committer:
- ushiroji
- Date:
- Sat Nov 06 01:27:45 2021 +0000
- Revision:
- 0:0454d5b5905b
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ushiroji | 0:0454d5b5905b | 1 | #include "mbed.h" |
ushiroji | 0:0454d5b5905b | 2 | #include "Function.h" |
ushiroji | 0:0454d5b5905b | 3 | |
ushiroji | 0:0454d5b5905b | 4 | int main() |
ushiroji | 0:0454d5b5905b | 5 | { |
ushiroji | 0:0454d5b5905b | 6 | //直進 |
ushiroji | 0:0454d5b5905b | 7 | Move('2', 1); // 前進 |
ushiroji | 0:0454d5b5905b | 8 | wait(10); |
ushiroji | 0:0454d5b5905b | 9 | Move('1', 0); // 停止 |
ushiroji | 0:0454d5b5905b | 10 | |
ushiroji | 0:0454d5b5905b | 11 | //回転 |
ushiroji | 0:0454d5b5905b | 12 | Move('4', 0.5); // 時計回りに回転 |
ushiroji | 0:0454d5b5905b | 13 | wait(10); |
ushiroji | 0:0454d5b5905b | 14 | Move('1', 0); // 停止 |
ushiroji | 0:0454d5b5905b | 15 | |
ushiroji | 0:0454d5b5905b | 16 | //FrontGetの距離調整 |
ushiroji | 0:0454d5b5905b | 17 | while(1) { |
ushiroji | 0:0454d5b5905b | 18 | if (FrontGet()) { |
ushiroji | 0:0454d5b5905b | 19 | Move('1', 0); //停止 |
ushiroji | 0:0454d5b5905b | 20 | Move('4', 0.5); //回転 |
ushiroji | 0:0454d5b5905b | 21 | wait(1); |
ushiroji | 0:0454d5b5905b | 22 | Move('1', 0); //停止 |
ushiroji | 0:0454d5b5905b | 23 | } |
ushiroji | 0:0454d5b5905b | 24 | else { |
ushiroji | 0:0454d5b5905b | 25 | Move('2', 1); |
ushiroji | 0:0454d5b5905b | 26 | } |
ushiroji | 0:0454d5b5905b | 27 | } |
ushiroji | 0:0454d5b5905b | 28 | |
ushiroji | 0:0454d5b5905b | 29 | /* main |
ushiroji | 0:0454d5b5905b | 30 | double GPS_x, GPS_y; // 現在地の座標 |
ushiroji | 0:0454d5b5905b | 31 | double direction; // 次CPへの向き |
ushiroji | 0:0454d5b5905b | 32 | double CPs_x[3]={34.54608391847546, 34.545845666047306, 34.545666059919796}; //CPリスト(x座標) |
ushiroji | 0:0454d5b5905b | 33 | double CPs_y[3]={135.50338843400897, 135.50368659080985, 135.50347298593758}; // CPリスト(y座標) |
ushiroji | 0:0454d5b5905b | 34 | double next_CP_x, next_CP_y; |
ushiroji | 0:0454d5b5905b | 35 | int cp_max = sizeof(CPs_x); |
ushiroji | 0:0454d5b5905b | 36 | |
ushiroji | 0:0454d5b5905b | 37 | while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定 |
ushiroji | 0:0454d5b5905b | 38 | for (int i = 0; i<=cp_max-1 ; i++) { |
ushiroji | 0:0454d5b5905b | 39 | // 移動 |
ushiroji | 0:0454d5b5905b | 40 | next_CP_x = CPs_x[i]; |
ushiroji | 0:0454d5b5905b | 41 | next_CP_y = CPs_y[i]; |
ushiroji | 0:0454d5b5905b | 42 | |
ushiroji | 0:0454d5b5905b | 43 | pc.printf("i=%d\r\n", i); |
ushiroji | 0:0454d5b5905b | 44 | pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); |
ushiroji | 0:0454d5b5905b | 45 | |
ushiroji | 0:0454d5b5905b | 46 | while (1) { |
ushiroji | 0:0454d5b5905b | 47 | // 角度調整 |
ushiroji | 0:0454d5b5905b | 48 | direction = AngleGet(); |
ushiroji | 0:0454d5b5905b | 49 | pc.printf("direction=%f\n", direction); |
ushiroji | 0:0454d5b5905b | 50 | while(1) { |
ushiroji | 0:0454d5b5905b | 51 | if(direction < 5 || direction > 355) { //角度判定 |
ushiroji | 0:0454d5b5905b | 52 | Move('2', 1); |
ushiroji | 0:0454d5b5905b | 53 | break; |
ushiroji | 0:0454d5b5905b | 54 | } |
ushiroji | 0:0454d5b5905b | 55 | else { |
ushiroji | 0:0454d5b5905b | 56 | Move('1', 0); |
ushiroji | 0:0454d5b5905b | 57 | Move('4', 0.5); |
ushiroji | 0:0454d5b5905b | 58 | } |
ushiroji | 0:0454d5b5905b | 59 | } |
ushiroji | 0:0454d5b5905b | 60 | |
ushiroji | 0:0454d5b5905b | 61 | //障害物判定 |
ushiroji | 0:0454d5b5905b | 62 | if (FrontGet()) { |
ushiroji | 0:0454d5b5905b | 63 | Move('1', 0); //停止 |
ushiroji | 0:0454d5b5905b | 64 | Move('4', 0.5); //回転 |
ushiroji | 0:0454d5b5905b | 65 | wait(1); |
ushiroji | 0:0454d5b5905b | 66 | Move('1', 0); //回転停止 |
ushiroji | 0:0454d5b5905b | 67 | continue; |
ushiroji | 0:0454d5b5905b | 68 | } |
ushiroji | 0:0454d5b5905b | 69 | else { |
ushiroji | 0:0454d5b5905b | 70 | Move('2', 0.5); |
ushiroji | 0:0454d5b5905b | 71 | } |
ushiroji | 0:0454d5b5905b | 72 | |
ushiroji | 0:0454d5b5905b | 73 | //到着判定 |
ushiroji | 0:0454d5b5905b | 74 | catchGPS(); |
ushiroji | 0:0454d5b5905b | 75 | pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); |
ushiroji | 0:0454d5b5905b | 76 | |
ushiroji | 0:0454d5b5905b | 77 | double lati = 111132.8715; //1度あたりの緯度の距離(m) |
ushiroji | 0:0454d5b5905b | 78 | double longi = 91535.79099; //1度あたりの経度の距離(m) |
ushiroji | 0:0454d5b5905b | 79 | 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到着判定 //試験で調整 |
ushiroji | 0:0454d5b5905b | 80 | pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y); |
ushiroji | 0:0454d5b5905b | 81 | break; |
ushiroji | 0:0454d5b5905b | 82 | } |
ushiroji | 0:0454d5b5905b | 83 | } |
ushiroji | 0:0454d5b5905b | 84 | } |
ushiroji | 0:0454d5b5905b | 85 | |
ushiroji | 0:0454d5b5905b | 86 | // 行動フロー終了 |
ushiroji | 0:0454d5b5905b | 87 | pc.printf("End\r\n"); |
ushiroji | 0:0454d5b5905b | 88 | Move('1', 0); //停止 |
ushiroji | 0:0454d5b5905b | 89 | return 0; |
ushiroji | 0:0454d5b5905b | 90 | } |
ushiroji | 0:0454d5b5905b | 91 | */ |
ushiroji | 0:0454d5b5905b | 92 | } |