Diff: direction.h

Revision:

0:e7b7def631c2

Child:

1:bb89b58cfa0e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/direction.h	Thu Oct 21 01:58:35 2021 +0000
@@ -0,0 +1,103 @@
+#include "mbed.h"
+#include "getGPS.h"
+#include "MPU9250.h"
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+GPS gps(D1, D0);
+
+float calculation_Θ(float x_0,float y_0,float x_1,float y_1,float x,float y){
+    //x,yは地磁気センサの値で北の方角の角度を90度、東の方角を0度とする。
+    float Θ_0 = atan((y_0 - y_1)/(x_0 - x_1));  //目的地の角度
+    
+    if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
+        Θ_0 = Θ_0 - 180;
+    }
+    if(y_0 - y_1 < 0 && x_0 - x_1 < 0){
+        Θ_0 = Θ_0 + 180;
+    }
+    
+    float Omag_x,Omag_y; //地磁気センサのxy平面が描く円の中点
+    float Θ_1 = atan((y - Omag_y)/(x- Omag_x));　//CanSatの角度 
+    
+    if(y - Omag_y > 0 && x- Omag_x < 0){
+        Θ_1 = Θ_1 - 180;
+    }
+    if(y - Omag_y < 0 && x - Omag_x < 0){
+        Θ_1 = Θ_1 + 180;
+    }
+    
+    Θ = Θ_0 - Θ_1;  //CanSatから見た目的地の角度
+}    
+
+class direction()
+{   
+    public:
+    
+    Serial pc(SERIAL_TX, SERIAL_RX);
+    GPS gps(D1, D0);
+    
+    float x_0 ,y_0; //目的地    
+    
+    MPU9250 mpu9250(D4, D5);                   // SDA, SCL
+
+    Serial pc(USBTX, USBRX, 115200);
+    
+    volatile bool interrupt_flag = false;
+
+    void detect_shock(void)
+    {
+        interrupt_flag = true;
+    }
+    
+    while(1) {
+        if(gps.getgps()) //現在地取得
+            pc.printf("(%lf, %lf)\r\n", gps.latitude, gps.longitude);//緯度と経度を出力
+        
+        else
+            pc.printf("No data\r\n");//データ取得に失敗した場合
+        
+        wait(1);
+    }
+
+
+
+    mpu9250.resetMPU9250();
+    mpu9250.setWakeOnMotion();
+
+    while(1)
+    {
+        if (interrupt_flag)
+        {
+            
+            pc.printf("detect\r\n");
+            wait_ms(1000);
+            interrupt_flag = false;
+        }
+
+        /* get mag data*/
+        int16_t mag[3];
+        mpu9250.readMagData(mag);
+        float mag_f[3];
+        for (int i = 0; i < 3; ++i)
+            mag_f[i] = float(mag[i]) * mpu9250.getMres();
+            
+        
+
+
+        
+        
+
+        /* serial */
+        
+        pc.printf("mx:%3.3f\t", mag_f[0]);
+        pc.printf("my:%3.3f\t", mag_f[1]);
+        pc.printf("mz:%3.3f\t\n", mag_f[2]);
+     
+               
+        pc.printf("\n\n---------------------------\r\n");
+
+        wait_ms(100);
+    }
+}
+
+ 
\ No newline at end of file