HARP: High Altitude Recovery Payload

Diff: main.cpp

Revision:

7:fcbf263a62b9

Parent:

6:2bd373ba18ae

Child:

8:513b31554e5f

--- a/main.cpp	Tue Aug 07 16:03:45 2012 +0000
+++ b/main.cpp	Mon Oct 15 22:07:07 2012 +0000
@@ -16,44 +16,40 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
-
 #include "mbed.h"
 #include "rtos.h"
 #include "watchdog.h"
+#include "Servo.h"
+
+#define CENTER  'C'
+#define LEFT    'L'
+#define RIGHT   'R'
 
 // Setup the watchdog timer
 Watchdog wdt;
 
-// xbee serial connection
-Serial xbee(p13,p14);
-
 // status leds
 BusOut status_led(LED4, LED3, LED2, LED1);
 
 typedef struct {
     char msg;   // the direction to turn in
-} message_xbee;
-
-enum state_p { LEFT, CENTER, RIGHT };
+} messageXbeeT;
 
-typedef struct {
-    state_p state;
-} message_para;
-
-MemoryPool<message_para, 16> mpool_para;
-Queue<message_para, 16> queue_para;
-
-MemoryPool<message_xbee, 16> mpool_xbee;
-Queue<message_xbee, 16> queue_xbee;
+MemoryPool<messageXbeeT, 16> mpool_xbee;
+Queue<messageXbeeT, 16> queue_xbee;
 
 /**
 * xbee_thread
 * this thread reads characters from the xbee serial connection and posts messages containing them
 */
