João Jardim / Mbed 2 deprecated frtos_v_8_Final

Final version of the m3pi software

Dependencies:   mbed m3pi FreeRTOS_V8_2_1_LPC1768

main.cpp@8:167680431cf4, 2018-12-21 (annotated)

Committer:
JoaoJardim
Date:
Fri Dec 21 18:34:21 2018 +0000
Revision:
8:167680431cf4
Parent:
6:00f820418d01 
Final version of the software for the m3pi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
JoaoJardim 8:167680431cf4 1 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 2 // Includes
JoaoJardim 8:167680431cf4 3 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 4
chalikias 0:7f7fa5e631ec 5
chalikias 0:7f7fa5e631ec 6 #include "mbed.h"
chalikias 0:7f7fa5e631ec 7 #include "FreeRTOS.h"
chalikias 0:7f7fa5e631ec 8 #include "task.h"
JoaoJardim 4:33929b1afb80 9 #include "m3pi.h"
JoaoJardim 8:167680431cf4 10 #include "queue.h"
JoaoJardim 8:167680431cf4 11
JoaoJardim 8:167680431cf4 12
JoaoJardim 8:167680431cf4 13 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 14 // Definitions & Initializations
JoaoJardim 8:167680431cf4 15 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 16
JoaoJardim 8:167680431cf4 17
JoaoJardim 8:167680431cf4 18 // Task Priorities
JoaoJardim 8:167680431cf4 19 #define PRIORITY_MAX 4
JoaoJardim 8:167680431cf4 20 #define PRIORITY_HIGH 3
JoaoJardim 8:167680431cf4 21 #define PRIORITY_NORMAL 2
JoaoJardim 8:167680431cf4 22 #define PRIORITY_LOW 1
JoaoJardim 8:167680431cf4 23 #define PRIORITY_IDLE 0
JoaoJardim 8:167680431cf4 24
JoaoJardim 8:167680431cf4 25 #define FORWARD 2
JoaoJardim 8:167680431cf4 26 #define BACKWARD 3
JoaoJardim 8:167680431cf4 27 #define LEFT 4
JoaoJardim 8:167680431cf4 28 #define RIGHT 5
JoaoJardim 8:167680431cf4 29 #define STOP 6
JoaoJardim 8:167680431cf4 30
JoaoJardim 4:33929b1afb80 31
JoaoJardim 4:33929b1afb80 32 m3pi m3pi;
chalikias 0:7f7fa5e631ec 33
chalikias 0:7f7fa5e631ec 34 DigitalOut led1(LED1);
chalikias 0:7f7fa5e631ec 35 DigitalOut led2(LED2);
JoaoJardim 8:167680431cf4 36 DigitalOut led3(LED3);
JoaoJardim 8:167680431cf4 37 DigitalOut led4(LED4);
chalikias 0:7f7fa5e631ec 38
JoaoJardim 6:00f820418d01 39 DigitalOut nRST(p26);
JoaoJardim 4:33929b1afb80 40
JoaoJardim 4:33929b1afb80 41 const portTickType xDelay = 100 / portTICK_RATE_MS;
JoaoJardim 8:167680431cf4 42 const portTickType xDelayMZ = 100 / portTICK_RATE_MS;
JoaoJardim 8:167680431cf4 43 const portTickType xDelayRst = 2000 / portTICK_RATE_MS;
JoaoJardim 8:167680431cf4 44 const portTickType xDelayLF = 1000 / portTICK_RATE_MS;
JoaoJardim 8:167680431cf4 45
JoaoJardim 4:33929b1afb80 46
JoaoJardim 4:33929b1afb80 47 const float fVref = 4.0;
JoaoJardim 4:33929b1afb80 48
JoaoJardim 8:167680431cf4 49 QueueHandle_t xQueue1;
JoaoJardim 4:33929b1afb80 50
JoaoJardim 8:167680431cf4 51
JoaoJardim 8:167680431cf4 52 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 53 // Task Free
JoaoJardim 8:167680431cf4 54 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 55
JoaoJardim 8:167680431cf4 56
JoaoJardim 8:167680431cf4 57 void TaskFree (void* pvParameters) {
JoaoJardim 8:167680431cf4 58
JoaoJardim 8:167680431cf4 59 (void) pvParameters; // Just to stop compiler warnings.
JoaoJardim 8:167680431cf4 60
JoaoJardim 8:167680431cf4 61
JoaoJardim 8:167680431cf4 62 void *pvQBuf = malloc(sizeof(int));
JoaoJardim 8:167680431cf4 63
JoaoJardim 8:167680431cf4 64 for( ;; ) {
JoaoJardim 8:167680431cf4 65 if ( xQueueReceive(xQueue1, pvQBuf, 0) ) {
JoaoJardim 8:167680431cf4 66
JoaoJardim 8:167680431cf4 67 taskENTER_CRITICAL(); // Critical section so that the Scheduler won't interrupt the m3pi commands
JoaoJardim 8:167680431cf4 68 switch(*((int*)pvQBuf)) {
JoaoJardim 8:167680431cf4 69 case FORWARD:
JoaoJardim 8:167680431cf4 70 m3pi.forward(0.2);
JoaoJardim 8:167680431cf4 71 break;
JoaoJardim 8:167680431cf4 72
JoaoJardim 8:167680431cf4 73 case BACKWARD:
JoaoJardim 8:167680431cf4 74 m3pi.backward(0.2);
JoaoJardim 8:167680431cf4 75 break;
JoaoJardim 8:167680431cf4 76
JoaoJardim 8:167680431cf4 77 case LEFT:
JoaoJardim 8:167680431cf4 78 m3pi.left(0.2);
JoaoJardim 8:167680431cf4 79 break;
JoaoJardim 8:167680431cf4 80
JoaoJardim 8:167680431cf4 81 case RIGHT:
JoaoJardim 8:167680431cf4 82 m3pi.right(0.2);
JoaoJardim 8:167680431cf4 83 break;
JoaoJardim 8:167680431cf4 84
JoaoJardim 8:167680431cf4 85 case STOP:
JoaoJardim 8:167680431cf4 86 m3pi.stop();
JoaoJardim 8:167680431cf4 87 break;
JoaoJardim 8:167680431cf4 88 }
JoaoJardim 8:167680431cf4 89 taskEXIT_CRITICAL();
JoaoJardim 8:167680431cf4 90 }
JoaoJardim 8:167680431cf4 91 }
JoaoJardim 8:167680431cf4 92 }
JoaoJardim 8:167680431cf4 93
JoaoJardim 8:167680431cf4 94
JoaoJardim 8:167680431cf4 95 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 96 // Task Maze
JoaoJardim 8:167680431cf4 97 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 98
JoaoJardim 8:167680431cf4 99
JoaoJardim 8:167680431cf4 100 void TaskMZ (void* pvParameters){
JoaoJardim 8:167680431cf4 101
JoaoJardim 8:167680431cf4 102 (void) pvParameters; // Just to stop compiler warnings.
JoaoJardim 8:167680431cf4 103
JoaoJardim 8:167680431cf4 104 led1 = 0;
JoaoJardim 8:167680431cf4 105
JoaoJardim 8:167680431cf4 106 m3pi.reset();
JoaoJardim 8:167680431cf4 107
JoaoJardim 8:167680431cf4 108 vTaskDelay(xDelayRst);
JoaoJardim 8:167680431cf4 109
JoaoJardim 8:167680431cf4 110 float speed = 0.125;
JoaoJardim 8:167680431cf4 111 float correction = 0.125;
JoaoJardim 8:167680431cf4 112 float threshold = 0.5;
JoaoJardim 8:167680431cf4 113
JoaoJardim 8:167680431cf4 114
JoaoJardim 8:167680431cf4 115 float u_turn_t = 0.2; // float u_turn_t = 1.4;
JoaoJardim 8:167680431cf4 116 float u_turn_thresh = 50;
JoaoJardim 8:167680431cf4 117
JoaoJardim 8:167680431cf4 118
JoaoJardim 8:167680431cf4 119
JoaoJardim 8:167680431cf4 120 // These variables record whether the robot has seen a line to the
JoaoJardim 8:167680431cf4 121 // left, straight ahead, and right, while examining the current intersection.
JoaoJardim 8:167680431cf4 122 int sensors[5];
JoaoJardim 8:167680431cf4 123 int foundjunction=0;
JoaoJardim 8:167680431cf4 124
JoaoJardim 8:167680431cf4 125 int threshold_sns = 500;
JoaoJardim 8:167680431cf4 126
JoaoJardim 8:167680431cf4 127 m3pi.locate(0,1);
JoaoJardim 8:167680431cf4 128 m3pi.printf("Maze slv");
JoaoJardim 8:167680431cf4 129
JoaoJardim 8:167680431cf4 130 m3pi.sensor_auto_calibrate();
JoaoJardim 8:167680431cf4 131
JoaoJardim 8:167680431cf4 132 for( ;; ){
JoaoJardim 8:167680431cf4 133 led1 = !led1;
JoaoJardim 8:167680431cf4 134
JoaoJardim 8:167680431cf4 135 // Line Follower ****************************************************************************
JoaoJardim 8:167680431cf4 136
JoaoJardim 8:167680431cf4 137 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
JoaoJardim 8:167680431cf4 138 float position_of_line = m3pi.line_position();
JoaoJardim 8:167680431cf4 139
JoaoJardim 8:167680431cf4 140 // Line is more than the threshold to the right, slow the left motor
JoaoJardim 8:167680431cf4 141 if (position_of_line > threshold) {
JoaoJardim 8:167680431cf4 142 m3pi.right_motor(speed);
JoaoJardim 8:167680431cf4 143 m3pi.left_motor(speed-(position_of_line*correction));
JoaoJardim 8:167680431cf4 144 }
JoaoJardim 8:167680431cf4 145
JoaoJardim 8:167680431cf4 146 // Line is more than 50% to the left, slow the right motor
JoaoJardim 8:167680431cf4 147 else if (position_of_line < -threshold) {
JoaoJardim 8:167680431cf4 148 m3pi.left_motor(speed);
JoaoJardim 8:167680431cf4 149 m3pi.right_motor(speed+(position_of_line*correction));
JoaoJardim 8:167680431cf4 150 }
JoaoJardim 8:167680431cf4 151
JoaoJardim 8:167680431cf4 152 // Line is in the middle
JoaoJardim 8:167680431cf4 153 else {
JoaoJardim 8:167680431cf4 154 m3pi.forward(speed);
JoaoJardim 8:167680431cf4 155 }
JoaoJardim 8:167680431cf4 156
JoaoJardim 8:167680431cf4 157
JoaoJardim 8:167680431cf4 158 // Detect junctions *************************************************************************
JoaoJardim 8:167680431cf4 159
JoaoJardim 8:167680431cf4 160 m3pi.readsensor(sensors); // Read the sensors
JoaoJardim 8:167680431cf4 161
JoaoJardim 8:167680431cf4 162
JoaoJardim 8:167680431cf4 163 if ( (sensors[1]<threshold_sns) && (sensors[2]<threshold_sns) && (sensors[3]<threshold_sns) ){
JoaoJardim 8:167680431cf4 164 // Detects the a dead end and resolves it by "u turning"
JoaoJardim 8:167680431cf4 165
JoaoJardim 8:167680431cf4 166 float a = sensors[1] - sensors[3];
JoaoJardim 8:167680431cf4 167
JoaoJardim 8:167680431cf4 168 while ( (a < u_turn_thresh) && ((-1*a) < u_turn_thresh) ) {
JoaoJardim 8:167680431cf4 169 led3 = !led3;
JoaoJardim 8:167680431cf4 170
JoaoJardim 8:167680431cf4 171 m3pi.left_motor(-1*speed); // UTURN
JoaoJardim 8:167680431cf4 172 m3pi.right_motor(speed);
JoaoJardim 8:167680431cf4 173
JoaoJardim 8:167680431cf4 174 wait(u_turn_t);
JoaoJardim 8:167680431cf4 175
JoaoJardim 8:167680431cf4 176 m3pi.readsensor(sensors); // Read the sensors
JoaoJardim 8:167680431cf4 177
JoaoJardim 8:167680431cf4 178 a = sensors[1] - sensors[3];
JoaoJardim 8:167680431cf4 179 }
JoaoJardim 8:167680431cf4 180
JoaoJardim 8:167680431cf4 181 }
JoaoJardim 6:00f820418d01 182
JoaoJardim 8:167680431cf4 183 else {
JoaoJardim 8:167680431cf4 184 // Detect "dead end"
JoaoJardim 8:167680431cf4 185
JoaoJardim 8:167680431cf4 186 if(sensors[0] > threshold_sns || sensors[4] > threshold_sns) {
JoaoJardim 8:167680431cf4 187 foundjunction = 1; // Found an intersection
JoaoJardim 8:167680431cf4 188 }
JoaoJardim 8:167680431cf4 189
JoaoJardim 8:167680431cf4 190 // Solve junctions ********************************************************
JoaoJardim 8:167680431cf4 191
JoaoJardim 8:167680431cf4 192 // The order of the statements in this "if" is sufficient
JoaoJardim 8:167680431cf4 193 // to implement a follow left-hand wall algorithm
JoaoJardim 8:167680431cf4 194
JoaoJardim 8:167680431cf4 195 if (foundjunction==1) {
JoaoJardim 8:167680431cf4 196 led2 = !led2;
JoaoJardim 8:167680431cf4 197
JoaoJardim 8:167680431cf4 198 taskENTER_CRITICAL(); // We dont want the maze solver to be stopped during a turn
JoaoJardim 8:167680431cf4 199
JoaoJardim 8:167680431cf4 200 m3pi.forward(0.1); // Move forward a bit in case it meet
JoaoJardim 8:167680431cf4 201 wait(0.05); // the intersection sideways
JoaoJardim 8:167680431cf4 202 m3pi.stop();
JoaoJardim 8:167680431cf4 203 m3pi.readsensor(sensors); // Read the sensors again
JoaoJardim 8:167680431cf4 204
JoaoJardim 8:167680431cf4 205 if(sensors[0] > threshold_sns) {
JoaoJardim 8:167680431cf4 206 m3pi.forward(0.1);
JoaoJardim 8:167680431cf4 207 wait(0.67);
JoaoJardim 8:167680431cf4 208
JoaoJardim 8:167680431cf4 209 m3pi.left_motor(speed); // LEFT
JoaoJardim 8:167680431cf4 210 m3pi.right_motor(-1*speed);
JoaoJardim 8:167680431cf4 211 wait(0.67); // Was tuned for a 90 deg turn
JoaoJardim 8:167680431cf4 212 }
JoaoJardim 8:167680431cf4 213
JoaoJardim 8:167680431cf4 214 else {
JoaoJardim 8:167680431cf4 215 m3pi.forward(0.1); // Move forward a bit to discover
JoaoJardim 8:167680431cf4 216 wait(0.3); // if its a R or FR junction
JoaoJardim 8:167680431cf4 217 m3pi.stop();
JoaoJardim 8:167680431cf4 218 m3pi.readsensor(sensors); // Read the sensors again
JoaoJardim 8:167680431cf4 219
JoaoJardim 8:167680431cf4 220 if ( (sensors[1]<threshold_sns) && (sensors[2]<threshold_sns) && (sensors[3]<threshold_sns) ) {
JoaoJardim 8:167680431cf4 221
JoaoJardim 8:167680431cf4 222 // if the center sensors are white, its a R junction
JoaoJardim 8:167680431cf4 223 m3pi.forward(0.1);
JoaoJardim 8:167680431cf4 224 wait(0.30);
JoaoJardim 8:167680431cf4 225
JoaoJardim 8:167680431cf4 226 m3pi.left_motor(-1*speed); // RIGHT
JoaoJardim 8:167680431cf4 227 m3pi.right_motor(speed);
JoaoJardim 8:167680431cf4 228 wait(0.57); // Was tuned for a 90 deg turn
JoaoJardim 8:167680431cf4 229 }
JoaoJardim 8:167680431cf4 230
JoaoJardim 8:167680431cf4 231 else {
JoaoJardim 8:167680431cf4 232 m3pi.forward(speed); // FRONT
JoaoJardim 8:167680431cf4 233 wait(0.3);
JoaoJardim 8:167680431cf4 234 }
JoaoJardim 8:167680431cf4 235 }
JoaoJardim 8:167680431cf4 236
JoaoJardim 8:167680431cf4 237 foundjunction = 0; // Resets foundjunction flag
JoaoJardim 8:167680431cf4 238
JoaoJardim 8:167680431cf4 239 taskEXIT_CRITICAL();
JoaoJardim 8:167680431cf4 240 }
JoaoJardim 8:167680431cf4 241 }
JoaoJardim 8:167680431cf4 242
JoaoJardim 8:167680431cf4 243 vTaskDelay(xDelayMZ);
JoaoJardim 8:167680431cf4 244 }
JoaoJardim 8:167680431cf4 245 }
JoaoJardim 8:167680431cf4 246
JoaoJardim 8:167680431cf4 247
JoaoJardim 8:167680431cf4 248 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 249 // Task Follow
JoaoJardim 8:167680431cf4 250 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 251
JoaoJardim 8:167680431cf4 252
JoaoJardim 8:167680431cf4 253 void TaskLF (void* pvParameters){
JoaoJardim 4:33929b1afb80 254
JoaoJardim 8:167680431cf4 255 (void) pvParameters; // Just to stop compiler warnings.
JoaoJardim 8:167680431cf4 256
JoaoJardim 8:167680431cf4 257 m3pi.reset();
JoaoJardim 8:167680431cf4 258
JoaoJardim 8:167680431cf4 259 vTaskDelay(xDelayRst);
JoaoJardim 8:167680431cf4 260
JoaoJardim 8:167680431cf4 261 float speed = 0.3;
JoaoJardim 8:167680431cf4 262 float correction = 0.2;
JoaoJardim 8:167680431cf4 263 float threshold = 0.5;
JoaoJardim 8:167680431cf4 264
JoaoJardim 8:167680431cf4 265 m3pi.locate(0,1);
JoaoJardim 8:167680431cf4 266 m3pi.printf("Line Flw");
JoaoJardim 8:167680431cf4 267
JoaoJardim 8:167680431cf4 268 vTaskDelay(xDelayLF);
JoaoJardim 8:167680431cf4 269
JoaoJardim 8:167680431cf4 270 m3pi.sensor_auto_calibrate();
JoaoJardim 8:167680431cf4 271
JoaoJardim 8:167680431cf4 272 for( ;; ){
JoaoJardim 8:167680431cf4 273 led1 = !led1;
JoaoJardim 8:167680431cf4 274
JoaoJardim 8:167680431cf4 275 taskENTER_CRITICAL(); // Critical section so that the Scheduler won't interrupt the m3pi commands
JoaoJardim 8:167680431cf4 276 // -1.0 is far left, 1.0 is far right, 0.0 in the middle
JoaoJardim 8:167680431cf4 277 float position_of_line = m3pi.line_position();
JoaoJardim 8:167680431cf4 278
JoaoJardim 8:167680431cf4 279 // Line is more than the threshold to the right, slow the left motor
JoaoJardim 8:167680431cf4 280 if (position_of_line > threshold) {
JoaoJardim 8:167680431cf4 281 m3pi.right_motor(speed);
JoaoJardim 8:167680431cf4 282 m3pi.left_motor(speed-correction);
JoaoJardim 8:167680431cf4 283 }
JoaoJardim 8:167680431cf4 284
JoaoJardim 8:167680431cf4 285 // Line is more than 50% to the left, slow the right motor
JoaoJardim 8:167680431cf4 286 else if (position_of_line < -threshold) {
JoaoJardim 8:167680431cf4 287 m3pi.left_motor(speed);
JoaoJardim 8:167680431cf4 288 m3pi.right_motor(speed-correction);
JoaoJardim 8:167680431cf4 289 }
JoaoJardim 8:167680431cf4 290
JoaoJardim 8:167680431cf4 291 // Line is in the middle
JoaoJardim 8:167680431cf4 292 else {
JoaoJardim 8:167680431cf4 293 m3pi.forward(speed);
JoaoJardim 8:167680431cf4 294 }
JoaoJardim 8:167680431cf4 295 taskEXIT_CRITICAL();
JoaoJardim 8:167680431cf4 296
JoaoJardim 8:167680431cf4 297 vTaskDelay(xDelay);
JoaoJardim 8:167680431cf4 298 }
JoaoJardim 8:167680431cf4 299 }
JoaoJardim 8:167680431cf4 300
JoaoJardim 8:167680431cf4 301
JoaoJardim 8:167680431cf4 302 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 303 // Task Bluetooth
JoaoJardim 8:167680431cf4 304 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 305
JoaoJardim 8:167680431cf4 306
JoaoJardim 8:167680431cf4 307 void TaskBluetooth (void* pvParameters) {
JoaoJardim 8:167680431cf4 308
JoaoJardim 8:167680431cf4 309 (void) pvParameters; // Just to stop compiler warnings.
JoaoJardim 8:167680431cf4 310
JoaoJardim 8:167680431cf4 311 TaskHandle_t xRunngTask = NULL;
JoaoJardim 8:167680431cf4 312
JoaoJardim 8:167680431cf4 313 void *pvQSnd = malloc(sizeof(int));
JoaoJardim 8:167680431cf4 314 // char cMsg[8]=""; // Used for writing to the LCD
JoaoJardim 8:167680431cf4 315
JoaoJardim 8:167680431cf4 316 // Resets the Wireless module
JoaoJardim 6:00f820418d01 317 nRST = 0;
JoaoJardim 6:00f820418d01 318 vTaskDelay(xDelay);
JoaoJardim 6:00f820418d01 319 nRST = 1;
JoaoJardim 6:00f820418d01 320
JoaoJardim 6:00f820418d01 321 Serial rn41(p28,p27); // Serial rn41(p9,p10);
JoaoJardim 4:33929b1afb80 322
JoaoJardim 4:33929b1afb80 323 rn41.baud(115200);
JoaoJardim 8:167680431cf4 324 led1 = 0;
JoaoJardim 8:167680431cf4 325 led2 = 0;
JoaoJardim 8:167680431cf4 326 led3 = 0;
JoaoJardim 8:167680431cf4 327 led4 = 0;
JoaoJardim 8:167680431cf4 328
JoaoJardim 8:167680431cf4 329 m3pi.cls(); // Clears the LCD
JoaoJardim 8:167680431cf4 330
JoaoJardim 8:167680431cf4 331 xTaskCreate( TaskFree, ( const char * ) "TaskFree", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, &xRunngTask );
JoaoJardim 4:33929b1afb80 332
chalikias 0:7f7fa5e631ec 333 for (;;) {
JoaoJardim 4:33929b1afb80 334
JoaoJardim 4:33929b1afb80 335 if(m3pi.battery() < fVref) {
JoaoJardim 4:33929b1afb80 336 m3pi.locate(0,0);
JoaoJardim 4:33929b1afb80 337 m3pi.print("Low Pwr!", 8);
JoaoJardim 4:33929b1afb80 338 }
JoaoJardim 4:33929b1afb80 339 else{
JoaoJardim 4:33929b1afb80 340 m3pi.locate(0,0);
JoaoJardim 4:33929b1afb80 341 m3pi.print("BT Task", 8);
JoaoJardim 4:33929b1afb80 342 }
JoaoJardim 4:33929b1afb80 343
JoaoJardim 4:33929b1afb80 344 if (rn41.readable()) { // When BT is readable
JoaoJardim 4:33929b1afb80 345
JoaoJardim 8:167680431cf4 346 *((int*)pvQSnd) = rn41.getc(); // Reads Bluetooth
JoaoJardim 8:167680431cf4 347 //led2 = *((int*)pvQSnd); // The received[0 / 1] turns [off / on] LED2
JoaoJardim 8:167680431cf4 348
JoaoJardim 8:167680431cf4 349 /*
JoaoJardim 8:167680431cf4 350 sprintf(&cMsg[0], "%d", *((int*)pvQSnd)); //
JoaoJardim 8:167680431cf4 351 m3pi.locate(0,1);
JoaoJardim 8:167680431cf4 352 m3pi.print(cMsg, 8);
JoaoJardim 8:167680431cf4 353 */
JoaoJardim 4:33929b1afb80 354
JoaoJardim 5:97a449a1e05f 355 if(rn41.writeable()) {
JoaoJardim 5:97a449a1e05f 356 rn41.putc(led2); // Send LED2 state to the BT terminal
JoaoJardim 5:97a449a1e05f 357 }
JoaoJardim 5:97a449a1e05f 358
JoaoJardim 8:167680431cf4 359 if (*((int*)pvQSnd) == 7) {
JoaoJardim 8:167680431cf4 360 vTaskDelete(xRunngTask); // Deletes the task that controls the currently running mode
JoaoJardim 8:167680431cf4 361 m3pi.stop(); // Prevents the m3pi from running wild while the system prepares the new mode
JoaoJardim 8:167680431cf4 362 xTaskCreate( TaskFree, ( const char * ) "TaskFree", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, &xRunngTask );
JoaoJardim 8:167680431cf4 363 }
JoaoJardim 8:167680431cf4 364
JoaoJardim 8:167680431cf4 365 else if (*((int*)pvQSnd) == 8) {
JoaoJardim 8:167680431cf4 366 vTaskDelete(xRunngTask); // Deletes the task that controls the currently running mode
JoaoJardim 8:167680431cf4 367 m3pi.stop(); // Prevents the m3pi from running wild while the system prepares the new mode
JoaoJardim 8:167680431cf4 368 xTaskCreate( TaskMZ, ( const char * ) "TaskMZ", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, &xRunngTask );
JoaoJardim 8:167680431cf4 369 }
JoaoJardim 8:167680431cf4 370
JoaoJardim 8:167680431cf4 371 else if (*((int*)pvQSnd) == 9) {
JoaoJardim 8:167680431cf4 372 vTaskDelete(xRunngTask); // Deletes the task that controls the currently running mode
JoaoJardim 8:167680431cf4 373 m3pi.stop(); // Prevents the m3pi from running wild while the system prepares the new mode
JoaoJardim 8:167680431cf4 374 xTaskCreate( TaskLF, ( const char * ) "TaskLF", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, &xRunngTask );
JoaoJardim 8:167680431cf4 375 }
JoaoJardim 8:167680431cf4 376
JoaoJardim 8:167680431cf4 377 else xQueueSend(xQueue1, pvQSnd, 0);
JoaoJardim 4:33929b1afb80 378 }
JoaoJardim 4:33929b1afb80 379
JoaoJardim 8:167680431cf4 380
chalikias 0:7f7fa5e631ec 381 led1 = !led1;
JoaoJardim 8:167680431cf4 382 vTaskDelay(xDelay);
chalikias 0:7f7fa5e631ec 383 }
chalikias 0:7f7fa5e631ec 384 }
chalikias 0:7f7fa5e631ec 385
JoaoJardim 4:33929b1afb80 386
JoaoJardim 8:167680431cf4 387 //*******************************************************************************************************************
JoaoJardim 8:167680431cf4 388 // Main
JoaoJardim 8:167680431cf4 389 //*******************************************************************************************************************
chalikias 0:7f7fa5e631ec 390
JoaoJardim 8:167680431cf4 391
JoaoJardim 8:167680431cf4 392 int main (void) {
JoaoJardim 8:167680431cf4 393
JoaoJardim 8:167680431cf4 394 xQueue1 = xQueueCreate( 1, sizeof( int ) );
JoaoJardim 8:167680431cf4 395 if(xQueue1 == NULL) led4=1;
JoaoJardim 8:167680431cf4 396
JoaoJardim 8:167680431cf4 397 xTaskCreate( TaskBluetooth, ( const char * ) "TaskBluetooth", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, ( xTaskHandle * ) NULL );
JoaoJardim 8:167680431cf4 398
chalikias 0:7f7fa5e631ec 399 vTaskStartScheduler();
JoaoJardim 8:167680431cf4 400 //should never get here
JoaoJardim 8:167680431cf4 401 printf("ERORR: vTaskStartScheduler returned!");
JoaoJardim 8:167680431cf4 402 for (;;);
chalikias 0:7f7fa5e631ec 403 }