CanSat-C 2021 / Mbed 2 deprecated loop_test

ループのテスト

Dependencies:   ATP3012 mbed TB6612FNG HMC6352 US015 getGPS

main.cpp@19:fb9b9795ad9e, 2021-10-28 (annotated)

Committer:
ushiroji
Date:
Thu Oct 28 10:30:13 2021 +0000
Revision:
19:fb9b9795ad9e
Parent:
18:057b39ca7bd1 
test

Who changed what in which revision?

UserRevisionLine numberNew 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 19:fb9b9795ad9e 89 if ((next_CP_x - gps.latitude)*(next_CP_x - gps.latitude) + (next_CP_y - gps.longitude)*(next_CP_y -gps.longitude) < 0.01) { // CP到着判定 //試験で調整
ushiroji 19:fb9b9795ad9e 90 pc.printf("now leach cp[%d]=x_%f,y_%f\r\n",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");
ushiroji 19:fb9b9795ad9e 98 Move('1', 0); //停止
user_ 6:1cda8471adc3 99 return 0;
ushiroji 14:1b5be519bd49 100 }
ushiroji 5:56eddb7b4a9e 101 }