Tyler Weaver / Mbed 2 deprecated HARP Featured

High Altitude Recovery Payload

HARP: High Altitude Recovery Payload

Information

Project Notebook Page

Version 0.1: RC design

/media/uploads/tylerjw/_scaled_2012-07-23_mbed_xbee_breadboard.jpg

By connecting the second xbee to a computer using a terminal command and supplying the characters L, R, C, F the light patterns change on the mbed.

main.cpp@8:513b31554e5f, 2012-10-16 (annotated)

Committer:
tylerjw
Date:
Tue Oct 16 03:11:34 2012 +0000
Revision:
8:513b31554e5f
Parent:
7:fcbf263a62b9 
Gyro feedback test implemented - working sensor thread

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tylerjw 8:513b31554e5f 1 /* Copyright (c) 2012 Tyler Weaver, MIT License
tylerjw 8:513b31554e5f 2 *
tylerjw 8:513b31554e5f 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
tylerjw 8:513b31554e5f 4 * and associated documentation files (the "Software"), to deal in the Software without restriction,
tylerjw 8:513b31554e5f 5 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
tylerjw 8:513b31554e5f 6 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
tylerjw 8:513b31554e5f 7 * furnished to do so, subject to the following conditions:
tylerjw 8:513b31554e5f 8 *
tylerjw 8:513b31554e5f 9 * The above copyright notice and this permission notice shall be included in all copies or
tylerjw 8:513b31554e5f 10 * substantial portions of the Software.
tylerjw 8:513b31554e5f 11 *
tylerjw 8:513b31554e5f 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
tylerjw 8:513b31554e5f 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
tylerjw 8:513b31554e5f 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
tylerjw 8:513b31554e5f 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
tylerjw 8:513b31554e5f 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
tylerjw 8:513b31554e5f 17 */
tylerjw 8:513b31554e5f 18
tylerjw 8:513b31554e5f 19 #include "mbed.h"
tylerjw 8:513b31554e5f 20 #include "rtos.h"
tylerjw 8:513b31554e5f 21 #include "watchdog.h"
tylerjw 8:513b31554e5f 22 #include "Servo.h"
tylerjw 8:513b31554e5f 23 #include "ITG3200.h"
tylerjw 8:513b31554e5f 24
tylerjw 8:513b31554e5f 25 #define CENTER 'C'
tylerjw 8:513b31554e5f 26 #define LEFT 'L'
tylerjw 8:513b31554e5f 27 #define RIGHT 'R'
tylerjw 8:513b31554e5f 28
tylerjw 8:513b31554e5f 29 // Setup the watchdog timer
tylerjw 8:513b31554e5f 30 Watchdog wdt;
tylerjw 8:513b31554e5f 31
tylerjw 8:513b31554e5f 32 // status leds
tylerjw 8:513b31554e5f 33 BusOut status_led(LED4, LED3, LED2, LED1);
tylerjw 8:513b31554e5f 34
tylerjw 8:513b31554e5f 35 typedef struct {
tylerjw 8:513b31554e5f 36 char msg; // the direction to turn in
tylerjw 8:513b31554e5f 37 } messageXbeeT;
tylerjw 8:513b31554e5f 38
tylerjw 8:513b31554e5f 39 MemoryPool<messageXbeeT, 16> mpool_xbee;
tylerjw 8:513b31554e5f 40 Queue<messageXbeeT, 16> queue_xbee;
tylerjw 8:513b31554e5f 41
tylerjw 8:513b31554e5f 42 Mutex direction_mutex;
tylerjw 8:513b31554e5f 43 char direction = CENTER;
tylerjw 8:513b31554e5f 44
tylerjw 8:513b31554e5f 45 /**
tylerjw 8:513b31554e5f 46 * xbee_thread
tylerjw 8:513b31554e5f 47 * this thread reads characters from the xbee serial connection and posts messages containing them
tylerjw 8:513b31554e5f 48 */
tylerjw 8:513b31554e5f 49 void xbee_thread(void const *argument)
tylerjw 8:513b31554e5f 50 {
tylerjw 8:513b31554e5f 51 // xbee serial connection
tylerjw 8:513b31554e5f 52 Serial xbee(p9,p10);
tylerjw 8:513b31554e5f 53 xbee.baud(9600);
tylerjw 8:513b31554e5f 54 while (true) {
tylerjw 8:513b31554e5f 55 if (xbee.readable()) {
tylerjw 8:513b31554e5f 56 messageXbeeT *message = mpool_xbee.alloc();
tylerjw 8:513b31554e5f 57 message->msg = xbee.getc();
tylerjw 8:513b31554e5f 58
tylerjw 8:513b31554e5f 59 queue_xbee.put(message);
tylerjw 8:513b31554e5f 60 }
tylerjw 8:513b31554e5f 61 Thread::wait(100);
tylerjw 8:513b31554e5f 62 }
tylerjw 8:513b31554e5f 63 }
tylerjw 8:513b31554e5f 64
tylerjw 8:513b31554e5f 65 /**
tylerjw 8:513b31554e5f 66 parachute_thread
tylerjw 8:513b31554e5f 67 this thread recieves messages from the main thread and turns the servos for control of the parachute
tylerjw 8:513b31554e5f 68 */
tylerjw 8:513b31554e5f 69 void parachute_thread(void const *argument)
tylerjw 8:513b31554e5f 70 {
tylerjw 8:513b31554e5f 71 // servos
tylerjw 8:513b31554e5f 72 Servo left_s(p21);
tylerjw 8:513b31554e5f 73 Servo right_s(p22);
tylerjw 8:513b31554e5f 74
tylerjw 8:513b31554e5f 75 float left = 0;
tylerjw 8:513b31554e5f 76 float right = 0;
tylerjw 8:513b31554e5f 77
tylerjw 8:513b31554e5f 78 left_s.calibrate_max(0.0007);
tylerjw 8:513b31554e5f 79 left_s.calibrate_min(-0.0014);
tylerjw 8:513b31554e5f 80 right_s.calibrate(0.0009);
tylerjw 8:513b31554e5f 81
tylerjw 8:513b31554e5f 82 for(float i = 0.0; i <= 1.0; i+=0.1) {
tylerjw 8:513b31554e5f 83 left_s = i;
tylerjw 8:513b31554e5f 84 right_s = i;
tylerjw 8:513b31554e5f 85 Thread::wait(100);
tylerjw 8:513b31554e5f 86 }
tylerjw 8:513b31554e5f 87 right_s = left;
tylerjw 8:513b31554e5f 88 left_s = right;
tylerjw 8:513b31554e5f 89
tylerjw 8:513b31554e5f 90 bool new_message = false;
tylerjw 8:513b31554e5f 91
tylerjw 8:513b31554e5f 92 char state = CENTER;
tylerjw 8:513b31554e5f 93
tylerjw 8:513b31554e5f 94 while (true) {
tylerjw 8:513b31554e5f 95 osEvent evt_xbee = queue_xbee.get(20); // 20 millisecond wait
tylerjw 8:513b31554e5f 96 if (evt_xbee.status == osEventMessage) {
tylerjw 8:513b31554e5f 97 messageXbeeT *message = (messageXbeeT*)evt_xbee.value.p;
tylerjw 8:513b31554e5f 98 state = message->msg;
tylerjw 8:513b31554e5f 99 new_message = true;
tylerjw 8:513b31554e5f 100 }
tylerjw 8:513b31554e5f 101 switch (state) {
tylerjw 8:513b31554e5f 102 case CENTER: // center
tylerjw 8:513b31554e5f 103 status_led = 0x6;
tylerjw 8:513b31554e5f 104 direction_mutex.lock();
tylerjw 8:513b31554e5f 105 if(new_message)
tylerjw 8:513b31554e5f 106 left = right = 0;
tylerjw 8:513b31554e5f 107 if(direction != CENTER) {
tylerjw 8:513b31554e5f 108 if(direction == LEFT) {
tylerjw 8:513b31554e5f 109 right += 0.02;
tylerjw 8:513b31554e5f 110 left -= 0.02;
tylerjw 8:513b31554e5f 111 } else { // right
tylerjw 8:513b31554e5f 112 right -= 0.02;
tylerjw 8:513b31554e5f 113 left += 0.02;
tylerjw 8:513b31554e5f 114 }
tylerjw 8:513b31554e5f 115 }
tylerjw 8:513b31554e5f 116 direction_mutex.unlock();
tylerjw 8:513b31554e5f 117 break;
tylerjw 8:513b31554e5f 118 case LEFT: // left
tylerjw 8:513b31554e5f 119 status_led = 0x3;
tylerjw 8:513b31554e5f 120 direction_mutex.lock();
tylerjw 8:513b31554e5f 121 if(direction != LEFT) {
tylerjw 8:513b31554e5f 122 left += 0.02;
tylerjw 8:513b31554e5f 123 right = 0;
tylerjw 8:513b31554e5f 124 }
tylerjw 8:513b31554e5f 125 direction_mutex.unlock();
tylerjw 8:513b31554e5f 126 break;
tylerjw 8:513b31554e5f 127 case RIGHT: // right
tylerjw 8:513b31554e5f 128 status_led = 0xC;
tylerjw 8:513b31554e5f 129 direction_mutex.lock();
tylerjw 8:513b31554e5f 130 if(direction != RIGHT) {
tylerjw 8:513b31554e5f 131 right += 0.02;
tylerjw 8:513b31554e5f 132 left = 0;
tylerjw 8:513b31554e5f 133 }
tylerjw 8:513b31554e5f 134 direction_mutex.unlock();
tylerjw 8:513b31554e5f 135 break;
tylerjw 8:513b31554e5f 136 }
tylerjw 8:513b31554e5f 137 if(left > 1.0) left = 1.0;
tylerjw 8:513b31554e5f 138 if(left < 0.0) left = 0.0;
tylerjw 8:513b31554e5f 139 if(right > 1.0) right = 1.0;
tylerjw 8:513b31554e5f 140 if(right < 0.0) right = 0.0;
tylerjw 8:513b31554e5f 141 right_s = right;
tylerjw 8:513b31554e5f 142 left_s = left;
tylerjw 8:513b31554e5f 143 new_message = false;
tylerjw 8:513b31554e5f 144 }
tylerjw 8:513b31554e5f 145
tylerjw 8:513b31554e5f 146 }
tylerjw 8:513b31554e5f 147
tylerjw 8:513b31554e5f 148 /**
tylerjw 8:513b31554e5f 149 sensor thread
tylerjw 8:513b31554e5f 150 for sampling from sensors
tylerjw 8:513b31554e5f 151 */
tylerjw 8:513b31554e5f 152
tylerjw 8:513b31554e5f 153 void sensor_thread(void const* arg)
tylerjw 8:513b31554e5f 154 {
tylerjw 8:513b31554e5f 155 Serial pc(USBTX, USBRX);
tylerjw 8:513b31554e5f 156 pc.baud(9600);
tylerjw 8:513b31554e5f 157
tylerjw 8:513b31554e5f 158 AnalogIn battery(p19);
tylerjw 8:513b31554e5f 159 DigitalOut battery_warning(p24);
tylerjw 8:513b31554e5f 160 battery_warning = 1;
tylerjw 8:513b31554e5f 161
tylerjw 8:513b31554e5f 162 const float BAT_MUL = 10.26;
tylerjw 8:513b31554e5f 163 float battery_voltage;
tylerjw 8:513b31554e5f 164
tylerjw 8:513b31554e5f 165 pc.puts("\n\rInitalizing Gyro, hold still.. ");
tylerjw 8:513b31554e5f 166 ITG3200 gyro(p28, p27); // sda, scl - gyro
tylerjw 8:513b31554e5f 167 gyro.setLpBandwidth(LPFBW_5HZ);
tylerjw 8:513b31554e5f 168 gyro.calibrate(0.5);
tylerjw 8:513b31554e5f 169 double gyro_readings[3];
tylerjw 8:513b31554e5f 170 pc.puts("OK\n\r");
tylerjw 8:513b31554e5f 171
tylerjw 8:513b31554e5f 172 while(1) {
tylerjw 8:513b31554e5f 173 gyro.getGyroXYZDegrees(gyro_readings);
tylerjw 8:513b31554e5f 174 pc.printf("\r\n%f,%f,%f \r\n", gyro_readings[0], gyro_readings[1], gyro_readings[2]);
tylerjw 8:513b31554e5f 175
tylerjw 8:513b31554e5f 176 direction_mutex.lock();
tylerjw 8:513b31554e5f 177 if(gyro_readings[2] > 10.0)
tylerjw 8:513b31554e5f 178 direction = LEFT;
tylerjw 8:513b31554e5f 179 else if (gyro_readings[2] < -10.0)
tylerjw 8:513b31554e5f 180 direction = RIGHT;
tylerjw 8:513b31554e5f 181 else
tylerjw 8:513b31554e5f 182 direction = CENTER;
tylerjw 8:513b31554e5f 183 pc.putc(direction);
tylerjw 8:513b31554e5f 184 direction_mutex.unlock();
tylerjw 8:513b31554e5f 185
tylerjw 8:513b31554e5f 186 battery_voltage = battery.read() * BAT_MUL;
tylerjw 8:513b31554e5f 187 if(battery_voltage < 6.4)
tylerjw 8:513b31554e5f 188 battery_warning = 0;
tylerjw 8:513b31554e5f 189
tylerjw 8:513b31554e5f 190 Thread::wait(100);
tylerjw 8:513b31554e5f 191 }
tylerjw 8:513b31554e5f 192 }
tylerjw 8:513b31554e5f 193
tylerjw 8:513b31554e5f 194 /**
tylerjw 8:513b31554e5f 195 main thread
tylerjw 8:513b31554e5f 196 this thread initializes everything then recieves messages from the xbee and sends messages to the parachute
tylerjw 8:513b31554e5f 197 */
tylerjw 8:513b31554e5f 198 int main (void)
tylerjw 8:513b31554e5f 199 {
tylerjw 8:513b31554e5f 200 status_led = 0x9;
tylerjw 8:513b31554e5f 201 // setup watchdog
tylerjw 8:513b31554e5f 202 wdt.kick(2.0); // 2 second watchdog
tylerjw 8:513b31554e5f 203 // setup xbee serial
tylerjw 8:513b31554e5f 204
tylerjw 8:513b31554e5f 205 Thread thread1(xbee_thread);
tylerjw 8:513b31554e5f 206 Thread thread2(parachute_thread);
tylerjw 8:513b31554e5f 207 Thread thread3(sensor_thread);
tylerjw 8:513b31554e5f 208
tylerjw 8:513b31554e5f 209 while (true) {
tylerjw 8:513b31554e5f 210 Thread::wait(500);
tylerjw 8:513b31554e5f 211 wdt.kick();
tylerjw 8:513b31554e5f 212 }
tylerjw 8:513b31554e5f 213 }