Revision 13:c482c4d7a585, committed 2021-12-19

Comitter:

tsubasa_nakajima

Date:

Sun Dec 19 02:42:53 2021 +0000

Parent:

12:79946f960100

Commit message:

a

Changed in this revision

diff -r 79946f960100 -r c482c4d7a585 Movement.cpp
--- a/Movement.cpp	Fri Nov 05 15:41:24 2021 +0000
+++ b/Movement.cpp	Sun Dec 19 02:42:53 2021 +0000
@@ -2,11 +2,11 @@
 #include "Servo.h"
 #include "Movement.h"  
 
-Servo servo1(D7);    
-Servo servo2(A3);    
-Servo servo3(A1); 
-Servo servo4(D12);
-Servo servo5(D10);
+Servo servo1(A1);
+Servo servo2(A3);        
+Servo servo3(D10); 
+Servo servo4(D7);
+Servo servo5(D12);
 Servo servo6(A5); 
 
    void Movement::stop(){
@@ -22,58 +22,45 @@
 //前進
     void Movement::move_forward(int time = 20)
     {
-        servo1 = 0;
-        servo2 = 0;
-        servo3 = 0;
-        servo4 = 0;
-        servo5 = 0;
-        servo6 = 0;
+        servo5  = 1;    
         wait(time);
     }
  
 //後退
     void Movement::move_backward()
-    {
-        servo1 = 1;
-        servo2 = 1;
-        servo3 = 1;
-        servo4 = 1;
-        servo5 = 1;
-        servo6 = 1;
+    {   
+        servo1 = 0.7;
+        servo2 = 0.7;
+        servo3 = 0.7;
+        servo4 = 0.3;
+        servo5 = 0.3;       
+        servo6 = 0.3;
         wait(5);
     }
  
 //右に曲がる
     void Movement::turn_right(int theta = 15)
     {
-        servo1 = 1;
-        servo2 = 1;
-        servo3 = 1;
-        servo4 = 0;
-        servo5 = 0;
-        servo6 = 0;
+        servo1 = 0.3;
+        servo2 = 0.3;
+        servo3 = 0.5;
+        servo4 = 0.5;
+        servo5 = 0.3;
+        servo6 = 0.3;
         wait(abs(theta)/15);
     }
  
 //左に曲がる
     void Movement::turn_left(int theta = 15)
     {
-        servo1 = 0;
-        servo2 = 0;
-        servo3 = 0;
-        servo4 = 1;
-        servo5 = 1;
-        servo6 = 1;
+        servo1 = 0.5;
+        servo2 = 0.7;
+        servo3 = 0.7;
+        servo4 = 0.7;
+        servo5 = 0.7;
+        servo6 = 0.5;
         wait(abs(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
+    
+    
+        
\ No newline at end of file

diff -r 79946f960100 -r c482c4d7a585 Movement.h
--- a/Movement.h	Fri Nov 05 15:41:24 2021 +0000
+++ b/Movement.h	Sun Dec 19 02:42:53 2021 +0000
@@ -18,8 +18,6 @@
     void turn_right(int theta);
 //左に曲がる
     void turn_left(int theta);
-//横転から復帰    
-    void wakeup(int time);
 };
 
 #endif
\ No newline at end of file

diff -r 79946f960100 -r c482c4d7a585 cansat_integrated_2.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cansat_integrated_2.lib	Sun Dec 19 02:42:53 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/CansatB_2021/code/cansat_integrated_2/#79946f960100

diff -r 79946f960100 -r c482c4d7a585 direction.cpp
--- a/direction.cpp	Fri Nov 05 15:41:24 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,285 +0,0 @@
-#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 PI = 3.14159;
-    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))*180/PI;   
-        if(theta_1 < 0){
-            theta_1 =  360 + theta_1;
-            }
-        
-        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))*180/PI;   
-        
-        if(theta_0 < 0){
-            theta_0 =  360 + theta_0;
-            }
-        
-        
-        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))*180/PI;  
-        theta_1 = atan(9*(y_1 - y_2)/11*(x_1- x_2))*180/PI;
-        
-        if(theta_0 < 0){
-            theta_0 =  360 + theta_0;
-            }
-        
-        if(theta_1 < 0){
-            theta_1 =  360 + theta_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 && 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_1;           //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_left(theta);
-                idou.stop();
-                idou.move_forward(20);
-                }
-            if(theta<=-30){
-                idou.turn_right(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_1;           //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_left(theta);
-                idou.move_forward(20);
-                }
-            if(theta<= -30){
-                idou.turn_right(theta);
-                idou.move_forward(20);
-                }        
-                            
-        }
-    
-        
-    } 
-    
-    idou.stop();  
-}
-            
\ No newline at end of file

