Revision 3:b48cac06f913, committed 2020-12-01

Comitter:

imadaemi

Date:

Tue Dec 01 12:07:32 2020 +0000

Parent:

2:7bf845f17396

Commit message:

20.12.1_main program test

Changed in this revision

--- a/main.cpp	Sun Nov 29 11:01:32 2020 +0000
+++ b/main.cpp	Tue Dec 01 12:07:32 2020 +0000
@@ -11,8 +11,8 @@
 #define SEA_LEVEL_PRESS 1029.3//投下前に設定
 #define CURRENT_LOCATION_PRESS 1020.624268//投下前に設定
 #define CURRENT_LOCATION_TEMP 10.650000//投下前に設定
-#define LAT_GOAL 33.594910//投下前に計測
-#define LON_GOAL 130.217892//投下前に計測
+#define LAT_GOAL 33.595085//投下前に計測
+#define LON_GOAL 130.217850//投下前に計測
 #define PI 3.14159265358979323846
 #define R 6378137
 
@@ -25,18 +25,12 @@
 DigitalIn flightpin(p8);
 DigitalOut pin(p22);
 
-//発注基板用
-PwmOut motor_left26(p23);//右IN2
-PwmOut motor_left25(p24);//右IN1
-PwmOut motor_right24(p25);//左IN1
-PwmOut motor_right23(p26);//左IN2
-
-/*切削基板用
+//切削基板・発注基板共用
 PwmOut motor_left26(p26);//左IN2
 PwmOut motor_left25(p25);//左IN1
 PwmOut motor_right24(p24);//右IN1
 PwmOut motor_right23(p23);//右IN2
-*/
+
 
 LPS22HB lps22hb(i2c, LPS22HB::SA0_LOW);
 mpu9250 mpu9250(i2c,AD0_HIGH);
@@ -45,13 +39,6 @@
 QQQCAM cam(xbee_raspi);
 
 Timer timer;
-//Timeout nich_timeout;
-//Timeout nich_wait_timeout;
-//Timeout motor_timeout;
-//Ticker tick_mpu9250;
-//Ticker tick_lps22hb;
-//Ticker tick_gps;
-//Ticker tick_time;
 Ticker tick_data;
 Ticker tick_data_status;
 
@@ -109,10 +96,11 @@
     cansat_phase = 1;
     Phase1();
     Phase2();
-    Phase3();
-    //cansat_phase = 5;
-    //Phase4();
+    //Phase3();
+    cansat_phase = 4;
+    Phase4();
     //rasp_data_received();
+    
 }
 
 void Phase1()
@@ -202,6 +190,7 @@
 
     MotorFront(1.0, 1.0, 3.0);
     while(cansat_phase == 4) {
+        //wait(4.0);
         GetGPS();
 
         //xbee.printf("lat : %f\tlon : %f\tlat_0 : %f\tlon_0 : %f\t\r\n", lat, lon, lat_0, lon_0);
@@ -329,6 +318,7 @@
             cansat_phase = 6;
             cansat_goal = 1;
             Goal = 1;
+            break;
             }
         
         percent = cam.get_rate();
@@ -364,10 +354,77 @@
         //MotorFront(0, 0, 2.0);    
     }
     MotorStop(); 
+    wait(3.0);
+    tick_data.detach();
     pc.printf("Bye!!!\n");
 
         
 }
+
+/*
+void rasp_data_received(){
+    int Goal = 0;
+    phase5_time = timer.read();
+    
+    //int flag = 0;
+    /*
+    while(flag == 0){
+        if(cam.get_rate() == 0){
+            //xbee.printf("Rasp Waiting\n");
+            
+        }else{
+            flag = 1;
+        }
+    }
+    */
+/*      
+    while(Goal == 0){
+        //pc.printf("rate:%.3f\r\n", cam.get_rate());
+        //xbee.printf("rate:%.3f\r\n", cam.get_rate());
+        
+        if(120 <= timer.read() - phase5_time){
+            cansat_phase = 6;
+            cansat_goal = 1;
+            Goal = 1;
+            }
+        
+        percent = cam.get_rate();
+        
+        if(percent >= 0.1){
+             color_mode = 2;
+             //pc.printf("Goal!!!");
+             //xbee.printf("Goal!!!");
+             //tick_data_status.detach();
+             //tick_data.detach();
+             cansat_phase = 6;
+             cansat_goal = 1;
+             Goal = 1;
+        }else if(percent >=0.001){
+            color_mode = 1;
+            direction_mode = 2;
+            //pc.printf("Red!!!");
+            //xbee.printf("Red!!!");
+            
+            MotorFront(1.0, 1.0, 1.0);
+            //MotorFront(0, 1.0, 0.3);
+            //MotorFront(0, 0, 1.0);
+        }else{
+            color_mode = 0;
+            direction_mode = -1;
+            
+            //pc.printf("No Red!!!");
+            //xbee.printf("No Red!!!");
+            MotorFront(1.0, 0.5, 1.0);
+            //MotorFront(0, 0, 1.0);
+        }  
+        MotorFront(0.1, 0.1, 2.0);
+        //MotorFront(0, 0, 2.0);    
+    }
+    MotorStop(); 
+    pc.printf("Bye!!!\n");   
+}
+
+
 /*
 void on_data_received()
 {
@@ -464,10 +521,6 @@
     xbee.printf("Driving Mode : %d\r\n", driving_mode);
     xbee.printf("Direction    : %d\r\n", direction_mode);
     xbee.printf("Distance     : %.2f\r\n", cansat_distance);
-    xbee.printf("lat          : %.2f\r\n", lat);
-    xbee.printf("lat_0        : %.2f\r\n", lat_0);
-    xbee.printf("lon          : %.2f\r\n", lon);
-    xbee.printf("lon_0        : %.2f\r\n", lon_0);
     xbee.printf("Color Percent: %f\r\n", percent);
     xbee.printf("Color        : %d\r\n", color_mode);
     xbee.printf("Phase5 Time  : %f\r\n", timer.read() - phase5_time);