Revision 8:7209c810309d, committed 2021-11-01

Comitter:

tsubasa_nakajima

Date:

Mon Nov 01 16:52:17 2021 +0000

Parent:

7:74994694ec04

Commit message:

completed

Changed in this revision

--- a/BMP180.h	Thu Oct 28 14:44:25 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,156 +0,0 @@
-/*
-  @file BMP180.h
-  
-  @brief Barometric Pressure and Temperature Sensor BMP180 Breakout I2C Library      
-
-  @Author spiridion (http://mailme.spiridion.net)
-
-  Tested on LPC1768 and FRDM-KL25Z
-  
-  Copyright (c) 2014 spiridion
-  Released under the MIT License (see http://mbed.org/license/mit)
-
-  Documentation regarding the BMP180 can be found here: 
-  http://mbed.org/media/uploads/spiridion/bst-bmp180-ds000-09.pdf
-*/
-
-#ifndef BMP180_H
-#define BMP180_H
-
-#include "mbed.h"
-
-///  default address is 0xEF 
-#define BMP180_I2C_ADDRESS 0xEF 
-
-// Oversampling settings
-#define BMP180_OSS_ULTRA_LOW_POWER 0        // 1 sample  and  4.5ms for conversion
-#define BMP180_OSS_NORMAL          1        // 2 samples and  7.5ms for conversion
-#define BMP180_OSS_HIGH_RESOLUTION 2        // 4 samples and 13.5ms for conversion
-#define BMP180_OSS_ULTRA_HIGH_RESOLUTION 3  // 8 samples and 25.5ms for conversion
-
-#define UNSET_BMP180_PRESSURE_VALUE 0.F
-#define UNSET_BMP180_TEMPERATURE_VALUE -273.15F // absolute zero
-
-/** BMP180 class.
- *  Read Pressure and temperature from the BMP180 Breakout I2C sensor
- *
- * Example:
- * @code
- * #include "mbed.h"
- * #include "BMP180.h"
- * 
- * #if defined(TARGET_LPC1768)
- *     #define PIN_SDA p9
- *     #define PIN_SCL p10
- * #elif defined(TARGET_KL25Z) // watch out for the PTE0/PTE1 mixed up in the KL25Z doc 
- *     #define PIN_SDA PTE0
- *     #define PIN_SCL PTE1
- * #endif
- * 
- * int main() 
- * {    
- *     BMP180 bmp180(PIN_SDA, PIN_SCL);
- *     float pressure, temperature;
- *     
- *     // bmp180.Initialize(); // no altitude compensation and normal oversampling 
- *     bmp180.Initialize(64, BMP180_OSS_ULTRA_LOW_POWER); // 64m altitude compensation and low power oversampling
- *     
- *     while(1) 
- *     {        
- *         if (bmp180.ReadData(&pressure, &temperature))
- *             printf("Pressure(hPa): %8.2f \t Temperature(C): %8.2f\n", pressure, temperature);  
- *         wait(1);
- *     }
- * }
- * @endcode
- */
-class BMP180 
-{
-
-public:
-
-    /** Create a BMP180 instance
-     * @param sda pin 
-     * @param scl pin 
-     * @param address: I2C slave address 
-     */
-    BMP180(PinName sda, PinName scl, int address = BMP180_I2C_ADDRESS); 
-
-    /** Create a BMP180 instance
-     * @param i2c object
-     * @param address: I2C slave address 
-     */
-    BMP180(I2C& i2c, int address = BMP180_I2C_ADDRESS); 
-
-    /** Initialization: set member values and read BMP180 calibration parameters
-     * @param altitude (in meter)
-     * @param overSamplingSetting 
-     */
-    int Initialize(float altitude = 0.F, int overSamplingSetting = BMP180_OSS_NORMAL);
-
-    /** Read pressure and temperature from the BMP180.
-     * @param pressure (hPa) 
-     * @param temperature (C) 
-     * @returns
-     *   1 on success,
-     *   0 on error
-     */    
-    int ReadData(float* pTemperature = NULL, float* pPressure = NULL);
-
-    /** Get temperature from a previous measurement 
-     *  
-     * @returns
-     *   temperature (C)
-     */    
-    float GetTemperature() {return m_temperature;};
-
-     /** Get pressure from a previous measurement 
-     *  
-     * @returns
-     *   pressure (hPa)
-     */    
-   float GetPressure() {return m_pressure;};
-
-protected:
-
-    /** Perform temperature measurement
-     *  
-     * @returns
-     *   temperature (C)
-     */    
-    int ReadRawTemperature(long* pUt);
-
-    /** Perform pressure measurement 
-     *  
-     * @returns
-     *   temperature (C)
-     */    
-    int ReadRawPressure(long* pUp);
-
-    /** Calculation of the temperature from the digital output
-     */    
-    float TrueTemperature(long ut);
-
-    /** Calculation of the pressure from the digital output
-     */    
-    float TruePressure(long up);
-
-    int m_oss;
-    float m_temperature;     
-    float m_pressure;
-    float m_altitude;
-
-    I2C m_i2c;   
-    int m_addr;
-    char m_data[4];    
-
-    short ac1, ac2, ac3; 
-    unsigned short ac4, ac5, ac6;
-    short b1, b2;
-    short mb, mc, md;
-    long x1, x2, x3, b3, b5, b6;
-    unsigned long b4, b7;
-
-};
-
-#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP180.lib	Mon Nov 01 16:52:17 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/spiridion/code/BMP180/#072073c79cfd