diff -r 79946f960100 -r c482c4d7a585 direction.h
--- a/direction.h	Fri Nov 05 15:41:24 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,26 +0,0 @@
-#include "mbed.h"
-#include "getGPS.h"
-#include "Movement.h"
-#include "Servo.h"
- 
-class direction
-{   
-    private:
-    
-    int s;
-    float x_0 ,y_0; //中間地点の座標(未定)
-    float x_01,y_01; //ゴール地点の座標(未定)
-    float x_1 ,y_1; //現在地
-    float x_2 ,y_2; //20秒前の現在地
-    float theta;   //CanSatから見た目的地の角度
-    float d,d1,d2;  //dはCanSatの20秒間の移動距離,d1はCanSatと中間地点の距離、d2はゴール地点との距離(単位はm)
-    
-    public:
-    
-    //歩行
-    void walk();
-
-};
-
-
-        
\ No newline at end of file

diff -r 79946f960100 -r c482c4d7a585 getGPS.cpp
--- a/getGPS.cpp	Fri Nov 05 15:41:24 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,57 +0,0 @@
-#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センテンスの情報が欠けている時
-}

diff -r 79946f960100 -r c482c4d7a585 getGPS.h
--- a/getGPS.h	Fri Nov 05 15:41:24 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,19 +0,0 @@
-#ifndef GPS_H
-#define GPS_H
-
-#define GPSBAUD 9600 //ボーレート
-
-class GPS {
-public:
-    GPS(PinName gpstx,PinName gpsrx);
-    
-    bool getgps();
-
-    double longitude;
-    double latitude;
-    
-private:
-    Serial _gps;
-};
- 
-#endif //GPS_H
\ No newline at end of file

diff -r 79946f960100 -r c482c4d7a585 main.cpp
--- a/main.cpp	Fri Nov 05 15:41:24 2021 +0000
+++ b/main.cpp	Sun Dec 19 02:42:53 2021 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
-#include "direction.h"
 #include "BMP180.h"
 #include "calculate.h"
+#include "Movement.h"
 #define PIN_SDA D4
 #define PIN_SCL D5
 
@@ -35,25 +35,25 @@
     while(x<10){
             if(bmp180.ReadData(&dt,&dp)){
                 h = calculate_h(dp0,dp,dt);
-                if(h >= 10){
+                if(h >= 15){
                 x = x + 1;
                     }
                 wait(1);
             }
     }
+    wait(10);
+ 
+  //離陸判定後、10秒以上高度2m以下にいた場合着地判定    
+    while(y<10){
+            if(bmp180.ReadData(&dt,&dp)){
+                h = calculate_h(dp0,dp,dt);
+                if(h <= 6){
+                y = y + 1;
+                    }
+                wait(1);
+            }
+    }
     
-    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);
  
@@ -62,9 +62,30 @@
     wait(10);
     Nichrome=0;    
     
-    //中間地点を経由してゴール地点まで移動
-    direction hokou;
-    hokou.walk();
+    //前進して段差を上る、15m前進
+    Movement idou;
+    idou.move_backward();
+    wait(180); 
+    
+    //右に45°方向転換して前進
+    idou.turn_right(45);
+    idou.move_backward();
+    wait(5);
+    
+    //左に45°方向転換して前進
+    idou.turn_left(45);
+    idou.move_backward();
+    wait(5);
+    
+    //右に90°方向転換して前進
+    idou.turn_right(90);
+    idou.move_backward();
+    wait(5);
+    
+    //左に90°方向転換して前進
+    idou.turn_left(90);
+    idou.move_backward();
+    wait(5);
     
     return 0;