ループのテスト
Dependencies: ATP3012 mbed TB6612FNG HMC6352 US015 getGPS
main.cpp@18:057b39ca7bd1, 2021-10-28 (annotated)
- Committer:
- ushiroji
- Date:
- Thu Oct 28 09:57:35 2021 +0000
- Revision:
- 18:057b39ca7bd1
- Parent:
- 17:1b6a84ab4433
- Child:
- 19:fb9b9795ad9e
test
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
user_ | 6:1cda8471adc3 | 1 | // ライブラリ |
ushiroji | 0:5a1b52164bbe | 2 | #include "mbed.h" |
ushiroji | 14:1b5be519bd49 | 3 | |
user_ | 6:1cda8471adc3 | 4 | // 自作関数 |
ushiroji | 14:1b5be519bd49 | 5 | #include "Function.h" |
ushiroji | 15:4779723a4f75 | 6 | #include "speak.h" |
ushiroji | 0:5a1b52164bbe | 7 | |
user_ | 7:8fab045d2616 | 8 | // フライトピン・ニクロム線関係 |
user_ | 7:8fab045d2616 | 9 | DigitalIn flight_pin(A0); |
user_ | 7:8fab045d2616 | 10 | DigitalOut nichrome(D13); |
user_ | 8:5fe1b4bbd108 | 11 | // |
ushiroji | 17:1b6a84ab4433 | 12 | #define cp_max 3//cpの数を自分で入れてね |
ushiroji | 0:5a1b52164bbe | 13 | |
ushiroji | 0:5a1b52164bbe | 14 | int main() { |
ushiroji | 0:5a1b52164bbe | 15 | // 変数宣言 |
ushiroji | 17:1b6a84ab4433 | 16 | /*int CP_num; // CPリストのインデックス |
ushiroji | 17:1b6a84ab4433 | 17 | int last_num; // CPリストの最後の要素のインデックス*/ |
user_ | 6:1cda8471adc3 | 18 | double GPS_x, GPS_y; // 現在地の座標 |
user_ | 6:1cda8471adc3 | 19 | double direction; // 次CPへの向き |
ushiroji | 18:057b39ca7bd1 | 20 | double CPs_x[3]={1, 2, 3}; // = []; //CPリスト(x座標) |
ushiroji | 18:057b39ca7bd1 | 21 | double CPs_y[3]={1, 2, 3}; // = []; // CPリスト(y座標) |
user_ | 6:1cda8471adc3 | 22 | double next_CP_x, next_CP_y; |
user_ | 6:1cda8471adc3 | 23 | |
user_ | 6:1cda8471adc3 | 24 | // 落下検知 |
user_ | 7:8fab045d2616 | 25 | // パラシュート分離 |
ushiroji | 17:1b6a84ab4433 | 26 | /* |
ushiroji | 14:1b5be519bd49 | 27 | wait(3);//電源ついてから3v3が安定するまで、秒数は適当、必要かもわからん |
ushiroji | 14:1b5be519bd49 | 28 | while(flight_pin){ |
user_ | 7:8fab045d2616 | 29 | pc.printf("flight_pin nuketa"); |
user_ | 7:8fab045d2616 | 30 | wait(35);//ピン抜けてから地面につくまで70m/2.8(m/s)=25(s)余裕を見て+10s |
user_ | 7:8fab045d2616 | 31 | nichrome=1; |
user_ | 7:8fab045d2616 | 32 | pc.printf("nichrome in"); |
user_ | 7:8fab045d2616 | 33 | wait(30); |
user_ | 7:8fab045d2616 | 34 | nichrome=0; |
ushiroji | 14:1b5be519bd49 | 35 | } |
user_ | 7:8fab045d2616 | 36 | // 落下終了 |
ushiroji | 17:1b6a84ab4433 | 37 | */ |
user_ | 6:1cda8471adc3 | 38 | |
user_ | 7:8fab045d2616 | 39 | |
user_ | 6:1cda8471adc3 | 40 | // 行動フロー開始 |
ushiroji | 17:1b6a84ab4433 | 41 | //last_num = sizeof(CPs_x) / sizeof(double) - 1; // ゴール地点のインデックスを算出 |
ushiroji | 17:1b6a84ab4433 | 42 | while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定 |
ushiroji | 17:1b6a84ab4433 | 43 | for (int i = 0; i<=cp_max-1 ; i++) { |
user_ | 6:1cda8471adc3 | 44 | // 移動 |
ushiroji | 17:1b6a84ab4433 | 45 | next_CP_x = CPs_x[i]; |
ushiroji | 17:1b6a84ab4433 | 46 | next_CP_y = CPs_y[i]; |
ushiroji | 17:1b6a84ab4433 | 47 | |
ushiroji | 17:1b6a84ab4433 | 48 | pc.printf("i=%d\r\n", i); |
ushiroji | 18:057b39ca7bd1 | 49 | //catchGPS(); |
ushiroji | 18:057b39ca7bd1 | 50 | pc.printf("\n\rlatitude="); |
ushiroji | 18:057b39ca7bd1 | 51 | pc.scanf("%lf",&gps.latitude); |
ushiroji | 18:057b39ca7bd1 | 52 | pc.printf("\n\rlongititude="); |
ushiroji | 18:057b39ca7bd1 | 53 | pc.scanf("%lf",&gps.longitude); |
ushiroji | 17:1b6a84ab4433 | 54 | pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); |
ushiroji | 14:1b5be519bd49 | 55 | |
ushiroji | 14:1b5be519bd49 | 56 | while (1) { |
ushiroji | 15:4779723a4f75 | 57 | speak(); |
ushiroji | 17:1b6a84ab4433 | 58 | /* |
ushiroji | 14:1b5be519bd49 | 59 | direction = AngleGet(); |
ushiroji | 17:1b6a84ab4433 | 60 | pc.printf("direction=%f\n", direction); |
ushiroji | 16:b5a60a976bf7 | 61 | while(1) { |
ushiroji | 16:b5a60a976bf7 | 62 | if(direction < 5 || direction > 355) { //角度判定 |
ushiroji | 16:b5a60a976bf7 | 63 | Move('2', 1); |
ushiroji | 16:b5a60a976bf7 | 64 | break; |
ushiroji | 16:b5a60a976bf7 | 65 | } |
ushiroji | 16:b5a60a976bf7 | 66 | else { |
ushiroji | 16:b5a60a976bf7 | 67 | Move('1', 0); |
ushiroji | 16:b5a60a976bf7 | 68 | Move('4', 0.5); |
ushiroji | 16:b5a60a976bf7 | 69 | } |
ushiroji | 14:1b5be519bd49 | 70 | } |
ushiroji | 17:1b6a84ab4433 | 71 | */ |
user_ | 8:5fe1b4bbd108 | 72 | |
ushiroji | 14:1b5be519bd49 | 73 | if (FrontGet()) { |
ushiroji | 14:1b5be519bd49 | 74 | Move('1', 0); //停止 |
ushiroji | 14:1b5be519bd49 | 75 | Move('4', 0.5); //回転 |
ushiroji | 14:1b5be519bd49 | 76 | wait(1); |
ushiroji | 14:1b5be519bd49 | 77 | Move('1', 0); //回転停止 |
ushiroji | 14:1b5be519bd49 | 78 | continue; |
ushiroji | 14:1b5be519bd49 | 79 | } |
ushiroji | 14:1b5be519bd49 | 80 | else { |
ushiroji | 14:1b5be519bd49 | 81 | Move('2', 0.5); |
ushiroji | 14:1b5be519bd49 | 82 | } |
ushiroji | 18:057b39ca7bd1 | 83 | ///catchGPS(); |
ushiroji | 18:057b39ca7bd1 | 84 | pc.printf("\n\rlatitude="); |
ushiroji | 18:057b39ca7bd1 | 85 | pc.scanf("%lf",&gps.latitude); |
ushiroji | 18:057b39ca7bd1 | 86 | pc.printf("\n\rlongititude="); |
ushiroji | 18:057b39ca7bd1 | 87 | pc.scanf("%lf",&gps.longitude); |
ushiroji | 17:1b6a84ab4433 | 88 | pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude); |
ushiroji | 17:1b6a84ab4433 | 89 | if ((next_CP_x - GPS_x)*(next_CP_x - GPS_x) + (next_CP_y = GPS_y)*(next_CP_y = GPS_y) < 0.01) { // CP到着判定 //試験で調整 |
ushiroji | 17:1b6a84ab4433 | 90 | pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y); |
ushiroji | 14:1b5be519bd49 | 91 | break; |
ushiroji | 14:1b5be519bd49 | 92 | } |
ushiroji | 17:1b6a84ab4433 | 93 | |
user_ | 8:5fe1b4bbd108 | 94 | } |
ushiroji | 3:74d0faefdd78 | 95 | } |
user_ | 6:1cda8471adc3 | 96 | // 行動フロー終了 |
ushiroji | 17:1b6a84ab4433 | 97 | pc.printf("End\r\n"); |
user_ | 6:1cda8471adc3 | 98 | return 0; |
ushiroji | 14:1b5be519bd49 | 99 | } |
ushiroji | 5:56eddb7b4a9e | 100 | } |