Diff: main.cpp

Revision:

0:5a1b52164bbe

Child:

1:f6d4f374b130

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 13 12:44:09 2021 +0000
@@ -0,0 +1,99 @@
+#include "mbed.h"
+#include "TB6612.h"
+#include "ATP3011.h" 
+#include "getGPS.h"
+#include "HMC6352.h"
+
+TB6612      motor_a(D10,D6,D7);  //モータA制御用(pwma,ain1,ain2)
+TB6612      motor_b(D11,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
+Serial      pc(USBTX,USBRX);    //USBシリアル通信用
+GPS gps(D1, D0);
+HMC6352 compass(D4, D5);
+
+int motor(char m) {
+    float   motor_speed;        //モータスピード情報格納用
+        while(1) {
+        m = pc.getc();   //キーボード入力情報取得
+        motor_speed=0.5; //モータスピード(低速運転させるため２分の１の値とする。)
+        switch(m)
+        {
+            case    '1':    motor_a=motor_speed;    //モータA正転
+                            break;    
+            case    '2':    motor_a=0;              //モータAブレーキ
+                            break;
+            case    '3':    motor_a=-motor_speed;   //モータA逆転
+                            break;                                        
+            case    '7':    motor_b=motor_speed;    //モータB正転
+                            break;    
+            case    '8':    motor_b=0;              //モータBブレーキ
+                            break;
+            case    '9':    motor_b=-motor_speed;   //モータB正転
+                            break;              
+            default    :    motor_a=0;
+                            motor_b=0;              //両方モータブレーキ
+                            break;        
+        }
+    }
+}
+
+
+
+int GPS()
+{
+    /* 1秒ごとに現在地を取得してターミナル出力 */
+    while(1) {
+        if(gps.getgps()) //現在地取得
+            pc.printf("(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+        
+        else
+            pc.printf("No data\r\n");//データ取得に失敗した場合
+        
+        wait(1);
+    }
+
+    return 0;
+}
+
+int compass()
+{
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    
+}
+
+int main() {
+    // 変数宣言
+    int CP_num;           // CPリストのインデックス
+int last_num;         // CPリストの最後の要素のインデックス
+double GPS_x, GPS_y;  // 現在地の座標
+double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
+double direction;     // 次CPへの向き
+double CPs_x[] = [];  // CPリスト(x座標)
+double CPs_y[] = [];  // CPリスト(y座標)
+double next_CP_x, next_CP_y;
+
+// 行動フロー開始
+last_num = sizeof(CPs_x) / sizeof(double) - 1;
+while (next_CP_x != CPs_x[last_num] && next_CP_y != CPs_y[last_num]) {
+    int i;
+    for (i = CP_num, last_num, i++) {
+    // 移動
+    位置情報を取得();
+    方向取得();
+    回転();
+    while(i >= 5)   // 5°ずれると方向転換する
+    {
+        
+        
+    }
+    motor(1)
+    while (True) {
+        前方取得();
+        if (/* 障害物あり */) 停止()
+        回避行動();
+        else 移動();
+        位置情報を取得();
+        if (next_CP_x = GPS_x &&next_CP_y = GPS_y) blake;
+            }
+        }
+        // 行動フロー終了
+    }