--- a/Landing_Judgement.h	Thu Oct 28 14:44:25 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,81 +0,0 @@
-#include "mbed.h"
-#include "BMP180.h"
-#include "calculate.h"
-#define PIN_SDA D4
-#define PIN_SCL D5
-
-class Landing_Judgement:public BMP180{
-    private:
-        
-    int x ,y ,n1 , n2 ;
-    int landing_judgement ;
-    float h,dp,dt,dp0;
-    float a ,b ,dp_ave,dt_ave;
-    //hは高度、dpは気圧、dtは温度、dp0は海面気圧
-    
-    public:
-    /*ここの関数をlanding_judgementの中に入れ込んじゃいます
-    //landing()が1を返せば着地、0を返せば未着地
-    int landing(){
-        return landing_judgement;
-        }
-    */
-    
-    //着地判定の計算を開始させる関数    
-    int land(){
-    
-    BMP180 bmp180(PIN_SDA,PIN_SCL); 
-    landing_judgement = 0;
-    x = 0;
-    y = 0;
-    n1 = 0;
-    n2 = 0;
-    a = 0;
-    b = 0;
-    
-    for(int i=0;i<15;i++){
-        if(bmp180.ReadData(&dt,&dp)){
-            a = a + dp;
-            b = b + dt;
-            n1 = n1 + 1;
-            n2 = n2 + 1;
-            wait(1);
-            }
-        }
-    
-    dp_ave = a / n1;
-    dt_ave = b / n2;    
-    dp0 = calculate_dp0(dp_ave,dt_ave);
-    
-    while(x<10){
-            if(bmp180.ReadData(&dt,&dp)){
-                h = calculate_h(dp0,dp,dt);
-                if(h >= 30){
-                x = x + 1;
-                    }
-                wait(1);
-            }
-    }
-    //10秒以上高度30mにいた場合離陸判定
-    
-    wait(10);
-      
-    while(y<10){
-            if(bmp180.ReadData(&dt,&dp)){
-                h = calculate_h(dp0,dp,dt); 
-                if(h <= 10){
-                y = y + 1;
-                    }
-                wait(1);
-            }
-    }
-    
-    wait(5);
-    
-    landing_judgement = landing_judgement + 1;
-    return landing_judgement;
-    
-    //離陸判定後、10秒以上高度10m以下にいた場合着地判定
-    
-}   
-};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Movement.cpp	Mon Nov 01 16:52:17 2021 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "Servo.h"
+#include "Movement.h"  
+
+Servo servo1(D7);    
+Servo servo2(A3);    
+Servo servo3(A1); 
+Servo servo4(D12);
+Servo servo5(D10);
+Servo servo6(A5); 
+
+   void Movement::stop(){
+        servo1 = 0.5;
+        servo2 = 0.5;
+        servo3 = 0.5;
+        servo4 = 0.5;
+        servo5 = 0.5;
+        servo6 = 0.5;
+        wait(1);
+    }
+ 
+//前進
+    void Movement::move_forward(int time = 20)
+    {
+        servo1 = 0;
+        servo2 = 0;
+        servo3 = 0;
+        servo4 = 0;
+        servo5 = 0;
+        servo6 = 0;
+        wait(time);
+    }
+ 
+//後退
+    void Movement::move_backward()
+    {
+        servo1 = 1;
+        servo2 = 1;
+        servo3 = 1;
+        servo4 = 1;
+        servo5 = 1;
+        servo6 = 1;
+        wait(5);
+    }
+ 
+//右に曲がる
+    void Movement::turn_right(int theta = 15)
+    {
+        servo1 = 1;
+        servo2 = 1;
+        servo3 = 1;
+        servo4 = 0;
+        servo5 = 0;
+        servo6 = 0;
+        wait(theta/15);
+    }
+ 
+//左に曲がる
+    void Movement::turn_left(int theta = 15)
+    {
+        servo1 = 0;
+        servo2 = 0;
+        servo3 = 0;
+        servo4 = 1;
+        servo5 = 1;
+        servo6 = 1;
+        wait(theta/15);
+    }
+ 
+//倒れているときの処理
+void Movement::wakeup(int time){
+        for(int i=1;i<=time;i++)
+        {
+        move_forward(5);
+        move_backward();
+        turn_right();
+        turn_left();
+    }
+}    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Movement.h	Mon Nov 01 16:52:17 2021 +0000
@@ -0,0 +1,25 @@
+#ifndef MOVEMENT_H
+#define MOVEMENT_H
+
+#include "mbed.h"
+#include "Servo.h"   
+
+class Movement
+{    
+public:
+
+//停止
+    void stop();
+//前進
+    void move_forward(int time);
+//後退
+    void move_backward();
+//右に曲がる
+    void turn_right(int theta);
+//左に曲がる
+    void turn_left(int theta);
+//横転から復帰    
+    void wakeup(int time);
+};
+
+#endif
\ No newline at end of file

