HARP: High Altitude Recovery Payload

File content as of revision 6:2bd373ba18ae:

/* 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"

// 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 };

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;

/**
* 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_xbee *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) {
    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
    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);
        }
        wdt.kick();
    }
}