Diff: main.cpp

Revision:

4:40fe9bdb34fc

Parent:

3:3d5f5e6ef385

--- a/main.cpp	Thu Jul 28 07:08:19 2016 +0000
+++ b/main.cpp	Sat Aug 27 01:53:55 2016 +0000
@@ -5,8 +5,8 @@
 #include "MBed_Adafruit_GPS.h"
 
 // target GPS
-const double target_x = 139.939062, target_y = 37.522337;
-
+//const double target_x = 139.988727, target_y = 40.142708;
+const double target_x = 139.987270, target_y = 40.142732;
 //******************************************
 //
 //  pin setting
@@ -18,7 +18,8 @@
 Serial xbee(p13,p14);
 
 // Compass
-DigitalIn compass[4] = {p12, p11, p8, p7};
+DigitalIn compass[4] = {p7, p8, p11, p12};
+//DigitalIn compass[4] = {p12, p11, p8, p7};
 // Pressure
 BME280 pressure(p9, p10);
 // MOtor
@@ -26,7 +27,7 @@
 
 DigitalIn short_in(p19);
 DigitalOut short_out(p20);
-DigitalInOut nic(p5);
+DigitalOut nic(p5);
 
 
 //******************************************
@@ -51,6 +52,8 @@
 int running_flag = 0;
 // stuck
 int stuck_flag = 0;
+// stop
+int stop_count = 0;
 
 // previous distance and ronot compass
 double d=0.0;
@@ -60,9 +63,11 @@
 Timer GPS_Timer;
 const int GPS_time = 1000;
 Timer fall_Timer;
-const int fall_time = 3000;
+const int fall_time = 30000;
 Timer action_Timer;
 const int action_time = 500;
+Timer stop_Timer;
+const int stop_time = 30000;
 Timer stuck_Timer;
 const int stuck_time = 5000;
 Timer back_Timer;
@@ -116,6 +121,7 @@
         xbee.printf("remove the parachute\n");
         wait_ms(3000);
         nic = 0;
+        wait_ms(3000);
         mode = 2;
         action_Timer.start();
         xbee.printf("-----------RUNNING MODE-----------\n");
@@ -160,10 +166,12 @@
         // finish a forward running
         if(running_flag == 20) {
             cansat.set_speed(128);
+            stop_Timer.start();
             stuck_Timer.start();
             back_Timer.start();
         }
 
+        // if stuck
         if(stuck_flag == 1) {
             if(back_Timer.read_ms() <= 10000) {
                 if(back_Timer.read_ms()<= back_time) {
@@ -182,19 +190,22 @@
 
     // around the target
     if(cansat.get_distance() < 0.5) {
+        cansat.set_action('s');
+        //stop_count ++;
+    } else {
+        stop_Timer.reset();
+    }
+
+    if(stop_Timer.read_ms() >= stop_time) {
         mode = 100;
         xbee.printf("-----------STOP MODE-----------\n");
     }
 
-
     if(stuck_flag != 1) {
         // stuck check
         if(!(d-0.15 <= cansat.get_distance() && cansat.get_distance() <= d+0.15)) {
             stuck_Timer.reset();
         }
-        /*if(c != cansat.get_compass('r')) {
-            stuck_Timer.reset();
-        }*/
 
         // stuck
         if(stuck_Timer.read_ms() >= stuck_time) {
@@ -242,6 +253,8 @@
     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
     myGPS.sendCommand(PGCMD_ANTENNA);
 
+    wait_ms(1000);
+
     GPS_Timer.start();
 
     // target GPS
@@ -253,7 +266,7 @@
     nic = 0;
 
 
-    xbee.getc();
+    //xbee.getc();
     xbee.printf("start\n");
     xbee.printf("-----------STANDBY MODE-----------\n");
     cansat.set_action('s');