File content as of revision 8:167680431cf4:

//*******************************************************************************************************************
//                               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;


//*******************************************************************************************************************
//                               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];
            }
                
        }
        
        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;
    
    Serial rn41(p28,p27); // Serial rn41(p9,p10);
    
    rn41.baud(115200);
    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.locate(0,0);
            m3pi.print("Low Pwr!", 8);            
        }        
        else{
            m3pi.locate(0,0);
            m3pi.print("BT Task", 8);
        }
         
        if (rn41.readable()) {          // When BT is readable
        
            *((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);            
    }
}


//*******************************************************************************************************************
//                               Main
//*******************************************************************************************************************


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 (;;);
}