-void xbee_thread(void const *argument) {
+void xbee_thread(void const *argument)
+{
+    // xbee serial connection
+    Serial xbee(p9,p10);
+    xbee.baud(9600);
     while (true) {
         if (xbee.readable()) {
-            message_xbee *message = mpool_xbee.alloc();
+            messageXbeeT *message = mpool_xbee.alloc();
             message->msg = xbee.getc();
 
             queue_xbee.put(message);
@@ -66,84 +62,57 @@
 parachute_thread
 this thread recieves messages from the main thread and turns the servos for control of the parachute
 */
-void parachute_thread(void const *argument) {
-    PwmOut left(p21);
-    PwmOut right(p22);
-    left.period(0.020);
-    right.period(0.020);
+void parachute_thread(void const *argument)
+{
+    Serial pc(USBTX, USBRX);
+    pc.baud(9600);
+    
+    pc.puts("\n\rCalibrating Servo Motors... ");
+    // servos
+    Servo left_s(p21);
+    Servo right_s(p22);
 
-    const float WIND = 0.001;
-    const float STOP = 0.0015;
-    const float UNWIND = 0.002;
-    const int HOLD = 500;
-
-    left.pulsewidth(STOP); // STOP
-    right.pulsewidth(STOP);
-
-    state_p state = CENTER;
-    state_p oldState = CENTER;
+    left_s.calibrate_max(0.0014);
+    left_s.calibrate_min(-0.0008);
+    right_s.calibrate_max(0.0016);
+    right_s.calibrate_min(-0.0008);
 
-    //initalize
-    // left
-    left.pulsewidth(WIND);
-    Thread::wait(HOLD);
-    left.pulsewidth(STOP);
-    Thread::wait(HOLD);
-    left.pulsewidth(UNWIND);
-    Thread::wait(HOLD);
-    left.pulsewidth(STOP);
-    right.pulsewidth(WIND);
-    Thread::wait(HOLD);
-    right.pulsewidth(STOP);
-    Thread::wait(HOLD);
-    right.pulsewidth(UNWIND);
-    Thread::wait(HOLD);
-    right.pulsewidth(STOP);
+    for(float i = 0.0; i <= 1.0; i+=0.1) {
+        left_s = i;
+        right_s = i;
+        //release = i;
+        Thread::wait(500);
+    }
+    right_s = 0;
+    left_s = 0;
+    pc.puts("OK!");
 
+    char state = CENTER;
+    pc.puts("\n\rEntering Main Loop... ");
     while (true) {
-        osEvent evt_para = queue_para.get(10); // 10 millisecond wait
-        if (evt_para.status == osEventMessage) {
-            message_para *message = (message_para*)evt_para.value.p;
-            printf("\nMessage for para: %d\n\r", message->state);
-            oldState = state;
-            state = message->state;
+        osEvent evt_xbee = queue_xbee.get(1); // 20 millisecond wait
+        if (evt_xbee.status == osEventMessage) {
+            messageXbeeT *message = (messageXbeeT*)evt_xbee.value.p;
+            state = message->msg;
+            pc.puts("\n\rMessage recieved!");
         }
         switch (state) {
-            case CENTER: // HOLD
-                if (oldState == LEFT) { // previous left turn
-                    left.pulsewidth(UNWIND); // wind forward
-                    Thread::wait(HOLD);
-                } else if (oldState == RIGHT) { // previous right turn
-                    right.pulsewidth(UNWIND); // wind forward
-                    Thread::wait(HOLD);
-                }
+            case CENTER: // center
+                status_led = 0x6;
+                right_s = 0;
+                left_s = 0;
                 break;
             case LEFT: // left
-                if (oldState == RIGHT) { // previous right turn
-                    right.pulsewidth(UNWIND); // wind forward
-                    Thread::wait(HOLD);
-                    right.pulsewidth(STOP); // HOLD
-                } else if (oldState == LEFT) // previous left
-                    break;
-                //initiate left turn
-                left.pulsewidth(WIND); // wind back
-                Thread::wait(HOLD); // 0.5 second turn (TODO: compass feedback)
+                status_led = 0x3;
+                right_s = 0;
+                left_s = 1;
                 break;
             case RIGHT: // right
-                if (oldState == LEFT) { // previous left turn
-                    left.pulsewidth(UNWIND); // wind forward
-                    Thread::wait(HOLD);
-                    left.pulsewidth(STOP); // HOLD
-                } else if (oldState == RIGHT) // previous right turn
-                    break;
-                //initiate right turn
-                right.pulsewidth(WIND); // wind back
-                Thread::wait(HOLD); // 0.5 second turn (TODO: compass feedback)
+                status_led = 0xC;
+                right_s = 1;
+                left_s = 0;
                 break;
         }
-        oldState = state;
-        right.pulsewidth(STOP);
-        left.pulsewidth(STOP);
     }
 
 }
@@ -152,48 +121,29 @@
 main thread
 this thread initializes everything then recieves messages from the xbee and sends messages to the parachute
 */
-int main (void) {
+int main (void)
+{
+    AnalogIn battery(p19);
+    DigitalOut battery_warning(p24);
+    battery_warning = 1;
+    
+    const float BAT_MUL = 10.26;
+    float battery_voltage;
+    
     status_led = 0x9;
     // setup watchdog
     wdt.kick(2.0); // 2 second watchdog
     // setup xbee serial
-    xbee.baud(9600);
 
     Thread thread1(xbee_thread);
     Thread thread2(parachute_thread);
 
     while (true) {
-
-        osEvent evt_xbee = queue_xbee.get(1000); // wait for 1 second
-
-        if (evt_xbee.status == osEventMessage) {
-            message_xbee *message_x = (message_xbee*)evt_xbee.value.p;
-            printf("\nMessage from xbee: %c\n\r", message_x->msg);
-
-            message_para *message_p = mpool_para.alloc();
-
-            switch (message_x->msg) {
-                case 'L': // turn left
-                    status_led = 0x3;
-                    message_p->state = LEFT;
-                    break;
-                case 'C': // center
-                    status_led = 0x6;
-                    message_p->state = CENTER;
-                    break;
-                case 'R': // turn right
-                    status_led = 0xC;
-                    message_p->state = RIGHT;
-                    break;
-                case 'F': // flare
-                    status_led = 0xF;
-                    message_p->state = CENTER;
-                    break;
-            }
-
-            queue_para.put(message_p);
-            mpool_xbee.free(message_x);
-        }
+        battery_voltage = battery.read() * BAT_MUL;
+        if(battery_voltage < 6.4)
+            battery_warning = 0;
+            
+        Thread::wait(1000);
         wdt.kick();
     }
 }