Diff: Function.h

Revision:

0:85414455b681

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Function.h	Wed Oct 27 11:59:45 2021 +0000
@@ -0,0 +1,140 @@
+#include "mbed.h"
+
+#include "getGPS.h"
+#include "us015.h"
+#include "HMC6352.h"
+#include "Math.h"
+#include "TB6612.h"
+
+US015 hs(D2, D3);
+DigitalOut Ultra(D2);
+GPS gps(D1, D0);
+HMC6352 compass(D4, D5);
+TB6612 motor_a(D10,D6,D7);  //モータA制御用(pwma,ain1,ain2)
+TB6612 motor_b(D11,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
+Serial pc(USBTX, USBRX);
+
+double GPS_x, GPS_y;  // 現在地の座標
+double next_CP_x, next_CP_y;
+double CPs_x[100];  // = [];  //CPリスト(x座標)
+double CPs_y[100];  // = [];  // CPリスト(y座標)
+double *pGPS_x = &GPS_x, *pGPS_y = &GPS_y;
+double theta;
+double delta;
+
+int FrontGet() 
+{
+        Ultra = 1;  //超音波on
+        hs.TrigerOut();
+        wait(1);
+        int distance;
+        distance = hs.GetDistance();
+        pc.printf("distance=%d\r\n", distance);  //距離出力
+        Ultra=0;//超音波off
+        
+        if(distance < 200) {
+            return 1;
+        }
+        else {
+            return 0;
+        }
+}
+
+
+int catchGPS()
+{
+    pc.printf("GPS Start\r\n");
+    /* 1秒ごとに現在地を取得してターミナル出力 */
+    while(1) {
+        if(gps.getgps()) {  //現在地取得
+            pc.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+            *pGPS_x = gps.latitude;
+            *pGPS_y = gps.longitude;
+            break;
+        }
+        else {
+            pc.printf("No data\r\n");//データ取得に失敗した場合
+            wait(1);
+        }
+    }
+    return 0;
+}
+
+
+void Move(char input_data, float motor_speed) {
+    switch (input_data) {
+        case '1':  // 停止
+            motor_a = 0;
+            motor_b = 0;
+            break;
+        case '2':  // 前進
+            motor_a = motor_speed;
+            motor_b = motor_speed;
+            break;
+        case '3':  // 後退
+            motor_a = -motor_speed;
+            motor_b = -motor_speed;
+            break;
+        case '4':  // 時計回りに回転
+            motor_a = motor_speed;
+            motor_b = -motor_speed;
+            break;
+        case '5':  // 反時計回りに回転
+            motor_a = -motor_speed;
+            motor_b = motor_speed;
+            break;
+        case '6':  // Aのみ正転
+            motor_a = motor_speed;
+            break;
+        case '7':  // Bのみ正転
+            motor_b = motor_speed;
+            break;
+        case '8':  // Aのみ逆転
+            motor_a = -motor_speed;
+            break;
+        case '9':  // Bのみ逆転
+            motor_b = -motor_speed;
+            break;
+    }
+    pc.printf("input_data=%d\r\n", input_data);
+}
+
+
+void Rotate(double rot){
+   // double time = angle; // 試験で調整
+    int time = 3;  // 試験用
+    if(rot > 0) {
+        Move('5', 1);
+        wait(time);
+        Move('1', 0);  
+    }
+    else {
+        Move('6', 1);
+        wait(time);
+        Move('1', 0);
+    }
+}
+
+double AngleGet() 
+{
+    double angle;
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    angle = compass.sample() / 10;
+
+    double theta;
+    double delta;
+
+    pc.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
+    theta = atan2(next_CP_x - gps.latitude , next_CP_y - gps.longitude) * 180 / M_PI;
+    printf("theta=%f\r\n", theta);
+    if(theta >= 0) {
+        delta = angle - theta;
+    }
+    else {
+        theta = theta + 360;
+        delta = angle - theta;
+    }
+    printf("%f-%f=%f\r\n", angle, theta, delta); 
+    wait(2);
+    return delta;
+}