--- a/Servo.h	Thu Oct 28 14:44:25 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,98 +0,0 @@
-/* mbed R/C Servo Library
- * Copyright (c) 2007-2010 sford, cstyles
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-  
-#ifndef MBED_SERVO_H
-#define MBED_SERVO_H
-
-#include "mbed.h"
-
-/** Servo control class, based on a PwmOut
- *
- * Example:
- * @code
- * // Continuously sweep the servo through it's full range
- * #include "mbed.h"
- * #include "Servo.h"
- * 
- * Servo myservo(p21);
- * 
- * int main() {
- *     while(1) {
- *         for(int i=0; i<100; i++) {
- *             myservo = i/100.0;
- *             wait(0.01);
- *         }
- *         for(int i=100; i>0; i--) {
- *             myservo = i/100.0;
- *             wait(0.01);
- *         }
- *     }
- * }
- * @endcode
- */
-class Servo {
-
-public:
-    /** Create a servo object connected to the specified PwmOut pin
-     *
-     * @param pin PwmOut pin to connect to 
-     */
-    Servo(PinName pin);
-    
-    /** Set the servo position, normalised to it's full range
-     *
-     * @param percent A normalised number 0.0-1.0 to represent the full range.
-     */
-    void write(float percent);
-    
-    /**  Read the servo motors current position
-     *
-     * @param returns A normalised number 0.0-1.0  representing the full range.
-     */
-    float read();
-    
-    /** Set the servo position
-     *
-     * @param degrees Servo position in degrees
-     */
-    void position(float degrees);
-    
-    /**  Allows calibration of the range and angles for a particular servo
-     *
-     * @param range Pulsewidth range from center (1.5ms) to maximum/minimum position in seconds
-     * @param degrees Angle from centre to maximum/minimum position in degrees
-     */
-    void calibrate(float range = 0.0005, float degrees = 45.0); 
-        
-    /**  Shorthand for the write and read functions */
-    Servo& operator= (float percent);
-    Servo& operator= (Servo& rhs);
-    operator float();
-
-protected:
-    PwmOut _pwm;
-    float _range;
-    float _degrees;
-    float _p;
-};
-
-#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Mon Nov 01 16:52:17 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07

--- a/calculate.h	Thu Oct 28 14:44:25 2021 +0000
+++ b/calculate.h	Mon Nov 01 16:52:17 2021 +0000
@@ -1,10 +1,11 @@
 #include "mbed.h"
-
+ 
 //nの階乗を計算する関数
-int fact(int n){
-    int i, result = 1;
+int fact(int n)
+{
+  int i, result = 1;
   
-    if(n == 0)
+  if(n == 0)
     return 1;
   else
     {
@@ -15,7 +16,7 @@
       return result;
     }
 }
-
+ 
 float my_pow(float x, int n)
 {
   int i;
@@ -32,39 +33,39 @@
       return pow_result;
     }
 }
-
+ 
 float my_exp(float x)
 {
   int i;
   float result = 0;
-
-
+ 
+ 
   for(i = 1;i <= 25; i++)
     {
       result += my_pow(x, i) / fact(i);
     }
-
+ 
   return result + 1;
 }
-
+ 
 float my_log(float x)
 {
   int i;
   float result1, result2;
-
+ 
   x -= 1;
   result1 = 0;
   result2 = 0;
-
+ 
   for(i = 1;i <= 40;i++)
     {
       if(i % 2 == 1)
     result1 += my_pow(x, i) / i;
       else
-
+ 
     result2 += my_pow(x, i) / i;
     }
-
+ 
   return result1 - result2;
 }
    
