Revision 8:167680431cf4, committed 2018-12-21

Comitter:

JoaoJardim

Date:

Fri Dec 21 18:34:21 2018 +0000

Parent:

7:19fa6a5b5a06

Commit message:

Final version of the software for the m3pi

Changed in this revision

--- a/m3pi.lib	Mon Dec 10 10:38:01 2018 +0000
+++ b/m3pi.lib	Fri Dec 21 18:34:21 2018 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/chris/code/m3pi/#4b7d6ea9b35b
+https://os.mbed.com/users/chris/code/m3pi/#e710e854e8a8

--- 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