Diff: main.cpp

Revision:

0:e7b7def631c2

Child:

1:bb89b58cfa0e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 21 01:58:35 2021 +0000
@@ -0,0 +1,255 @@
+#include "mbed.h"
+#include "BMP180.h"
+#include "getGPS.h"
+#include "Servo.h"
+#include "calculate.h"
+#include "MPU9250.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);
+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;
+
+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++){
+        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);
+    int x,y;
+    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以下にいた場合着地判定
+    
+}
+
+
+//止まる
+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;
+        wait(10);
+        Nichrome=0;
+    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()
+    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();
+
+//倒れているときの処理
+wakeup(2)
+
+
+#
+}
+
+    
+    
\ No newline at end of file