@@ -73,7 +74,7 @@
 {
   return my_exp(y * my_log(x));
 }
-
+ 
 //高度計算
 float calculate_h(float dP0FIX,float dp,float dt){
     float dpow = 1.0/5.256;
@@ -82,7 +83,7 @@
     float s = (mypow(dP0/dp,dpow)- mypow(dP0/dP0FIX,dpow))*a - 27;
     return s;
     }
-
+ 
 //海面気圧計算
 float calculate_dp0(float dp,float dt){
     float s = dp*mypow(1 - (0.0065*27)/(dt+0.0065*27+273.15),-5.257);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/direction.cpp	Mon Nov 01 16:52:17 2021 +0000
@@ -0,0 +1,266 @@
+#include "mbed.h"
+#include "getGPS.h"
+#include "Movement.h"
+#include "direction.h"
+
+  
+// 球面三角法により、大円距離(メートル)を求める
+double distance1(double lat1, double lng1, double lat2, double lng2){
+    // 円周率
+    const double pi = 3.14159265359;
+ 
+    // 緯度経度をラジアンに変換
+    double rlat1 = lat1 * pi / 180;
+    double rlng1 = lng1 * pi / 180;
+    double rlat2 = lat2 * pi / 180;
+    double rlng2 = lng2 * pi / 180;
+ 
+    // 2点の中心角(ラジアン)を求める
+    double a =
+      sin(rlat1) * sin(rlat2) +
+      cos(rlat1) * cos(rlat2) *
+      cos(rlng1 - rlng2);
+    double rr = acos(a);
+ 
+    // 地球赤道半径(メートル)
+    const double earth_radius = 6378140;
+ 
+    // 2点間の距離(メートル)
+    double distance2 = earth_radius * rr;
+ 
+    return distance2;
+}
+
+
+
+//CanSatから見た目的地の角度theta(-180<=theta<=180で、角度は正面が0で反時計回り)の計算  
+float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){
+    
+    //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地
+    //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180)   
+    //theta_0,theta_1は北が90、東が0
+    //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り)   
+    
+    float theta_0,theta_1;
+    
+    if(x_0 == x_1 && x_1 == x_2){
+        
+        if(y_0 - y_1 > 0){
+            theta_0 =  90;
+            }
+        if(y_0 - y_1 < 0){
+            theta_0 = 270;
+            }
+        if(y_0 - y_1 == 0){
+            theta_0 = 0;
+            }
+        
+        if(y_1 - y_2 > 0){
+            theta_1 =  90;
+            }
+        if(y_1 - y_2 < 0){
+            theta_1 = 270;
+            }
+        if(y_1 - y_2 == 0){
+            theta_1 = 0;
+            }    
+    }
+        
+    if(x_0 == x_1 && x_1 != x_2){
+        theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2));  
+        
+        if(y_0 - y_1 > 0){
+            theta_0 =  90;
+            }
+            
+        if(y_0 - y_1 < 0){
+            theta_0 = 270;
+            }
+            
+        if(y_0 - y_1 == 0){
+            theta_0 = 0;
+            }
+    
+        if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
+            theta_1 = theta_1 - 180;
+        }
+        if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
+            theta_1 = 180;
+        }
+        if(y_1 - y_2 < 0 && x_1 - x_2 > 0){
+            theta_1 = theta_1 + 180;
+        }     
+    }
+    
+    if(x_0 != x_1 && x_1 == x_2){
+        
+        theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1));  
+    
+        if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
+            theta_0 = theta_0 - 180;
+        }
+        if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
+            theta_0 = 180;
+        }
+        if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
+            theta_0 = theta_0 + 180;
+        }
+        
+        if(y_1 - y_2 > 0){
+            theta_1 =  90;
+            }
+        if(y_1 - y_2 < 0){
+            theta_1 = 270;
+            }
+        if(y_1 - y_2 == 0){
+            theta_1 = 0;
+            }    
+    }
+    
+    else{
+        theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1));
+        theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2));  
+    
+        if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
+            theta_0 = theta_0 - 180;
+        }
+        if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
+            theta_0 = 180;
+        }
+        if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
+            theta_0 = theta_0 + 180;
+        }
+    
+        if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
+            theta_1 = theta_1 - 180;
+        }
+        if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
+            theta_1 = 180;
+        }
+        if(y_1 - y_2 < 0 && x_1- x_2 > 0){
+            theta_1 = theta_1 + 180;
+        } 
+    
+        
+    }
+    
+    float theta = theta_0 - theta_1;
+    if(theta < -180){
+      theta = 360 - theta;
+      }
+    if(theta > 180){
+      theta = -360 + theta;
+      }  
+      
+    return theta;
+}      
+
+void direction::walk(){
+        GPS gps(D1, D0);
+        Movement idou;
+        s = 0; 
+        x_0 = 0;
+        y_0 = 0;
+        x_01 = 0;
+        y_01 = 0;
+        
+        while(s<1){
+        if(gps.getgps()){
+            x_1 = gps.longitude; 
+            y_1 = gps.latitude;
+            idou.move_forward(20);
+            s = s + 1;
+            }
+        }    
+        
+       while(1){        
+        if(gps.getgps()){ //現在地取得
+            x_2 = x_1;           
+            y_2 = y_2;           //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
+            x_1 = gps.longitude; 
+            y_1 = gps.latitude; //現在地の緯度と経度を取得
+            d1 = distance1(y_1,x_1,y_0,x_0);
+            d = distance1(y_1,x_1,y_2,x_2);
+            theta =  calculate_theta(x_0,y_0,x_1,y_1,x_2,y_2);
+            
+            //中間地点に到達したらbreak            
+            if(d1 < 15){
+                idou.stop();
+                break;
+                } 
+            
+            //移動距離が短ければ横転と判定し復帰   
+            if(d < 3){
+                idou.wakeup(2);
+                idou.stop();
+            } 
+            
+            //Θによって回転か直進か決定
+            if(-30 < theta && theta < 30){
+                idou.move_forward(20);
+                }
+            if(theta >=30){
+                idou.turn_right(theta);
+                idou.stop();
+                idou.move_forward(20);
+                }
+            if(theta<=-30){
+                idou.turn_left(theta);
+                idou.stop();
+                idou.move_forward(20);
+                }        
+                            
+        }
+    }
+        while(s < 2){
+            if(gps.getgps()){
+            x_1 = gps.longitude; 
+            y_1 = gps.latitude;
+            idou.move_forward(20);
+            s  = s + 1;
+            }  
+        }
+    
+    while(1){
+        
+        if(gps.getgps()){ //現在地取得
+            x_2 = x_1;           
+            y_2 = y_2;           //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
+            x_1 = gps.longitude; 
+            y_1 = gps.latitude; //現在地の緯度と経度を取得
+            d2 = distance1(y_1,x_1,y_01,x_01);
+            d = distance1(y_1,x_1,y_2,x_2);
+            theta =  calculate_theta(x_01,y_01,x_1,y_1,x_2,y_2);
+            
+            //ゴール地点に到達したらbreak
+            if(d2 < 15){
+                idou.stop();
+                break;
+                } 
+            
+            //移動距離が短ければ横転と判定し復帰   
+            if(d < 3){
+                idou.wakeup(2);
+                idou.stop();
+            } 
+            
+            //thetaによって回転か直進か決定
+            if(-30 < theta && theta < 30){
+                idou.move_forward(20);
+                }
+            if(theta >=30){
+                idou.turn_right(theta);
+                idou.move_forward(20);
+                }
+            if(theta<= -30){
+                idou.turn_left(theta);
+                idou.move_forward(20);
+                }        
+                            
+        }
+    
+        
+    } 
+    
+    idou.stop();  
+}
\ No newline at end of file

