Diff: main.cpp

Revision:

28:032d55fa57b8

Parent:

27:24fd8e32511c

Child:

29:8cdb56a0fe57

--- a/main.cpp	Thu Jan 10 19:20:18 2013 +0000
+++ b/main.cpp	Thu Jan 10 19:25:13 2013 +0000
@@ -39,6 +39,9 @@
 MemoryPool<sensor_line, 16> mpool_sensor_line;
 Queue<sensor_line, 16> queue_sensor_line;
 
+volatile float left_pos = 0.0; // servo position for logging
+volatile float right_pos = 0.0;
+
 void gps_thread(void const *args)
 {
     char buffer[80];
@@ -138,7 +141,7 @@
     queue_sensor_line.put(message);
     
     message = mpool_sensor_line.alloc();
-    sprintf(message->line, ",Temperature,GPS Altitude,GPS Course,Course to Target,Distance \r\n");
+    sprintf(message->line, ",Temperature,GPS Altitude,GPS Course,Course to Target,Distance,Left Servo,Right Servo \r\n");
     queue_sensor_line.put(message);
 
     while(true) {
@@ -163,7 +166,7 @@
         course_to = nmea.calc_initial_bearing(target_lat, target_lon);
         course = nmea.track();
 
-        sprintf(message->line, "%f,%f,%f,%f,%f,%f,%f,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft(),course,course_to,distance);
+        sprintf(message->line, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", time,gps_battery_voltage,mbed_battery_voltage,temp,nmea.calc_altitude_ft(),course,course_to,distance,left_pos,right_pos);
         queue_sensor_line.put(message);
         
         Thread::wait(100);
@@ -172,8 +175,8 @@
 
 void parachute_thread(const void *args)
 {
-    DigitalOut left_turn(LED4);
-    DigitalOut right_turn(LED1);
+    DigitalOut left_turn_led(LED4);
+    DigitalOut right_turn_led(LED1);
     
     // servos ////////////////////////// NOT TESTED!!! ///////////////////////////
     Servo left_s(p21);
@@ -187,12 +190,12 @@
     right_s = 0.0;
     ////////////////////////////////////////////////////////////////////////////////
 
-    right_turn = 1;
+    right_turn_led = 1;
     Thread::wait(400);
-    left_turn = 1;
+    left_turn_led = 1;
     Thread::wait(400);
     while(true) {
-        right_turn = left_turn = 0;
+        right_turn_led = left_turn_led = 0;
         parachute_sem.wait();
 
         float distance = nmea.calc_dist_to_km(target_lat, target_lon);
@@ -208,21 +211,21 @@
             continue; // do nothing
 
         if(course_diff < course_fudge && course_diff > neg_course_fudge) {
-            right_turn = left_turn = 1;
+            right_turn_led = left_turn_led = 1;
             Thread::wait(400);
             continue; // don't do anything
         } else if(course_diff > 180.0 || course_diff < 0.0) {
-            left_turn = 1;
-            right_s = 0.0;                                      // NOT TESTED!!!
-            left_s = 1.0;
+            left_turn_led = 1;
+            right_s = right_pos = 0.0;                                      // NOT TESTED!!!
+            left_s = left_pos = 1.0;
             Thread::wait(400); // turn left
             left_s = 0.0;
         } else {
-            right_turn = 1;
-            left_s = 0.0;
-            right_s = 1.0;
+            right_turn_led = 1;
+            left_s = left_pos = 0.0;
+            right_s = right_pos = 1.0;
             Thread::wait(400); // turn righ
-            right_s = 0.0;
+            right_s = right_pos = 0.0;
         }
     }
 }