Diff: Function.h

Revision:

0:00e63cfc4661

diff -r 000000000000 -r 00e63cfc4661 Function.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Function.h	Mon Dec 13 11:35:59 2021 +0000
@@ -0,0 +1,179 @@
+#include "mbed.h"
+#include "getGPS.h"
+#include "us015.h"
+#include "HMC6352.h"
+#include "TB6612.h"
+#include "ATP3011.h"
+
+US015 hs(D2, D3);
+DigitalOut Ultra(D2);
+GPS gps(D1, D0);
+HMC6352 compass(D4, D5);
+ATP3011 talk(D4,D5); // I2C sda scl
+TB6612 motor_a(D10,D6,D7);  //モータA制御用(pwma,ain1,ain2)
+TB6612 motor_b(D11,D8,D9);  //モータB制御用(pwmb,bin1,bin2)
+Serial xbee(A7, A2);
+
+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 theta;
+double delta;
+
+int FrontGet() 
+{
+        Ultra = 1;  //超音波on
+        hs.TrigerOut();
+        wait(1);
+        int distance;
+        distance = hs.GetDistance(); 
+        xbee.printf("distance=%d\r\n", distance);  //距離出力
+        Ultra=0;//超音波off
+        
+        if(distance < 1500) {
+            return 1;
+        }
+        else {
+            return 0;
+        }
+}
+
+
+void catchGPS()
+{
+    xbee.printf("GPS Start\r\n");
+    /* 1秒ごとに現在地を取得してターミナル出力 */
+    while(1) {
+        if(gps.getgps()) {  //現在地取得
+            xbee.printf("%lf %lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+            GPS_x = gps.latitude;
+            GPS_y = gps.longitude;
+            break;
+        }
+        else {
+            xbee.printf("No data\r\n");//データ取得に失敗した場合
+            wait(1);
+        }
+    }
+    return;
+}
+
+
+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;
+    }
+}
+
+double AngleGet() 
+{
+    double angle = 0;
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    angle = compass.sample() / 10;
+
+    double theta;
+    double delta;
+
+    xbee.printf("gps.latitude=%f, gps.longitude=%f\r\n", gps.latitude, gps.longitude);
+    theta = atan2(next_CP_y - gps.longitude , next_CP_x - gps.latitude) * 180 / 3.14159265359;
+    printf("theta=%f\r\n", theta);
+    if(theta >= 0) {
+        delta = angle - theta;
+    }
+    else {
+        theta = theta + 360;
+        delta = angle - theta;
+    }    
+    if(delta<0){
+        delta+=360;
+        }
+    printf("%f-%f=%f\r\n", angle, theta, delta); 
+    return delta;
+}
+
+void Calibration()
+{
+    xbee.printf("calibration start\r\n");
+    compass.setCalibrationMode(0x43);
+    Move('4', 0.2);
+    xbee.printf("mortor mode:4 speed:0.15\n\r");
+    wait(60);
+    Move('1', 0);
+    xbee.printf("mortor mode:1 speed:0\n\r");
+    compass.setCalibrationMode(0x45);
+    xbee.printf("calibration end\r\n");
+    while(1) {
+        if(gps.getgps()) {  //現在地取得
+        xbee.printf("lati=%lf\nlong=%lf\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+        GPS_x = gps.latitude;
+        GPS_y = gps.longitude;
+        break;
+        }
+        else {
+            xbee.printf("No data\r\n");//データ取得に失敗した場合
+            wait(1);
+        }
+    }
+
+    return;
+}
+
+ /*地上局から新情報を送るときはflag=がでてきたらスペースか.を入力
+ 3秒後ぐらいにmessage=が出てくるので、そしたら新情報を入力*/
+void speak()
+{
+    int timeout_ms=500;
+    char mess[100];            
+        if(talk.IsActive(timeout_ms)==true){
+            xbee.printf("Active\n\rflag=");
+            wait(3);
+            if(xbee.readable()){
+                xbee.printf("\n\rmessage=");
+                int i=0;
+                    do{
+                        mess[i++]= xbee.getc();
+                    }
+                    while(mess[i-1]!= 0x0d && i<99);
+                talk.Synthe(mess);
+            }
+            else{
+                xbee.printf("preset_message speak\r\n");
+                talk.Synthe("hinannyuudoudesu,,tuitekitekudasai.");
+            }
+        }
+        else{
+            xbee.printf("\r\nNot Active\n");
+        }
+}
\ No newline at end of file