CanSat-C 2021 / Mbed 2 deprecated CanSat-C_test

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   ATP3012 mbed TB6612FNG HMC US015 getGPS

main.cpp@18:42cbba4fb371, 2021-10-28 (annotated)

Committer:
ushiroji
Date:
Thu Oct 28 13:15:05 2021 +0000
Revision:
18:42cbba4fb371
Parent:
17:1b6a84ab4433
Child:
19:046524ac0985
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 18:42cbba4fb371 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:42cbba4fb371 20 double CPs_x[3]={34.545658, 34.545837, 34.546052}; //CPリスト(x座標)
ushiroji 18:42cbba4fb371 21 double CPs_y[3]={135.503469, 135.503676, 135.503503}; // 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 18:42cbba4fb371 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 // 落下終了
user_ 6:1cda8471adc3 37
user_ 7:8fab045d2616 38
user_ 6:1cda8471adc3 39 // 行動フロー開始
ushiroji 17:1b6a84ab4433 40 //last_num = sizeof(CPs_x) / sizeof(double) - 1; // ゴール地点のインデックスを算出
ushiroji 17:1b6a84ab4433 41 while (next_CP_x != CPs_x[cp_max-1] && next_CP_y != CPs_y[cp_max-1]) { // ゴール判定
ushiroji 17:1b6a84ab4433 42 for (int i = 0; i<=cp_max-1 ; i++) {
user_ 6:1cda8471adc3 43 // 移動
ushiroji 17:1b6a84ab4433 44 next_CP_x = CPs_x[i];
ushiroji 17:1b6a84ab4433 45 next_CP_y = CPs_y[i];
ushiroji 17:1b6a84ab4433 46
ushiroji 17:1b6a84ab4433 47 pc.printf("i=%d\r\n", i);
ushiroji 14:1b5be519bd49 48 catchGPS();
ushiroji 17:1b6a84ab4433 49 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
ushiroji 14:1b5be519bd49 50
ushiroji 14:1b5be519bd49 51 while (1) {
ushiroji 15:4779723a4f75 52 speak();
ushiroji 18:42cbba4fb371 53
ushiroji 14:1b5be519bd49 54 direction = AngleGet();
ushiroji 17:1b6a84ab4433 55 pc.printf("direction=%f\n", direction);
ushiroji 16:b5a60a976bf7 56 while(1) {
ushiroji 16:b5a60a976bf7 57 if(direction < 5 || direction > 355) { //角度判定
ushiroji 16:b5a60a976bf7 58 Move('2', 1);
ushiroji 16:b5a60a976bf7 59 break;
ushiroji 16:b5a60a976bf7 60 }
ushiroji 16:b5a60a976bf7 61 else {
ushiroji 16:b5a60a976bf7 62 Move('1', 0);
ushiroji 16:b5a60a976bf7 63 Move('4', 0.5);
ushiroji 16:b5a60a976bf7 64 }
ushiroji 14:1b5be519bd49 65 }
user_ 8:5fe1b4bbd108 66
ushiroji 14:1b5be519bd49 67 if (FrontGet()) {
ushiroji 14:1b5be519bd49 68 Move('1', 0); //停止
ushiroji 14:1b5be519bd49 69 Move('4', 0.5); //回転
ushiroji 14:1b5be519bd49 70 wait(1);
ushiroji 14:1b5be519bd49 71 Move('1', 0); //回転停止
ushiroji 14:1b5be519bd49 72 continue;
ushiroji 14:1b5be519bd49 73 }
ushiroji 14:1b5be519bd49 74 else {
ushiroji 14:1b5be519bd49 75 Move('2', 0.5);
ushiroji 14:1b5be519bd49 76 }
ushiroji 14:1b5be519bd49 77 catchGPS();
ushiroji 17:1b6a84ab4433 78 pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);
ushiroji 17:1b6a84ab4433 79 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 80 pc.printf("now leach cp[%d]=x_%f,y_%f",i,next_CP_x ,next_CP_y);
ushiroji 14:1b5be519bd49 81 break;
ushiroji 14:1b5be519bd49 82 }
ushiroji 17:1b6a84ab4433 83
user_ 8:5fe1b4bbd108 84 }
ushiroji 3:74d0faefdd78 85 }
user_ 6:1cda8471adc3 86 // 行動フロー終了
ushiroji 17:1b6a84ab4433 87 pc.printf("End\r\n");
ushiroji 18:42cbba4fb371 88 Move('1', 0); //停止
user_ 6:1cda8471adc3 89 return 0;
ushiroji 14:1b5be519bd49 90 }
ushiroji 5:56eddb7b4a9e 91 }