João Jardim
Final version of the m3pi software
Dependencies: mbed m3pi FreeRTOS_V8_2_1_LPC1768
Diff: main.cpp
- Revision:
- 8:167680431cf4
- Parent:
- 6:00f820418d01
--- a/main.cpp Mon Dec 10 10:38:01 2018 +0000
+++ b/main.cpp Fri Dec 21 18:34:21 2018 +0000
@@ -1,30 +1,319 @@
-// N.C. Freertos mbed minimal example based on below:
-// https://developer.mbed.org/users/rgrover1/code/FreeRTOS/
-// http://www.radekdostal.com/content/freertos-610-minimal-example
+//*******************************************************************************************************************
+// Includes
+//*******************************************************************************************************************
+
#include "mbed.h"
#include "FreeRTOS.h"
#include "task.h"
#include "m3pi.h"
+#include "queue.h"
+
+
+//*******************************************************************************************************************
+// Definitions & Initializations
+//*******************************************************************************************************************
+
+
+// Task Priorities
+#define PRIORITY_MAX 4
+#define PRIORITY_HIGH 3
+#define PRIORITY_NORMAL 2
+#define PRIORITY_LOW 1
+#define PRIORITY_IDLE 0
+
+#define FORWARD 2
+#define BACKWARD 3
+#define LEFT 4
+#define RIGHT 5
+#define STOP 6
+
m3pi m3pi;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
DigitalOut nRST(p26);
const portTickType xDelay = 100 / portTICK_RATE_MS;
+const portTickType xDelayMZ = 100 / portTICK_RATE_MS;
+const portTickType xDelayRst = 2000 / portTICK_RATE_MS;
+const portTickType xDelayLF = 1000 / portTICK_RATE_MS;
+
const float fVref = 4.0;
+QueueHandle_t xQueue1;
-void TaskBluetooth (void* pvParameters)
-{
- (void) pvParameters; // Just to stop compiler warnings.
+
+//*******************************************************************************************************************
+// Task Free
+//*******************************************************************************************************************
+
+
+void TaskFree (void* pvParameters) {
+
+ (void) pvParameters; // Just to stop compiler warnings.
+
+
+ void *pvQBuf = malloc(sizeof(int));
+
+ for( ;; ) {
+ if ( xQueueReceive(xQueue1, pvQBuf, 0) ) {
+
+ taskENTER_CRITICAL(); // Critical section so that the Scheduler won't interrupt the m3pi commands
+ switch(*((int*)pvQBuf)) {
+ case FORWARD:
+ m3pi.forward(0.2);
+ break;
+
+ case BACKWARD:
+ m3pi.backward(0.2);
+ break;
+
+ case LEFT:
+ m3pi.left(0.2);
+ break;
+
+ case RIGHT:
+ m3pi.right(0.2);
+ break;
+
+ case STOP:
+ m3pi.stop();
+ break;
+ }
+ taskEXIT_CRITICAL();
+ }
+ }
+}
+
+
+//*******************************************************************************************************************
+// Task Maze
+//*******************************************************************************************************************
+
+
+void TaskMZ (void* pvParameters){
+
+ (void) pvParameters; // Just to stop compiler warnings.
+
+ led1 = 0;
+
+ m3pi.reset();
+
+ vTaskDelay(xDelayRst);
+
+ float speed = 0.125;
+ float correction = 0.125;
+ float threshold = 0.5;
+
+
+ float u_turn_t = 0.2; // float u_turn_t = 1.4;
+ float u_turn_thresh = 50;
+
+
+
+ // These variables record whether the robot has seen a line to the
+ // left, straight ahead, and right, while examining the current intersection.
+ int sensors[5];
+ int foundjunction=0;
+
+ int threshold_sns = 500;
+
+ m3pi.locate(0,1);
+ m3pi.printf("Maze slv");
+
+ m3pi.sensor_auto_calibrate();
+
+ for( ;; ){
+ led1 = !led1;
+
+ // Line Follower ****************************************************************************
+
+ // -1.0 is far left, 1.0 is far right, 0.0 in the middle
+ float position_of_line = m3pi.line_position();
+
+ // Line is more than the threshold to the right, slow the left motor
+ if (position_of_line > threshold) {
+ m3pi.right_motor(speed);
+ m3pi.left_motor(speed-(position_of_line*correction));
+ }
+
+ // Line is more than 50% to the left, slow the right motor
+ else if (position_of_line < -threshold) {
+ m3pi.left_motor(speed);
+ m3pi.right_motor(speed+(position_of_line*correction));
+ }
+
+ // Line is in the middle
+ else {
+ m3pi.forward(speed);
+ }
+
+
+ // Detect junctions *************************************************************************
+
+ m3pi.readsensor(sensors); // Read the sensors
+
+
+ if ( (sensors[1]<threshold_sns) && (sensors[2]<threshold_sns) && (sensors[3]<threshold_sns) ){
+ // Detects the a dead end and resolves it by "u turning"
+
+ float a = sensors[1] - sensors[3];
+
+ while ( (a < u_turn_thresh) && ((-1*a) < u_turn_thresh) ) {
+ led3 = !led3;
+
+ m3pi.left_motor(-1*speed); // UTURN
+ m3pi.right_motor(speed);
+
+ wait(u_turn_t);
+
+ m3pi.readsensor(sensors); // Read the sensors
+
+ a = sensors[1] - sensors[3];
+ }
+
+ }
- int i;
+ else {
+ // Detect "dead end"
+
+ if(sensors[0] > threshold_sns || sensors[4] > threshold_sns) {
+ foundjunction = 1; // Found an intersection
+ }
+
+ // Solve junctions ********************************************************
+
+ // The order of the statements in this "if" is sufficient
+ // to implement a follow left-hand wall algorithm
+
+ if (foundjunction==1) {
+ led2 = !led2;
+
+ taskENTER_CRITICAL(); // We dont want the maze solver to be stopped during a turn
+
+ m3pi.forward(0.1); // Move forward a bit in case it meet
+ wait(0.05); // the intersection sideways
+ m3pi.stop();
+ m3pi.readsensor(sensors); // Read the sensors again
+
+ if(sensors[0] > threshold_sns) {
+ m3pi.forward(0.1);
+ wait(0.67);
+
+ m3pi.left_motor(speed); // LEFT
+ m3pi.right_motor(-1*speed);
+ wait(0.67); // Was tuned for a 90 deg turn
+ }
+
+ else {
+ m3pi.forward(0.1); // Move forward a bit to discover
+ wait(0.3); // if its a R or FR junction
+ m3pi.stop();
+ m3pi.readsensor(sensors); // Read the sensors again
+
+ if ( (sensors[1]<threshold_sns) && (sensors[2]<threshold_sns) && (sensors[3]<threshold_sns) ) {
+
+ // if the center sensors are white, its a R junction
+ m3pi.forward(0.1);
+ wait(0.30);
+
+ m3pi.left_motor(-1*speed); // RIGHT
+ m3pi.right_motor(speed);
+ wait(0.57); // Was tuned for a 90 deg turn
+ }
+
+ else {
+ m3pi.forward(speed); // FRONT
+ wait(0.3);
+ }
+ }
+
+ foundjunction = 0; // Resets foundjunction flag
+
+ taskEXIT_CRITICAL();
+ }
+ }
+
+ vTaskDelay(xDelayMZ);
+ }
+}
+
+
+//*******************************************************************************************************************
+// Task Follow
+//*******************************************************************************************************************
+
+
+void TaskLF (void* pvParameters){
+ (void) pvParameters; // Just to stop compiler warnings.
+
+ m3pi.reset();
+
+ vTaskDelay(xDelayRst);
+
+ float speed = 0.3;
+ float correction = 0.2;
+ float threshold = 0.5;
+
+ m3pi.locate(0,1);
+ m3pi.printf("Line Flw");
+
+ vTaskDelay(xDelayLF);
+
+ m3pi.sensor_auto_calibrate();
+
+ for( ;; ){
+ led1 = !led1;
+
+ taskENTER_CRITICAL(); // Critical section so that the Scheduler won't interrupt the m3pi commands
+ // -1.0 is far left, 1.0 is far right, 0.0 in the middle
+ float position_of_line = m3pi.line_position();
+
+ // Line is more than the threshold to the right, slow the left motor
+ if (position_of_line > threshold) {
+ m3pi.right_motor(speed);
+ m3pi.left_motor(speed-correction);
+ }
+
+ // Line is more than 50% to the left, slow the right motor
+ else if (position_of_line < -threshold) {
+ m3pi.left_motor(speed);
+ m3pi.right_motor(speed-correction);
+ }
+
+ // Line is in the middle
+ else {
+ m3pi.forward(speed);
+ }
+ taskEXIT_CRITICAL();
+
+ vTaskDelay(xDelay);
+ }
+}
+
+
+//*******************************************************************************************************************
+// Task Bluetooth
+//*******************************************************************************************************************
+
+
+void TaskBluetooth (void* pvParameters) {
+
+ (void) pvParameters; // Just to stop compiler warnings.
+
+ TaskHandle_t xRunngTask = NULL;
+
+ void *pvQSnd = malloc(sizeof(int));
+ // char cMsg[8]=""; // Used for writing to the LCD
+
+ // Resets the Wireless module
nRST = 0;
vTaskDelay(xDelay);
nRST = 1;
@@ -32,45 +321,83 @@
Serial rn41(p28,p27); // Serial rn41(p9,p10);
rn41.baud(115200);
- led1 = 1;
- led2 = 1;
+ led1 = 0;
+ led2 = 0;
+ led3 = 0;
+ led4 = 0;
+
+ m3pi.cls(); // Clears the LCD
+
+ xTaskCreate( TaskFree, ( const char * ) "TaskFree", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, &xRunngTask );
for (;;) {
if(m3pi.battery() < fVref) {
- m3pi.cls();
m3pi.locate(0,0);
m3pi.print("Low Pwr!", 8);
}
else{
- m3pi.cls();
m3pi.locate(0,0);
m3pi.print("BT Task", 8);
}
if (rn41.readable()) { // When BT is readable
- i = rn41.getc(); // Reads Bluetooth
- led2 = i; // The received[0 / 1] turns [off / on] LED2
+ *((int*)pvQSnd) = rn41.getc(); // Reads Bluetooth
+ //led2 = *((int*)pvQSnd); // The received[0 / 1] turns [off / on] LED2
+
+ /*
+ sprintf(&cMsg[0], "%d", *((int*)pvQSnd)); //
+ m3pi.locate(0,1);
+ m3pi.print(cMsg, 8);
+ */
if(rn41.writeable()) {
rn41.putc(led2); // Send LED2 state to the BT terminal
}
+ if (*((int*)pvQSnd) == 7) {
+ vTaskDelete(xRunngTask); // Deletes the task that controls the currently running mode
+ m3pi.stop(); // Prevents the m3pi from running wild while the system prepares the new mode
+ xTaskCreate( TaskFree, ( const char * ) "TaskFree", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, &xRunngTask );
+ }
+
+ else if (*((int*)pvQSnd) == 8) {
+ vTaskDelete(xRunngTask); // Deletes the task that controls the currently running mode
+ m3pi.stop(); // Prevents the m3pi from running wild while the system prepares the new mode
+ xTaskCreate( TaskMZ, ( const char * ) "TaskMZ", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, &xRunngTask );
+ }
+
+ else if (*((int*)pvQSnd) == 9) {
+ vTaskDelete(xRunngTask); // Deletes the task that controls the currently running mode
+ m3pi.stop(); // Prevents the m3pi from running wild while the system prepares the new mode
+ xTaskCreate( TaskLF, ( const char * ) "TaskLF", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, &xRunngTask );
+ }
+
+ else xQueueSend(xQueue1, pvQSnd, 0);
}
+
led1 = !led1;
- vTaskDelay(xDelay);
+ vTaskDelay(xDelay);
}
}
+//*******************************************************************************************************************
+// Main
+//*******************************************************************************************************************
-int main (void)
-{
- xTaskCreate( TaskBluetooth, ( const char * ) "TaskBluetooth", configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY, ( xTaskHandle * ) NULL );
+
+int main (void) {
+
+ xQueue1 = xQueueCreate( 1, sizeof( int ) );
+ if(xQueue1 == NULL) led4=1;
+
+ xTaskCreate( TaskBluetooth, ( const char * ) "TaskBluetooth", configMINIMAL_STACK_SIZE, NULL, PRIORITY_LOW, ( xTaskHandle * ) NULL );
+
vTaskStartScheduler();
- //should never get here
- printf("ERORR: vTaskStartScheduler returned!");
- for (;;);
+ //should never get here
+ printf("ERORR: vTaskStartScheduler returned!");
+ for (;;);
}
\ No newline at end of file