Diff: main.cpp

Revision:

14:4dfeeca65308

Parent:

13:4f3fd6c4ddc2

Child:

15:dd7c2ab65a09

--- a/main.cpp	Tue Aug 11 23:03:59 2015 +0000
+++ b/main.cpp	Wed Aug 12 07:27:45 2015 +0000
@@ -115,6 +115,8 @@
 const int sep_Time = 30000; //seperate time in ms
 int fall_flag = 0;
 
+int log_number = 0;
+
 
 /////////////////////////////////////////
 //
@@ -319,6 +321,7 @@
     cansat.control_Motor(1, cansat.get_speed());
     if(short_in == 0){
         mode = 1;
+        xbee.printf("change mode: falling\n");
     }
     
 }
@@ -341,6 +344,7 @@
         if(sep_Timer.read_ms() >= sep_Time){
             mode = 2;
             nic = 0;
+            xbee.printf("change mode: running\n");
         }
 
     }    
@@ -349,13 +353,26 @@
 /******************************
 走行モード
 ******************************/
+
+void running_print(){
+    
+    xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log_number, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
+    xbee.printf("moter command: %c\n", cansat.motor_command);
+    xbee.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_angle(), cansat.get_pressure());
+
+    log_number++;
+    
+}
+
 void running(){
+    
     double r = 6378.137;
     double y1 = cansat.get_target_y();
     double y2 = cansat.get_robot_y();
     double x1 = cansat.get_target_x();
     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_angle(calc_angle(robot_compass(x1-x2, y1-y2)));
     
@@ -369,7 +386,8 @@
             //ひっくり返っている
             cansat.control_Motor(0, cansat.get_speed());
         } else {
-            switch(robot_Action(cansat.get_robot_angle(), cansat.get_target_angle())) {
+            cansat.motor_command = robot_Action(cansat.get_robot_angle(), cansat.get_target_angle());
+            switch(cansat.motor_command) {
                 case 'f': //前進
                     cansat.control_Motor(0, cansat.get_speed());
                     break;
@@ -387,7 +405,11 @@
     }
     if(cansat.get_target_distance() <= 1){
         mode = 100;
+        xbee.printf("change mode: stopping\n");
+        xbee.printf("it is goal point.\n");
     }
+    
+    running_print();
 }
 
 /******************************
@@ -420,6 +442,8 @@
     Adafruit_GPS myGPS(gps_Serial); 
     Timer refresh_Timer;
     const int refresh_Time = 1000; //refresh time in ms
+    Timer running_Timer;
+    const int running_Time = 500;
     int count = 0;
 
     myGPS.begin(GPS_BAUD_RATE); 
@@ -434,7 +458,7 @@
     //interrupt start
     refresh_Timer.start();
     compass_Timer.start();
-    
+    running_Timer.start(); 
     cansat.set_target(target_x, target_y);
    // wait_ms(10000);
 
@@ -446,10 +470,9 @@
     short_out = 1; //ショートピンの出力:high
     nic.output();
     nic = 0;
-    int log = 0;
     
     while (true) {
-          
+
         switch(mode){
             //スタートモード：パラシュートが開くまではこのモードを実行
             case -1:
@@ -461,7 +484,10 @@
             break;
             //走行モード：ターゲットにむかって走行を行う
             case 2:
-                running();
+                if (running_Timer.read_ms() >= running_Time) {
+                    running_Timer.reset();
+                    running();
+                }
             break;
             //停止モード：ターゲット
             case 100:
@@ -495,10 +521,11 @@
             refresh_Timer.reset();
             //print_gps(count);
             Get_GPS(&myGPS);
-            log++;
-            xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
+            //log++;
+ /*           xbee.printf("%d times, x:%f, y:%f, speed:%d\n", log, cansat.get_robot_x(), cansat.get_robot_y(), cansat.get_speed());
             xbee.printf("moter command: %c\n", cansat.motor_command);
             xbee.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_angle(), cansat.get_pressure());
+ */
         }
     }