Diff: main.cpp

Revision:

1:bb89b58cfa0e

Parent:

0:e7b7def631c2

Child:

2:d2cb6b50a8c4

--- a/main.cpp	Thu Oct 21 01:58:35 2021 +0000
+++ b/main.cpp	Wed Oct 27 19:11:01 2021 +0000
@@ -2,33 +2,116 @@
 #include "BMP180.h"
 #include "getGPS.h"
 #include "Servo.h"
-#include "calculate.h"
-#include "MPU9250.h"
+#include "Movement.h"
+#include "direction.h"
 #define PIN_SDA D4
 #define PIN_SCL D5
-#define STOPTIME 1
-#define GOTIME 5
-#define ROTATETIME 1
 
-
-Serial xbee(PA_9, PA_10);
-GPS gps(PA_2,PA_3);
+GPS gps(D1,D0);
 DigitalOut Nichrome(A6);
 BMP180 bmp180(PIN_SDA,PIN_SCL);
-MPU9250 mpu9250(D4, D5);
-Servo myservo1(D7);    
-Servo myservo2(A3);    
-Servo myservo3(A1); 
-Servo myservo4(D12);
-Servo myservo5(D10);
-Servo myservo6(A5); 
 double bias_la=0,bias_lo=0;
 
+//nの階乗を計算する関数
+int fact(int n)
+{
+  int i, result = 1;
+  
+  if(n == 0)
+    return 1;
+  else
+    {
+      for(i = 1;i <= n;i++)
+    {
+      result *= i;
+    } 
+      return result;
+    }
+}
+
+float my_pow(float x, int n)
+{
+  int i;
+  float pow_result = 1;
+ 
+  if(n == 0)
+    return 1;
+  else
+    {
+      for(i = 0;i < n;i++)
+    {
+      pow_result *= x;
+    }
+      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;
+}
+   
+//累乗 
+float mypow(float x, float y)
+{
+  return my_exp(y * my_log(x));
+}
+
+//高度計算
+float calculate_h(float dP0FIX,float dp,float dt){
+    float dpow = 1.0/5.256;
+    float dP0 = 1013.25;
+    float a = (dt+(float)273.15)/(float)0.0065;
+    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);
+    return s;
+    }
+    
 int landing_judgement(){
-    bmp180.Initialize(27,0);
-    int i,n1,n2,landing_judgement;
-    float dt,dp,h,dp0,a,b,dt_ave,dp_ave;
-    for(i=0;i<15;i++){
+    
+    int x ,y ,n1 , n2 ;
+    int landing_judgement=0 ;
+    float h,dp,dt,dp0;
+    float a ,b ,dp_ave,dt_ave;
+    
+    bmp180.Initialize(27,BMP180_OSS_ULTRA_LOW_POWER);//27は府大の海抜高度
+    
+    for(int i=0;i<15;i++){
         if(bmp180.ReadData(&dt,&dp)){
             a = a + dp;
             b = b + dt;
@@ -41,7 +124,7 @@
     dp_ave = a / n1;
     dt_ave = b / n2;    
     dp0 = calculate_dp0(dp_ave,dt_ave);
-    int x,y;
+    
     while(x<10){
             if(bmp180.ReadData(&dt,&dp)){
                 h = calculate_h(dp0,dp,dt);
@@ -72,111 +155,8 @@
     
     //離陸判定後、10秒以上高度10m以下にいた場合着地判定
     
-}
-
-
-//止まる
-void stop()
-    {
-        myservo1 = 0.5;
-        myservo2 = 0.5;
-        myservo3 = 0.5;
-        myservo4 = 0.5;
-        myservo5 = 0.5;
-        myservo6 = 0.5;
-        wait(STOPTIME);
-    }
-
-//前進
-void move_forward()
-{
-    myservo1 = 0;
-    myservo2 = 0;
-    myservo3 = 0;
-    myservo4 = 0;
-    myservo5 = 0;
-    myservo6 = 0;
-    wait(GOTIME);
-}
-
-//後退
-void move_backward()
-{
-    myservo1 = 1;
-    myservo2 = 1;
-    myservo3 = 1;
-    myservo4 = 1;
-    myservo5 = 1;
-    myservo6 = 1;
-    wait(GOTIME);
-}
-
-//右に曲がる
-void turn_right()
-{
-    myservo1 = 1;
-    myservo2 = 1;
-    myservo3 = 1;
-    myservo4 = 0;
-    myservo5 = 0;
-    myservo6 = 0;
-    wait(ROTATETIME);
-}
-
-//左に曲がる
-void turn_left()
-{
-    myservo1 = 0;
-    myservo2 = 0;
-    myservo3 = 0;
-    myservo4 = 1;
-    myservo5 = 1;
-    myservo6 = 1;
-    wait(ROTATETIME);
-}
-
-//倒れているときの処理
-void wakeup(int time)
-{
-    int i;
-    for(i=1;i<=time;i++)
-    {
-    move_forward()
-    move_backward()
-    turn_right()
-    turn_left()
-    }
-}
-
-
-
-float calculation_sita(float x_0,float y_0,float x_1,float y_1,float x,float y){
-    //x,yは地磁気センサの値で北の方角の角度を90度、東の方角を0度とする。
-    float sita_0 = atan((y_0 - y_1)/(x_0 - x_1));  //目的地の角度
-    
-    if(y_0 - y_1 > 0 && x_0 - x_1 < 0){
-        sita_0 = sita_0 - 180;
-    }
-    if(y_0 - y_1 < 0 && x_0 - x_1 < 0){
-        sita_0 = sita_0 + 180;
-    }
-    
-    float Omag_x=0,Omag_y=0; //地磁気センサのxy平面が描く円の中点
-    float sita_1 = atan((y - Omag_y)/(x- Omag_x));　//CanSatの角度 
-    
-    if(y - Omag_y > 0 && x- Omag_x < 0){
-        sita_1 = sita_1 - 180;
-    }
-    if(y - Omag_y < 0 && x - Omag_x < 0){
-        sita_1 = sita_1 + 180;
-    }
-    float sita
-    sita= sita_0 - sita_1;  //CanSatから見た目的地の角度
-    return sita
-}    
-      
-    
-    
+}        
+             
 int parachute_separation()
 {
         Nichrome=1;
@@ -185,70 +165,27 @@
     return 0;
     }
 
-int end_flag=0;
-double bias_la=0,bias_lo=0;
-
-
-
 int main(void)
 {   
 int land_judgement_1=0,sep_judge=1;
+
 //着地判定
 while(1)
 {
-    land_judgement=landing_judgement()
+    land_judgement_1 = landing_judgement();
     if(land_judgement_1==1)
     {
-xbee.printf("landing\r\n");  
         break;
     }  
 }
+
 wait(30);
-//パラシュート切り離し
-while(1)
-{
-    sep_judge==1;
-    sep_judge=parachute_separation();
-    if(sep_judge==0)
-    {
-        xbee.printf("did separation\r\n");
-        xbee.printf("separation complleted?\ty/n")
-        int received_data = xbee.getc();
-        if(received_data==89||received_data==121)
-        {   
-            xbee.printf("separation complieted\n\r");
-            break;
-        }
-        if(received_data==78||received_data==110)
-        {   
-            xbee.printf("try again now\n\r");
-        }
-    }
-}
-//中間座標の取得
-double midpoint_latitude,midpoint_longtitude;
-xbee.printf("\r\n-----------------------\r\n");
-xbee.printf("midpoint latitude:");
-xbee.scanf("%lf",&midpoint_latitude);
-xbee.printf("\r\nmidpoint longtitude:");
-xbee.scanf("%lf",&midpoint_longtitude);
-xbee.printf("\r\n-----------------------\r\n");
-//倒れているかの判定
-int i;
-gps
-for(i=1;i<=2;i++)
-{
-    move_forward();
-}
 
-
-move_forward();
+//中間地点を経由してゴール地点まで自律移動
+direction.walk();
 
-//倒れているときの処理
-wakeup(2)
+return 0;
 
-
-#
 }