HARP: High Altitude Recovery Payload

Diff: main.cpp

Revision:

6:2bd373ba18ae

Parent:

5:49ccc75efb93

Child:

7:fcbf263a62b9

--- a/main.cpp	Thu Jul 26 22:49:36 2012 +0000
+++ b/main.cpp	Tue Aug 07 16:03:45 2012 +0000
@@ -1,21 +1,21 @@
 /* Copyright (c) 2012 Tyler Weaver, MIT License
  *
- * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
- * and associated documentation files (the "Software"), to deal in the Software without restriction, 
- * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
- * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+ * and associated documentation files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute,
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
  * furnished to do so, subject to the following conditions:
  *
- * The above copyright notice and this permission notice shall be included in all copies or 
+ * The above copyright notice and this permission notice shall be included in all copies or
  * substantial portions of the Software.
  *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
- * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
- * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
- * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
- 
+
 
 #include "mbed.h"
 #include "rtos.h"
@@ -32,23 +32,126 @@
 
 typedef struct {
     char msg;   // the direction to turn in
-} message_p;
+} message_xbee;
+
+enum state_p { LEFT, CENTER, RIGHT };
+
+typedef struct {
+    state_p state;
+} message_para;
 
-MemoryPool<message_p, 16> mpool_p;
-Queue<message_p, 16> queue_p;
+MemoryPool<message_para, 16> mpool_para;
+Queue<message_para, 16> queue_para;
 
+MemoryPool<message_xbee, 16> mpool_xbee;
+Queue<message_xbee, 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) {
     while (true) {
         if (xbee.readable()) {
-            message_p *message = mpool_p.alloc();
+            message_xbee *message = mpool_xbee.alloc();
             message->msg = xbee.getc();
 
-            queue_p.put(message);
+            queue_xbee.put(message);
         }
         Thread::wait(100);
     }
 }
 
+/**
+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);
+
+    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;
+
+    //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);
+
+    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;
+        }
+        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);
+                }
+                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)
+                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)
+                break;
+        }
+        oldState = state;
+        right.pulsewidth(STOP);
+        left.pulsewidth(STOP);
+    }
+
+}
+
+/**
+main thread
+this thread initializes everything then recieves messages from the xbee and sends messages to the parachute
+*/
 int main (void) {
     status_led = 0x9;
     // setup watchdog
@@ -57,30 +160,39 @@
     xbee.baud(9600);
 
     Thread thread1(xbee_thread);
+    Thread thread2(parachute_thread);
 
     while (true) {
 
-        osEvent evt_p = queue_p.get(1000); // wait for 1 second
+        osEvent evt_xbee = queue_xbee.get(1000); // wait for 1 second
 
-        if (evt_p.status == osEventMessage) {
-            message_p *message = (message_p*)evt_p.value.p;
-            printf("\nMessage from xbee: %c\n\r", message->msg);
-            switch (message->msg) {
+        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;
             }
 
-            mpool_p.free(message);
+            queue_para.put(message_p);
+            mpool_xbee.free(message_x);
         }
         wdt.kick();
     }