移動系の統合試験用プログラムです。
Dependencies: mbed TB6612FNG HMC6352 getGPS
Function.h@5:9f9cf3912fda, 2021-10-27 (annotated)
- Committer:
- ushiroji
- Date:
- Wed Oct 27 12:26:48 2021 +0000
- Revision:
- 5:9f9cf3912fda
- Parent:
- 4:135619de1646
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ushiroji | 2:0c73afb20925 | 1 | #include "mbed.h" |
ushiroji | 2:0c73afb20925 | 2 | #include "getGPS.h" |
ushiroji | 2:0c73afb20925 | 3 | #include "us015.h" |
ushiroji | 2:0c73afb20925 | 4 | #include "HMC6352.h" |
ushiroji | 2:0c73afb20925 | 5 | #include "Math.h" |
ushiroji | 2:0c73afb20925 | 6 | |
ushiroji | 2:0c73afb20925 | 7 | US015 hs(D2, D3); |
ushiroji | 2:0c73afb20925 | 8 | DigitalOut Ultra(D2); |
ushiroji | 2:0c73afb20925 | 9 | GPS gps(D1, D0); |
ushiroji | 2:0c73afb20925 | 10 | HMC6352 compass(D4, D5); |
ushiroji | 2:0c73afb20925 | 11 | Serial pc(USBTX, USBRX); |
ushiroji | 2:0c73afb20925 | 12 | double GPS_x, GPS_y; // 現在地の座標 |
ushiroji | 2:0c73afb20925 | 13 | double next_CP_x, next_CP_y; |
ushiroji | 2:0c73afb20925 | 14 | double CPs_x[100]; // = []; //CPリスト(x座標) |
ushiroji | 2:0c73afb20925 | 15 | double CPs_y[100]; // = []; // CPリスト(y座標) |
ushiroji | 2:0c73afb20925 | 16 | double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y; |
ushiroji | 4:135619de1646 | 17 | double theta; |
ushiroji | 4:135619de1646 | 18 | double delta; |
ushiroji | 2:0c73afb20925 | 19 | |
ushiroji | 2:0c73afb20925 | 20 | int FrontGet() |
ushiroji | 2:0c73afb20925 | 21 | { |
ushiroji | 2:0c73afb20925 | 22 | Ultra = 1; //超音波on |
ushiroji | 2:0c73afb20925 | 23 | hs.TrigerOut(); |
ushiroji | 2:0c73afb20925 | 24 | wait(1); |
ushiroji | 2:0c73afb20925 | 25 | int distance; |
ushiroji | 2:0c73afb20925 | 26 | distance = hs.GetDistance(); |
ushiroji | 2:0c73afb20925 | 27 | pc.printf("distance=%d\r\n", distance); //距離出力 |
ushiroji | 2:0c73afb20925 | 28 | Ultra=0;//超音波off |
ushiroji | 2:0c73afb20925 | 29 | |
ushiroji | 2:0c73afb20925 | 30 | if(distance < 200) { |
ushiroji | 2:0c73afb20925 | 31 | return 1; |
ushiroji | 2:0c73afb20925 | 32 | } |
ushiroji | 2:0c73afb20925 | 33 | else { |
ushiroji | 2:0c73afb20925 | 34 | return 0; |
ushiroji | 2:0c73afb20925 | 35 | } |
ushiroji | 2:0c73afb20925 | 36 | } |
ushiroji | 2:0c73afb20925 | 37 | |
ushiroji | 2:0c73afb20925 | 38 | int catchGPS() |
ushiroji | 2:0c73afb20925 | 39 | { |
ushiroji | 4:135619de1646 | 40 | pc.printf("GPS Start\r\n"); |
ushiroji | 2:0c73afb20925 | 41 | /* 1秒ごとに現在地を取得してターミナル出力 */ |
ushiroji | 2:0c73afb20925 | 42 | while(1) { |
ushiroji | 2:0c73afb20925 | 43 | if(gps.getgps()) { //現在地取得 |
ushiroji | 2:0c73afb20925 | 44 | pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力 |
ushiroji | 2:0c73afb20925 | 45 | *pGPS_x = gps.latitude; |
ushiroji | 2:0c73afb20925 | 46 | *pGPS_y = gps.longitude; |
ushiroji | 2:0c73afb20925 | 47 | break; |
ushiroji | 2:0c73afb20925 | 48 | } |
ushiroji | 2:0c73afb20925 | 49 | else { |
ushiroji | 4:135619de1646 | 50 | pc.printf("No data\r\n");//データ取得に失敗した場合 |
ushiroji | 2:0c73afb20925 | 51 | wait(1); |
ushiroji | 2:0c73afb20925 | 52 | } |
ushiroji | 2:0c73afb20925 | 53 | } |
ushiroji | 2:0c73afb20925 | 54 | return 0; |
ushiroji | 2:0c73afb20925 | 55 | } |
ushiroji | 2:0c73afb20925 | 56 | |
ushiroji | 2:0c73afb20925 | 57 | |
ushiroji | 4:135619de1646 | 58 | double AngleGet() |
ushiroji | 2:0c73afb20925 | 59 | { |
ushiroji | 2:0c73afb20925 | 60 | double angle; |
ushiroji | 2:0c73afb20925 | 61 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
ushiroji | 2:0c73afb20925 | 62 | angle = compass.sample() / 10; |
ushiroji | 2:0c73afb20925 | 63 | |
ushiroji | 4:135619de1646 | 64 | double next_x; |
ushiroji | 4:135619de1646 | 65 | double next_y; |
ushiroji | 4:135619de1646 | 66 | double theta; |
ushiroji | 4:135619de1646 | 67 | double delta; |
ushiroji | 4:135619de1646 | 68 | //if(gps.getgps()) { |
ushiroji | 4:135619de1646 | 69 | pc.printf("%f, %f\r\n", 0, 0); //gps.latitude=0, gps.longitude=0 |
ushiroji | 4:135619de1646 | 70 | next_x = 0; |
ushiroji | 4:135619de1646 | 71 | next_y = 100; |
ushiroji | 4:135619de1646 | 72 | theta = atan(next_x - 0 / next_y - 0) * 180 / M_PI; |
ushiroji | 4:135619de1646 | 73 | printf("theta=%f\r\n", theta); |
ushiroji | 4:135619de1646 | 74 | if(theta >= 0) { |
ushiroji | 4:135619de1646 | 75 | delta = angle - theta; |
ushiroji | 4:135619de1646 | 76 | } |
ushiroji | 4:135619de1646 | 77 | else { |
ushiroji | 4:135619de1646 | 78 | theta = theta + 360; |
ushiroji | 4:135619de1646 | 79 | delta = angle - theta; |
ushiroji | 4:135619de1646 | 80 | } |
ushiroji | 4:135619de1646 | 81 | printf("%f-%f=%f\r\n", angle, theta, delta); |
ushiroji | 4:135619de1646 | 82 | //} |
ushiroji | 4:135619de1646 | 83 | /* |
ushiroji | 4:135619de1646 | 84 | else { |
ushiroji | 4:135619de1646 | 85 | pc.printf("No data\r\n");//データ取得に失敗した場合 |
ushiroji | 4:135619de1646 | 86 | wait(1); |
ushiroji | 4:135619de1646 | 87 | } |
ushiroji | 4:135619de1646 | 88 | */ |
ushiroji | 2:0c73afb20925 | 89 | wait(2); |
ushiroji | 4:135619de1646 | 90 | return delta; |
ushiroji | 4:135619de1646 | 91 | } |
ushiroji | 4:135619de1646 | 92 | /* |
ushiroji | 4:135619de1646 | 93 | double angle; |
ushiroji | 4:135619de1646 | 94 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
ushiroji | 4:135619de1646 | 95 | angle = compass.sample() / 10; |
ushiroji | 4:135619de1646 | 96 | |
ushiroji | 4:135619de1646 | 97 | if(gps.getgps()) { |
ushiroji | 4:135619de1646 | 98 | pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); |
ushiroji | 4:135619de1646 | 99 | next_CP_x = gps.latitude; |
ushiroji | 4:135619de1646 | 100 | next_CP_y = gps.longitude; |
ushiroji | 4:135619de1646 | 101 | theta = atan(next_CP_x - 0 / next_CP_y - 0) * 180 / M_PI; |
ushiroji | 4:135619de1646 | 102 | printf("%f", theta); |
ushiroji | 4:135619de1646 | 103 | delta = theta - angle; |
ushiroji | 4:135619de1646 | 104 | printf("%f-%f=%f\r\n", theta, angle, delta); |
ushiroji | 4:135619de1646 | 105 | return delta; |
ushiroji | 2:0c73afb20925 | 106 | } |
ushiroji | 4:135619de1646 | 107 | else { |
ushiroji | 4:135619de1646 | 108 | pc.printf("No data\r\n");//データ取得に失敗した場合 |
ushiroji | 4:135619de1646 | 109 | wait(1); |
ushiroji | 4:135619de1646 | 110 | } |
ushiroji | 4:135619de1646 | 111 | } |
ushiroji | 4:135619de1646 | 112 | */ |