HARP: High Altitude Recovery Payload

Diff: main.cpp

Revision:

8:513b31554e5f

Parent:

7:fcbf263a62b9

--- a/main.cpp	Mon Oct 15 22:07:07 2012 +0000
+++ b/main.cpp	Tue Oct 16 03:11:34 2012 +0000
@@ -1,149 +1,213 @@
-/* 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
- * furnished to do so, subject to the following conditions:
- *
- * 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,
- * 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;
-
-// status leds
-BusOut status_led(LED4, LED3, LED2, LED1);
-
-typedef struct {
-    char msg;   // the direction to turn in
-} messageXbeeT;
-
-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)
-{
-    // xbee serial connection
-    Serial xbee(p9,p10);
-    xbee.baud(9600);
-    while (true) {
-        if (xbee.readable()) {
-            messageXbeeT *message = mpool_xbee.alloc();
-            message->msg = xbee.getc();
-
-            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)
-{
-    Serial pc(USBTX, USBRX);
-    pc.baud(9600);
-    
-    pc.puts("\n\rCalibrating Servo Motors... ");
-    // servos
-    Servo left_s(p21);
-    Servo right_s(p22);
-
-    left_s.calibrate_max(0.0014);
-    left_s.calibrate_min(-0.0008);
-    right_s.calibrate_max(0.0016);
-    right_s.calibrate_min(-0.0008);
-
-    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_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: // center
-                status_led = 0x6;
-                right_s = 0;
-                left_s = 0;
-                break;
-            case LEFT: // left
-                status_led = 0x3;
-                right_s = 0;
-                left_s = 1;
-                break;
-            case RIGHT: // right
-                status_led = 0xC;
-                right_s = 1;
-                left_s = 0;
-                break;
-        }
-    }
-
-}
-
-/**
-main thread
-this thread initializes everything then recieves messages from the xbee and sends messages to the parachute
-*/
-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
-
-    Thread thread1(xbee_thread);
-    Thread thread2(parachute_thread);
-
-    while (true) {
-        battery_voltage = battery.read() * BAT_MUL;
-        if(battery_voltage < 6.4)
-            battery_warning = 0;
-            
-        Thread::wait(1000);
-        wdt.kick();
-    }
-}
+/* 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
+ * furnished to do so, subject to the following conditions:
+ *
+ * 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,
+ * 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"
+#include "ITG3200.h"
+
+#define CENTER  'C'
+#define LEFT    'L'
+#define RIGHT   'R'
+
+// Setup the watchdog timer
+Watchdog wdt;
+
+// status leds
+BusOut status_led(LED4, LED3, LED2, LED1);
+
+typedef struct {
+    char msg;   // the direction to turn in
+} messageXbeeT;
+
+MemoryPool<messageXbeeT, 16> mpool_xbee;
+Queue<messageXbeeT, 16> queue_xbee;
+
+Mutex direction_mutex;
+char direction = CENTER;
+
+/**
+* xbee_thread
+* this thread reads characters from the xbee serial connection and posts messages containing them
+*/
+void xbee_thread(void const *argument)
+{
+    // xbee serial connection
+    Serial xbee(p9,p10);
+    xbee.baud(9600);
+    while (true) {
+        if (xbee.readable()) {
+            messageXbeeT *message = mpool_xbee.alloc();
+            message->msg = xbee.getc();
+
+            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)
+{
+    // servos
+    Servo left_s(p21);
+    Servo right_s(p22);
+
+    float left = 0;
+    float right = 0;
+
+    left_s.calibrate_max(0.0007);
+    left_s.calibrate_min(-0.0014);
+    right_s.calibrate(0.0009);
+
+    for(float i = 0.0; i <= 1.0; i+=0.1) {
+        left_s = i;
+        right_s = i;
+        Thread::wait(100);
+    }
+    right_s = left;
+    left_s = right;
+    
+    bool new_message = false;
+
+    char state = CENTER;
+    
+    while (true) {
+        osEvent evt_xbee = queue_xbee.get(20); // 20 millisecond wait
+        if (evt_xbee.status == osEventMessage) {
+            messageXbeeT *message = (messageXbeeT*)evt_xbee.value.p;
+            state = message->msg;
+            new_message = true;
+        }
+        switch (state) {
+            case CENTER: // center
+                status_led = 0x6;
+                direction_mutex.lock();
+                if(new_message)
+                    left = right = 0;
+                if(direction != CENTER) {
+                    if(direction == LEFT) {
+                        right += 0.02;
+                        left -= 0.02;
+                    } else { // right
+                        right -= 0.02;
+                        left += 0.02;
+                    }
+                }
+                direction_mutex.unlock();
+                break;
+            case LEFT: // left
+                status_led = 0x3;
+                direction_mutex.lock();
+                if(direction != LEFT) {
+                    left += 0.02;
+                    right = 0;
+                }
+                direction_mutex.unlock();
+                break;
+            case RIGHT: // right
+                status_led = 0xC;
+                direction_mutex.lock();
+                if(direction != RIGHT) {
+                    right += 0.02;
+                    left = 0;
+                }
+                direction_mutex.unlock();
+                break;
+        }
+        if(left > 1.0) left = 1.0;
+        if(left < 0.0) left = 0.0;
+        if(right > 1.0) right = 1.0;
+        if(right < 0.0) right = 0.0;
+        right_s = right;
+        left_s = left;
+        new_message = false;
+    }
+
+}
+
+/**
+sensor thread
+for sampling from sensors
+*/
+
+void sensor_thread(void const* arg)
+{
+    Serial pc(USBTX, USBRX);
+    pc.baud(9600);
+    
+    AnalogIn battery(p19);
+    DigitalOut battery_warning(p24);
+    battery_warning = 1;
+
+    const float BAT_MUL = 10.26;
+    float battery_voltage;
+
+    pc.puts("\n\rInitalizing Gyro, hold still.. ");
+    ITG3200 gyro(p28, p27); // sda, scl - gyro
+    gyro.setLpBandwidth(LPFBW_5HZ);
+    gyro.calibrate(0.5);
+    double gyro_readings[3];
+    pc.puts("OK\n\r");
+
+    while(1) {
+        gyro.getGyroXYZDegrees(gyro_readings);
+        pc.printf("\r\n%f,%f,%f \r\n", gyro_readings[0], gyro_readings[1], gyro_readings[2]);
+
+        direction_mutex.lock();
+        if(gyro_readings[2] > 10.0)
+            direction = LEFT;
+        else if (gyro_readings[2] < -10.0)
+            direction = RIGHT;
+        else
+            direction = CENTER;
+        pc.putc(direction);
+        direction_mutex.unlock();
+        
+        battery_voltage = battery.read() * BAT_MUL;
+        if(battery_voltage < 6.4)
+            battery_warning = 0;
+            
+        Thread::wait(100);
+    }
+}
+
+/**
+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
+    wdt.kick(2.0); // 2 second watchdog
+    // setup xbee serial
+
+    Thread thread1(xbee_thread);
+    Thread thread2(parachute_thread);
+    Thread thread3(sensor_thread);
+
+    while (true) {
+        Thread::wait(500);
+        wdt.kick();
+    }
+}