Revision 21:8799ee63c2cd, committed 2012-12-13

Comitter:

tylerjw

Date:

Thu Dec 13 18:27:41 2012 +0000

Parent:

19:1cfe22ef30e2

Child:

22:becb67846755

Commit message:

working route finding

Changed in this revision

--- a/main.cpp	Thu Dec 13 06:54:16 2012 +0000
+++ b/main.cpp	Thu Dec 13 18:27:41 2012 +0000
@@ -8,13 +8,13 @@
 #define WAIT_FOR_LOCK   1 // set to 1 to not open log file until gps lock
 #define UNLOCK_ON_FALL  0 // set to 1 to not signal parachute untill falling
 
-const float target_lat = 39.63186;    // for setting the target location!
-const float target_lon = -105.028206;
+const float target_lat = 39.920936;    // for setting the target location!
+const float target_lon = -105.009991;
 
 const float course_fudge = 5.0; // if -course_fudge < course > course_fudge then don't turn
 const float neg_course_fudge = -5.0;
 
-const float distance_fudge_km = 0.1; // stop turning if within distance_fudge km
+const float distance_fudge_km = 0.05; // stop turning if within distance_fudge km
 
 I2C i2c(p9, p10); // sda, scl
 BMP085 alt_sensor(i2c);
@@ -49,12 +49,12 @@
     float alt, alt_prev;
     alt = alt_prev = 0;
 
-    DigitalOut gps_led(LED4);
+    //DigitalOut gps_led(LED4);
 
     gps.baud(4800);
 
     while(true) {
-        gps_led = !gps_led;
+        //gps_led = !gps_led;
         gps.read_line(buffer);
         int line_type = nmea.parse(buffer);
         //pc.puts(buffer);
@@ -166,26 +166,37 @@
 
 void parachute_thread(const void *args)
 {
-    DigitalOut para_led(LED1);
+    DigitalOut left_turn(LED4);
+    DigitalOut right_turn(LED1);
 
-    para_led = !para_led;
+    right_turn = 1;
+    Thread::wait(400);
+    left_turn = 1;
+    Thread::wait(400);
     while(true) {
+        right_turn = left_turn = 0;
         parachute_sem.wait();
-        para_led = !para_led;
+        
+        float distance = nmea.calc_dist_to_km(target_lat, target_lon);
 
-        if(nmea.calc_dist_to_km(target_lat, target_lon) < distance_fudge_km)
+        if(distance < distance_fudge_km)
             continue; // dont do anything
 
         float course = nmea.get_course_d();
         float course_to = nmea.calc_course_to(target_lat, target_lon);
         float course_diff = course_to - course;
 
-        if(course_diff < course_fudge && course_diff > neg_course_fudge)
+        if(course_diff < course_fudge && course_diff > neg_course_fudge) {
+            right_turn = left_turn = 1;
+            Thread::wait(400);
             continue; // don't do anything
-        else if(course_diff > 180.0 || course_diff < 0.0)
-            Thread::wait(200); // turn right
-        else
-            Thread::wait(200); // turn left
+        } else if(course_diff > 180.0 || course_diff < 0.0) {
+            right_turn = 1;
+            Thread::wait(400); // turn right
+        } else {
+            left_turn = 1;
+            Thread::wait(400); // turn left
+        }
     }
 }