main.cpp

00001 /* Copyright (c) 2012 Tyler Weaver, MIT License
00002  *
00003  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00004  * and associated documentation files (the "Software"), to deal in the Software without restriction,
00005  * including without limitation the rights to use, copy, modify, merge, publish, distribute,
00006  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
00007  * furnished to do so, subject to the following conditions:
00008  *
00009  * The above copyright notice and this permission notice shall be included in all copies or
00010  * substantial portions of the Software.
00011  *
00012  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00013  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00014  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017  */
00018 
00019 #include "mbed.h"
00020 #include "rtos.h"
00021 #include "watchdog.h"
00022 #include "Servo.h"
00023 #include "ITG3200.h"
00024 
00025 #define CENTER  'C'
00026 #define LEFT    'L'
00027 #define RIGHT   'R'
00028 
00029 // Setup the watchdog timer
00030 Watchdog wdt;
00031 
00032 // status leds
00033 BusOut status_led(LED4, LED3, LED2, LED1);
00034 
00035 typedef struct {
00036     char msg;   // the direction to turn in
00037 } messageXbeeT;
00038 
00039 MemoryPool<messageXbeeT, 16> mpool_xbee;
00040 Queue<messageXbeeT, 16> queue_xbee;
00041 
00042 Mutex direction_mutex;
00043 char direction = CENTER;
00044 
00045 /**
00046 * xbee_thread
00047 * this thread reads characters from the xbee serial connection and posts messages containing them
00048 */
00049 void xbee_thread(void const *argument)
00050 {
00051     // xbee serial connection
00052     Serial xbee(p9,p10);
00053     xbee.baud(9600);
00054     while (true) {
00055         if (xbee.readable()) {
00056             messageXbeeT *message = mpool_xbee.alloc();
00057             message->msg = xbee.getc();
00058 
00059             queue_xbee.put(message);
00060         }
00061         Thread::wait(100);
00062     }
00063 }
00064 
00065 /**
00066 parachute_thread
00067 this thread recieves messages from the main thread and turns the servos for control of the parachute
00068 */
00069 void parachute_thread(void const *argument)
00070 {
00071     // servos
00072     Servo left_s(p21);
00073     Servo right_s(p22);
00074 
00075     float left = 0;
00076     float right = 0;
00077 
00078     left_s.calibrate_max(0.0007);
00079     left_s.calibrate_min(-0.0014);
00080     right_s.calibrate(0.0009);
00081 
00082     for(float i = 0.0; i <= 1.0; i+=0.1) {
00083         left_s = i;
00084         right_s = i;
00085         Thread::wait(100);
00086     }
00087     right_s = left;
00088     left_s = right;
00089     
00090     bool new_message = false;
00091 
00092     char state = CENTER;
00093     
00094     while (true) {
00095         osEvent evt_xbee = queue_xbee.get(20); // 20 millisecond wait
00096         if (evt_xbee.status == osEventMessage) {
00097             messageXbeeT *message = (messageXbeeT*)evt_xbee.value.p;
00098             state = message->msg;
00099             new_message = true;
00100         }
00101         switch (state) {
00102             case CENTER: // center
00103                 status_led = 0x6;
00104                 direction_mutex.lock();
00105                 if(new_message)
00106                     left = right = 0;
00107                 if(direction != CENTER) {
00108                     if(direction == LEFT) {
00109                         right += 0.02;
00110                         left -= 0.02;
00111                     } else { // right
00112                         right -= 0.02;
00113                         left += 0.02;
00114                     }
00115                 }
00116                 direction_mutex.unlock();
00117                 break;
00118             case LEFT: // left
00119                 status_led = 0x3;
00120                 direction_mutex.lock();
00121                 if(direction != LEFT) {
00122                     left += 0.02;
00123                     right = 0;
00124                 }
00125                 direction_mutex.unlock();
00126                 break;
00127             case RIGHT: // right
00128                 status_led = 0xC;
00129                 direction_mutex.lock();
00130                 if(direction != RIGHT) {
00131                     right += 0.02;
00132                     left = 0;
00133                 }
00134                 direction_mutex.unlock();
00135                 break;
00136         }
00137         if(left > 1.0) left = 1.0;
00138         if(left < 0.0) left = 0.0;
00139         if(right > 1.0) right = 1.0;
00140         if(right < 0.0) right = 0.0;
00141         right_s = right;
00142         left_s = left;
00143         new_message = false;
00144     }
00145 
00146 }
00147 
00148 /**
00149 sensor thread
00150 for sampling from sensors
00151 */
00152 
00153 void sensor_thread(void const* arg)
00154 {
00155     Serial pc(USBTX, USBRX);
00156     pc.baud(9600);
00157     
00158     AnalogIn battery(p19);
00159     DigitalOut battery_warning(p24);
00160     battery_warning = 1;
00161 
00162     const float BAT_MUL = 10.26;
00163     float battery_voltage;
00164 
00165     pc.puts("\n\rInitalizing Gyro, hold still.. ");
00166     ITG3200 gyro(p28, p27); // sda, scl - gyro
00167     gyro.setLpBandwidth(LPFBW_5HZ);
00168     gyro.calibrate(0.5);
00169     double gyro_readings[3];
00170     pc.puts("OK\n\r");
00171 
00172     while(1) {
00173         gyro.getGyroXYZDegrees(gyro_readings);
00174         pc.printf("\r\n%f,%f,%f \r\n", gyro_readings[0], gyro_readings[1], gyro_readings[2]);
00175 
00176         direction_mutex.lock();
00177         if(gyro_readings[2] > 10.0)
00178             direction = LEFT;
00179         else if (gyro_readings[2] < -10.0)
00180             direction = RIGHT;
00181         else
00182             direction = CENTER;
00183         pc.putc(direction);
00184         direction_mutex.unlock();
00185         
00186         battery_voltage = battery.read() * BAT_MUL;
00187         if(battery_voltage < 6.4)
00188             battery_warning = 0;
00189             
00190         Thread::wait(100);
00191     }
00192 }
00193 
00194 /**
00195 main thread
00196 this thread initializes everything then recieves messages from the xbee and sends messages to the parachute
00197 */
00198 int main (void)
00199 {
00200     status_led = 0x9;
00201     // setup watchdog
00202     wdt.kick(2.0); // 2 second watchdog
00203     // setup xbee serial
00204 
00205     Thread thread1(xbee_thread);
00206     Thread thread2(parachute_thread);
00207     Thread thread3(sensor_thread);
00208 
00209     while (true) {
00210         Thread::wait(500);
00211         wdt.kick();
00212     }
00213 }