--- a/direction.h	Thu Oct 28 14:44:25 2021 +0000
+++ b/direction.h	Mon Nov 01 16:52:17 2021 +0000
@@ -1,273 +1,9 @@
 #include "mbed.h"
 #include "getGPS.h"
+#include "Movement.h"
 #include "Servo.h"
  
-// 球面三角法により、大円距離(メートル)を求める
-double distance(double lat1, double lng1, double lat2, double lng2){
-    // 円周率
-    const double pi = 3.14159265359;
- 
-    // 緯度経度をラジアンに変換
-    double rlat1 = lat1 * pi / 180;
-    double rlng1 = lng1 * pi / 180;
-    double rlat2 = lat2 * pi / 180;
-    double rlng2 = lng2 * pi / 180;
- 
-    // 2点の中心角(ラジアン)を求める
-    double a =
-      sin(rlat1) * sin(rlat2) +
-      cos(rlat1) * cos(rlat2) *
-      cos(rlng1 - rlng2);
-    double rr = acos(a);
- 
-    // 地球赤道半径(メートル)
-    const double earth_radius = 6378140;
- 
-    // 2点間の距離(メートル)
-    double distance = earth_radius * rr;
- 
-    return distance;
-}
-
-
-
-//CanSatから見た目的地の角度theta(-180<=theta<=180で、角度は正面が0で反時計回り)の計算  
-float calculate_theta(float x_0,float y_0,float x_1,float y_1,float x_2,float y_2){
-    
-    //float x_0 ,y_0: 目的地 , float x_1 ,y_1: 現在地, float x_2 ,y_2: 20秒前の現在地
-    //theta_0:目的地の角度,theta_1:CanSatの角度theta:CanSatから見た目的地の角度(-180)   
-    //theta_0,theta_1は北が90、東が0
-    //theta:CanSatから見た目的地の角度(-180<=theta<=180で、角度は正面が0で反時計回り)   
-    
-    float theta_0,theta_1;
-    
-    if(x_0 == x_1 && x_1 == x_2){
-        
-        if(y_0 - y_1 > 0){
-            theta_0 =  90;
-            }
-        if(y_0 - y_1 < 0){
-            theta_0 = 270;
-            }
-        if(y_0 - y_1 == 0){
-            theta_0 = 0;
-            }
-        
-        if(y_1 - y_2 > 0){
-            theta_1 =  90;
-            }
-        if(y_1 - y_2 < 0){
-            theta_1 = 270;
-            }
-        if(y_1 - y_2 == 0){
-            theta_1 = 0;
-            }    
-    }
-        
-    if(x_0 == x_1 && x_1 != x_2){
-        theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2));  
-        
-        if(y_0 - y_1 > 0){
-            theta_0 =  90;
-            }
-            
-        if(y_0 - y_1 < 0){
-            theta_0 = 270;
-            }
-            
-        if(y_0 - y_1 == 0){
-            theta_0 = 0;
-            }
-    
-        if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
-            theta_1 = theta_1 - 180;
-        }
-        if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
-            theta_1 = 180;
-        }
-        if(y_1 - y_2 < 0 && x_1 - x_2 > 0){
-            theta_1 = theta_1 + 180;
-        }     
-    }
-    
-    if(x_0 != x_1 && x_1 == x_2){
-        
-        theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1));  
-    
-        if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
-            theta_0 = theta_0 - 180;
-        }
-        if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
-            theta_0 = 180;
-        }
-        if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
-            theta_0 = theta_0 + 180;
-        }
-        
-        if(y_1 - y_2 > 0){
-            theta_1 =  90;
-            }
-        if(y_1 - y_2 < 0){
-            theta_1 = 270;
-            }
-        if(y_1 - y_2 == 0){
-            theta_1 = 0;
-            }    
-    }
-    
-    else{
-        theta_0 = atan(9*(y_0 - y_1)/11*(x_0 - x_1));
-        theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2));  
-    
-        if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
-            theta_0 = theta_0 - 180;
-        }
-        if(y_0 - y_1 == 0 && x_0 - x_1 < 0){
-            theta_0 = 180;
-        }
-        if(y_0 - y_1 < 0 && x_0 - x_1 > 0){
-            theta_0 = theta_0 + 180;
-        }
-    
-        if(y_1 - y_2 > 0 && x_1 - x_2 < 0){
-            theta_1 = theta_1 - 180;
-        }
-        if(y_1 - y_2 == 0 && x_1 - x_2 < 0){
-            theta_1 = 180;
-        }
-        if(y_1 - y_2 < 0 && x_1- x_2 > 0){
-            theta_1 = theta_1 + 180;
-        } 
-    
-        
-    }
-    
-    float theta = theta_0 - theta_1;
-    if(theta < -180){
-      theta = 360 - theta;
-      }
-    if(theta > 180){
-      theta = -360 + theta;
-      }  
-      
-    return theta;
-}
- 
-   
-//停止
-   void stop(){
-        Servo servo1(D7);
-Servo servo2(A3);  
-Servo servo3(A1);
-Servo servo4(D12);
-Servo servo5(D10);
-Servo servo6(A5);
-
-        servo1 = 0.5;
-        servo2 = 0.5;
-        servo3 = 0.5;
-        servo4 = 0.5;
-        servo5 = 0.5;
-        servo6 = 0.5;
-        wait(1);
-    }
-
-//前進
-    void move_forward(int time = 20)
-    {
-        Servo servo1(D7);
-Servo servo2(A3);  
-Servo servo3(A1);
-Servo servo4(D12);
-Servo servo5(D10);
-Servo servo6(A5);
-
-        servo1 = 0;
-        servo2 = 0;
-        servo3 = 0;
-        servo4 = 0;
-        servo5 = 0;
-        servo6 = 0;
-        wait(time);
-    }
-
-//後退
-    void move_backward()
-    {
-        Servo servo1(D7);
-Servo servo2(A3);  
-Servo servo3(A1);
-Servo servo4(D12);
-Servo servo5(D10);
-Servo servo6(A5);
-
-        servo1 = 1;
-        servo2 = 1;
-        servo3 = 1;
-        servo4 = 1;
-        servo5 = 1;
-        servo6 = 1;
-        wait(5);
-    }
-
-//右に曲がる
-    void turn_right(int theta = 15)
-    {
-        Servo servo1(D7);
-Servo servo2(A3);  
-Servo servo3(A1);
-Servo servo4(D12);
-Servo servo5(D10);
-Servo servo6(A5);
-
-        servo1 = 1;
-        servo2 = 1;
-        servo3 = 1;
-        servo4 = 0;
-        servo5 = 0;
-        servo6 = 0;
-        wait(theta/15);
-    }
-
-//左に曲がる
-    void turn_left(int theta = 15)
-    {
-        Servo servo1(D7);
-Servo servo2(A3);  
-Servo servo3(A1);
-Servo servo4(D12);
-Servo servo5(D10);
-Servo servo6(A5);
-
-        servo1 = 0;
-        servo2 = 0;
-        servo3 = 0;
-        servo4 = 1;
-        servo5 = 1;
-        servo6 = 1;
-        wait(theta/15);
-    }
-
-//倒れているときの処理
-    void wakeup(int time)
-    {
-        Servo servo1(D7);
-Servo servo2(A3);  
-Servo servo3(A1);
-Servo servo4(D12);
-Servo servo5(D10);
-Servo servo6(A5);
-
-        int i;
-        for(i=1;i<=time;i++)
-        {
-        move_forward(5);
-        move_backward();
-        turn_right();
-        turn_left();
-}
-
-class direction:Servo
+class direction
 {   
     private:
     
@@ -282,116 +18,9 @@
     public:
     
     //歩行
-    void walk(){
-        GPS gps(D1, D0);
-        s = 0; 
-        x_0 = 0;
-        y_0 = 0;
-        x_01 = 0;
-        y_01 = 0;
-        
-        while(s<1){
-        if(gps.getgps()){
-            x_1 = gps.longitude; 
-            y_1 = gps.latitude;
-            move_forward();
-            s = s + 1;
-            }
-        }    
-        
-       while(1){        
-        if(gps.getgps()){ //現在地取得
-            x_2 = x_1;           
-            y_2 = y_2;           //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
-            x_1 = gps.longitude; 
-            y_1 = gps.latitude; //現在地の緯度と経度を取得
-            d1 = distance(y_1,x_1,y_0,x_0);
-            d = distance(y_1,x_1,y_2,x_2);
-            theta =  calculate_theta(x_0,y_0,x_1,y_1,x_2,y_2);
-            
-            //中間地点に到達したらbreak            
-            if(d1 < 15){
-                stop();
-                break;
-                } 
-            
-            //移動距離が短ければ横転と判定し復帰   
-            if(d < 3){
-                wakeup(2);
-                stop();
-            } 
-            
-            //Θによって回転か直進か決定
-            if(-30 < theta && theta < 30){
-                move_forward();
-                }
-            if(theta >=30){
-                turn_right(theta);
-                stop();
-                move_forward();
-                }
-            if(theta<=-30){
-                turn_left(theta);
-                stop();
-                move_forward();
-                }        
-                            
-        }
-    }
-        while(s < 2){
-            if(gps.getgps()){
-            x_1 = gps.longitude; 
-            y_1 = gps.latitude;
-            move_forward();
-            s = s + 1;
-            }  
-        }
-    
-    while(1){
-        
-        if(gps.getgps()){ //現在地取得
-            x_2 = x_1;           
-            y_2 = y_2;           //20(+回転or復帰時間)秒前に居た地点の緯度と経度を取得
-            x_1 = gps.longitude; 
-            y_1 = gps.latitude; //現在地の緯度と経度を取得
-            d2 = distance(y_1,x_1,y_01,x_01);
-            d = distance(y_1,x_1,y_2,x_2);
-            theta =  calculate_theta(x_01,y_01,x_1,y_1,x_2,y_2);
-            
-            //ゴール地点に到達したらbreak
-            if(d2 < 15){
-                stop();
-                break;
-                } 
-            
-            //移動距離が短ければ横転と判定し復帰   
-            if(d < 3){
-                wakeup(2);
-                stop();
-            } 
-            
-            //thetaによって回転か直進か決定
-            if(-30 < theta && theta < 30){
-                move_forward();
-                }
-            if(theta >=30){
-                turn_right(theta);
-                move_forward();
-                }
-            if(theta<= -30){
-                turn_left(theta);
-                move_forward();
-                }        
-                            
-        }
-    
-        
-    } 
-    
-    stop();  
-    }
+    void walk();
 
-  
 };
 
+
         
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/getGPS.cpp	Mon Nov 01 16:52:17 2021 +0000
@@ -0,0 +1,57 @@
+#include "mbed.h"
+#include "getGPS.h"
+
+GPS::GPS(PinName gpstx,PinName gpsrx): _gps(gpstx,gpsrx)
+{
+    latitude = 0;
+    longitude = 0;
+    _gps.baud(GPSBAUD);
+    _gps.printf("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29");
+}
+
+bool GPS::getgps()
+{
+    char gps_data[256];
+    int i;
+    
+    do {
+        while(_gps.getc() != '$'); //$マークまで読み飛ばし
+        i = 0;
+
+        /* gpa_data初期化 */
+        for(int j = 0; j < 256; j++)
+            gps_data[j] = '\0';
+
+        /* NMEAから一行読み込み */
+        while((gps_data[i] = _gps.getc()) != '\r') {
+            i++;
+            if(i == 256) {
+                i = 255;
+                break;
+            }
+        }
+    } while(strstr(gps_data, "GPGGA") == NULL); //GGAセンテンスまで一行ずつ読み込み続ける
+    
+    int rlock;
+    char ns,ew;
+    double w_time, raw_longitude, raw_latitude;
+    int satnum;
+    double hdop;
+
+    if(sscanf(gps_data, "GPGGA, %lf, %lf, %c, %lf, %c, %d, %d, %lf", &w_time, &raw_latitude, &ns, &raw_longitude, &ew, &rlock, &satnum, &hdop) > 1) {
+        /* 座標1（度部分） */
+        int latitude_dd = (int)(raw_latitude / 100);
+        int longitude_dd = (int)(raw_longitude / 100);
+
+        /* 座標2（分部分 → 度） */
+        double latitude_md = (raw_latitude - latitude_dd * 100) / 60;
+        double longitude_md = (raw_longitude - longitude_dd * 100) / 60;
+
+        /* 座標1 + 2 */
+        latitude = latitude_dd + latitude_md;
+        longitude = longitude_dd + longitude_md;
+
+        return true;
+    } else
+        return false; //GGAセンテンスの情報が欠けている時
+}

--- a/main.cpp	Thu Oct 28 14:44:25 2021 +0000
+++ b/main.cpp	Mon Nov 01 16:52:17 2021 +0000
@@ -1,30 +1,71 @@
 #include "mbed.h"
 #include "direction.h"
-#include "Landing_Judgement.h"
+#include "BMP180.h"
+#include "calculate.h"
+#define PIN_SDA D4
+#define PIN_SCL D5
+
 DigitalOut Nichrome(A6);
-            
+ 
 int main(){
-    int land_judgement1=0;
-    //着地判定
-    while(1){
-        land_judgement1 = Landing_Judgement.land();
-        if(land_judgement1==1){
-        break;
-        }  
+    BMP180 bmp180(PIN_SDA,PIN_SCL);
+    int x = 0,y = 0,n1 = 0, n2 = 0;
+    int i;
+    float h,dp,dt,dp0;
+    float a = 0,b = 0,dp_ave,dt_ave;
+    //hは高度、dpは気圧、dtは温度、dp0は海面気圧
+    bmp180.Initialize(27,BMP180_OSS_ULTRA_LOW_POWER);//27は府大の海抜高度
+    
+    //海面気圧を計算
+    for(i=0;i<15;i++){
+        if(bmp180.ReadData(&dt,&dp)){
+            a = a + dp;
+            b = b + dt;
+            n1 = n1 + 1;
+            n2 = n2 + 1;
+            wait(1);
+            }
+        }
+    
+    dp_ave = a / n1;
+    dt_ave = b / n2;    
+    dp0 = calculate_dp0(dp_ave,dt_ave);
+ 
+    //10秒以上高度10mにいた場合離陸判定       
+    while(x<10){
+            if(bmp180.ReadData(&dt,&dp)){
+                h = calculate_h(dp0,dp,dt);
+                if(h >= 10){
+                x = x + 1;
+                    }
+                wait(1);
+            }
     }
     
-    wait(5);
+    wait(10);    
+ 
+  //離陸判定後、10秒以上高度2m以下にいた場合着地判定    
+    while(y<10){
+            if(bmp180.ReadData(&dt,&dp)){
+                h = calculate_h(dp0,dp,dt);
+                if(h <= 2){
+                y = y + 1;
+                    }
+                wait(1);
+            }
+    }
+  //30秒待機                
     wait(30);
 
     //パラシュート分離
     Nichrome=1;
     wait(10);
     Nichrome=0;
-
-    //中間地点を経由してゴール地点まで自律移動
-    direction.walk();
+    
+    //中間地点を経由してゴール地点まで移動
+    direction hokou;
+    hokou.walk();
     
     return 0;
-
-}
-   
\ No newline at end of file
+    
+    }
\ No newline at end of file