Diff: main.cpp

Revision:

7:db6b436c0baa

Parent:

6:23bb3b018f44

Child:

9:2741e17438d6

--- a/main.cpp	Fri Aug 07 14:35:26 2015 +0000
+++ b/main.cpp	Sat Aug 08 16:29:30 2015 +0000
@@ -80,6 +80,10 @@
 int mode = -1; //ロボットのモード
 double  goal_Pressure, goal_Temperature,goal_Humidity; //地表地点の気圧、気温、湿度
 
+Timer sep_Timer;
+const int sep_Time = 30000; //seperate time in ms
+int fall_flag = 0;
+
 
 /////////////////////////////////////////
 //
@@ -261,10 +265,22 @@
 落下モード
 ******************************/
 void falling(){
+    
+    cansat.set_temperature(sensor.getTemperature()); 
+    cansat.set_pressure(sensor.getPressure());
+    cansat.set_humidity(sensor.getHumidity());
+    
     if(cansat.get_pressure() >= goal_Pressure){
-        nic = 1;
-        wait_ms(10000);
-        mode = 2;
+        if(fall_flag == 0){
+            nic = 1;
+            fall_flag = 1;
+            sep_Timer.reset();
+        }
+        if(sep_Timer.read_ms() >= sep_Time){
+            mode = 2;
+            nic = 0;
+        }
+
     }    
 }
 
@@ -279,7 +295,7 @@
     double x2 = cansat.get_robot_x();
     double dx = x2 - x1;
 
-   cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
+    cansat.set_target_distance(r*acos(sin(y1)*sin(y2)+cos(y1)*cos(y2)*cos(dx)));
     
     if(cansat.get_target_distance() < 10) cansat.set_speed(32);
     else if(cansat.get_target_distance() < 20 && cansat.get_target_distance() > 10) cansat.set_speed(64);
@@ -302,7 +318,7 @@
         }
     }
     
-    if(cansat.get_robot_x() - cansat.get_target_x() <= 0 && cansat.get_robot_y() - cansat.get_target_y() <= 0){
+    if(cansat.get_target_distance() <= 1){
         mode = 100;
     }
 }
@@ -355,6 +371,7 @@
     short_out = 1; //ショートピンの出力:high
     nic.output();
     nic = 0;
+    int log = 0;
     
     while (true) {
         
@@ -393,7 +410,9 @@
             refresh_Timer.reset();
             //print_gps(count);
             Get_GPS(&myGPS);
-            pc.printf("%2.2f degC, %04.2f hPa, %2.2f %%\n", sensor.getTemperature(), sensor.getPressure(), sensor.getHumidity());
+            log++;
+            pc.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
+            pc.printf("robot_angle:%d, target_angle:%d, robot_compass:%f, %04.2f hPa\n",cansat.get_robot_angle(), cansat.get_target_angle(),  cansat.get_compass_z(), cansat.get_pressure());
         }
     }