CanSat-C 2021 / Mbed 2 deprecated CanSat-C_test

統合試験用です。

Dependencies:   ATP3012 mbed TB6612FNG HMC US015 getGPS

main.cpp@0:5a1b52164bbe, 2021-10-13 (annotated)

Committer:
ushiroji
Date:
Wed Oct 13 12:44:09 2021 +0000
Revision:
0:5a1b52164bbe
Child:
1:f6d4f374b130
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ushiroji 0:5a1b52164bbe 1 #include "mbed.h"
ushiroji 0:5a1b52164bbe 2 #include "TB6612.h"
ushiroji 0:5a1b52164bbe 3 #include "ATP3011.h"
ushiroji 0:5a1b52164bbe 4 #include "getGPS.h"
ushiroji 0:5a1b52164bbe 5 #include "HMC6352.h"
ushiroji 0:5a1b52164bbe 6
ushiroji 0:5a1b52164bbe 7 TB6612 motor_a(D10,D6,D7); //モータA制御用(pwma,ain1,ain2)
ushiroji 0:5a1b52164bbe 8 TB6612 motor_b(D11,D8,D9); //モータB制御用(pwmb,bin1,bin2)
ushiroji 0:5a1b52164bbe 9 Serial pc(USBTX,USBRX); //USBシリアル通信用
ushiroji 0:5a1b52164bbe 10 GPS gps(D1, D0);
ushiroji 0:5a1b52164bbe 11 HMC6352 compass(D4, D5);
ushiroji 0:5a1b52164bbe 12
ushiroji 0:5a1b52164bbe 13 int motor(char m) {
ushiroji 0:5a1b52164bbe 14 float motor_speed; //モータスピード情報格納用
ushiroji 0:5a1b52164bbe 15 while(1) {
ushiroji 0:5a1b52164bbe 16 m = pc.getc(); //キーボード入力情報取得
ushiroji 0:5a1b52164bbe 17 motor_speed=0.5; //モータスピード(低速運転させるため２分の１の値とする。)
ushiroji 0:5a1b52164bbe 18 switch(m)
ushiroji 0:5a1b52164bbe 19 {
ushiroji 0:5a1b52164bbe 20 case '1': motor_a=motor_speed; //モータA正転
ushiroji 0:5a1b52164bbe 21 break;
ushiroji 0:5a1b52164bbe 22 case '2': motor_a=0; //モータAブレーキ
ushiroji 0:5a1b52164bbe 23 break;
ushiroji 0:5a1b52164bbe 24 case '3': motor_a=-motor_speed; //モータA逆転
ushiroji 0:5a1b52164bbe 25 break;
ushiroji 0:5a1b52164bbe 26 case '7': motor_b=motor_speed; //モータB正転
ushiroji 0:5a1b52164bbe 27 break;
ushiroji 0:5a1b52164bbe 28 case '8': motor_b=0; //モータBブレーキ
ushiroji 0:5a1b52164bbe 29 break;
ushiroji 0:5a1b52164bbe 30 case '9': motor_b=-motor_speed; //モータB正転
ushiroji 0:5a1b52164bbe 31 break;
ushiroji 0:5a1b52164bbe 32 default : motor_a=0;
ushiroji 0:5a1b52164bbe 33 motor_b=0; //両方モータブレーキ
ushiroji 0:5a1b52164bbe 34 break;
ushiroji 0:5a1b52164bbe 35 }
ushiroji 0:5a1b52164bbe 36 }
ushiroji 0:5a1b52164bbe 37 }
ushiroji 0:5a1b52164bbe 38
ushiroji 0:5a1b52164bbe 39
ushiroji 0:5a1b52164bbe 40
ushiroji 0:5a1b52164bbe 41 int GPS()
ushiroji 0:5a1b52164bbe 42 {
ushiroji 0:5a1b52164bbe 43 /* 1秒ごとに現在地を取得してターミナル出力 */
ushiroji 0:5a1b52164bbe 44 while(1) {
ushiroji 0:5a1b52164bbe 45 if(gps.getgps()) //現在地取得
ushiroji 0:5a1b52164bbe 46 pc.printf("(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
ushiroji 0:5a1b52164bbe 47
ushiroji 0:5a1b52164bbe 48 else
ushiroji 0:5a1b52164bbe 49 pc.printf("No data\r\n");//データ取得に失敗した場合
ushiroji 0:5a1b52164bbe 50
ushiroji 0:5a1b52164bbe 51 wait(1);
ushiroji 0:5a1b52164bbe 52 }
ushiroji 0:5a1b52164bbe 53
ushiroji 0:5a1b52164bbe 54 return 0;
ushiroji 0:5a1b52164bbe 55 }
ushiroji 0:5a1b52164bbe 56
ushiroji 0:5a1b52164bbe 57 int compass()
ushiroji 0:5a1b52164bbe 58 {
ushiroji 0:5a1b52164bbe 59 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
ushiroji 0:5a1b52164bbe 60
ushiroji 0:5a1b52164bbe 61 }
ushiroji 0:5a1b52164bbe 62
ushiroji 0:5a1b52164bbe 63 int main() {
ushiroji 0:5a1b52164bbe 64 // 変数宣言
ushiroji 0:5a1b52164bbe 65 int CP_num; // CPリストのインデックス
ushiroji 0:5a1b52164bbe 66 int last_num; // CPリストの最後の要素のインデックス
ushiroji 0:5a1b52164bbe 67 double GPS_x, GPS_y; // 現在地の座標
ushiroji 0:5a1b52164bbe 68 double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
ushiroji 0:5a1b52164bbe 69 double direction; // 次CPへの向き
ushiroji 0:5a1b52164bbe 70 double CPs_x[] = []; // CPリスト(x座標)
ushiroji 0:5a1b52164bbe 71 double CPs_y[] = []; // CPリスト(y座標)
ushiroji 0:5a1b52164bbe 72 double next_CP_x, next_CP_y;
ushiroji 0:5a1b52164bbe 73
ushiroji 0:5a1b52164bbe 74 // 行動フロー開始
ushiroji 0:5a1b52164bbe 75 last_num = sizeof(CPs_x) / sizeof(double) - 1;
ushiroji 0:5a1b52164bbe 76 while (next_CP_x != CPs_x[last_num] && next_CP_y != CPs_y[last_num]) {
ushiroji 0:5a1b52164bbe 77 int i;
ushiroji 0:5a1b52164bbe 78 for (i = CP_num, last_num, i++) {
ushiroji 0:5a1b52164bbe 79 // 移動
ushiroji 0:5a1b52164bbe 80 位置情報を取得();
ushiroji 0:5a1b52164bbe 81 方向取得();
ushiroji 0:5a1b52164bbe 82 回転();
ushiroji 0:5a1b52164bbe 83 while(i >= 5) // 5°ずれると方向転換する
ushiroji 0:5a1b52164bbe 84 {
ushiroji 0:5a1b52164bbe 85
ushiroji 0:5a1b52164bbe 86
ushiroji 0:5a1b52164bbe 87 }
ushiroji 0:5a1b52164bbe 88 motor(1)
ushiroji 0:5a1b52164bbe 89 while (True) {
ushiroji 0:5a1b52164bbe 90 前方取得();
ushiroji 0:5a1b52164bbe 91 if (/* 障害物あり */) 停止()
ushiroji 0:5a1b52164bbe 92 回避行動();
ushiroji 0:5a1b52164bbe 93 else 移動();
ushiroji 0:5a1b52164bbe 94 位置情報を取得();
ushiroji 0:5a1b52164bbe 95 if (next_CP_x = GPS_x &&next_CP_y = GPS_y) blake;
ushiroji 0:5a1b52164bbe 96 }
ushiroji 0:5a1b52164bbe 97 }
ushiroji 0:5a1b52164bbe 98 // 行動フロー終了
ushiroji 0:5a1b52164bbe 99 }