File content as of revision 19:4fbffc755ed7:

/******************************************************************************/
// ECE4333
// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
// LAB Partner 2:   Brandon Kingman - ID: 3470444
// Project:         Autonomous Robot Design
// Instructor:      Prof. Chris Diduch
/******************************************************************************/
// filename: ui.cpp
// file content description:
//      * Functions to display and manage the user interface on the PC terminal. 
/******************************************************************************/

#include "mbed.h"
#include "WatchdogThread.h"
#include "ui.h"
#include "CameraThread.h"
#include "PiControlThread.h"

Serial bluetooth(p9,p10); // Define the bluetooth channel and IO pins
Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel

Mutex setpointR_mutex;
Mutex setpointL_mutex;

// speed
int setpointR = 0;
int setpointL = 0;


bool killRobot = false;

// variable to store character recieved from terminal
char x;
int16_t position;


/******************************************************************************
                           User interface 3 Camera
******************************************************************************/
void consoleUIcamera(void){

    cameraData_mutex.lock();
    bluetooth.printf("\r\nsetpointR: %d   setpointL: %d    ObjectWidth: %d     ObjectXcoordinate: %d   SteeringError: %d   DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);    
    cameraData_mutex.unlock();
    //bluetooth.printf("\r\nsetpointR: %d   setpointL: %d    ObjectWidth: %d     ObjectXcoordinate: %d   SteeringError: %d   DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);       

    // safety mechanism
    if (bluetooth.readable()){
        x = bluetooth.getc();
        if (x == 't')
        {
            killRobot = true;
        }
        else if (x == 'r')
        {
            killRobot = false;
        }
        
        
    }
    
}



/******************************************************************************
            A function to test blutooth communication
******************************************************************************/
void twoTerminalsTest()
{
    if (pc.readable()) { // If a key is pressed on the pc channel
        x=pc.getc(); // read the character and send it to both the
        bluetooth.putc(x); // bluetooth channel and the pc channel for
        pc.putc(x); // display.
    }
    if (bluetooth.readable()) {
        x=bluetooth.getc(); // If there’s a keypress on the bluetooth
        pc.putc(x); // channel, read the character and send it to
        bluetooth.putc(x); // both the pc channel and the bluetooth
    } // channel for display
}


/******************************************************************************
            A function to Display startup Messsage
******************************************************************************/
void displayStartupMsg_old()
{
    bluetooth.printf("\r\n************************************");
    bluetooth.printf("\r\n**** DC Motor Control using PWM ****");
    bluetooth.printf("\r\n************************************");
    bluetooth.printf("\r\n-Enter r to reset the watchdog timer");
    bluetooth.printf("\r\n-press w to increase motor speedR");
    bluetooth.printf("\r\n-press s to decrease motor speedR");
    bluetooth.printf("\r\n-press i to increase motor speedL");
    bluetooth.printf("\r\n-press k to decrease motor speedL");
}

void displayStartupMsg()
{
    bluetooth.printf("\r\n************************************");
    bluetooth.printf("\r\n**** AUTONOMOUS FOLLOWING ROBOT ****");
    bluetooth.printf("\r\n************************************");
    bluetooth.printf("\r\n-PRESS 'r' TO KILL ROBOT");
}

/******************************************************************************
                           User interface 1
******************************************************************************/
/*
void consoleUI(void)
{
    if (bluetooth.readable()) {
        x = bluetooth.getc();
        
        // if input from console is the letter 'r'
        if(x == 'r') {
            // reset watchdog timer
            WatchdogReset();
            bluetooth.printf("\r\nWatchdog has been reset");
        }

        // if w is pressed increase the speed
        // by incrementing u
        else if(x == 'w') {
            setpoint_mutex.lock();
            if ( setpoint < 560 ) 
            {
                //setpoint = setpoint + SPEED_STEP;
                setpoint = 100;
            }
            setpoint_mutex.unlock();

            // display speed
            bluetooth.printf("\r\n %5d", setpoint);
        }

        // if s is pressed decrease the speed
        // by decrementing u
        else if(x == 's') {

            setpoint_mutex.lock();
            if (setpoint > -560) 
            {
                setpoint = -100;
                //setpoint = setpoint - SPEED_STEP;
            }
                
            setpoint_mutex.unlock();

            // display speed
            bluetooth.printf("\r\n %5d", setpoint);
        }

        // error wrong input
        else {
            bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
        }
    }
    position += dPosition;
    bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, xState, dTime, Kp, Ki, vel, e);
    
}
*/
/******************************************************************************
                           User interface 2
******************************************************************************/

void consoleUIold(void)
{
     
    if (bluetooth.readable()) {
        x = bluetooth.getc();
        
        // if input from console is the letter 'r'
        if(x == 'r') {
            // reset watchdog timer
            WatchdogReset();
            setpointR = 0;
            setpointL = 0;
            bluetooth.printf("\r\nWatchdog has been reset");
        }
        
/******************************RIGHT MOTOR*************************************/
        // if w is pressed increase the speed
        // by incrementing u
        else if(x == 'w') {
            setpointR_mutex.lock();
            //setpoint = setpoint + SPEED_STEP;
            setpointR = 25;
            setpointR_mutex.unlock();
            
            bluetooth.printf("\r\n %5d", setpointR);
        }

        // if s is pressed decrease the speed
        // by decrementing u
        else if(x == 's') {

            setpointR_mutex.lock();

            setpointR = -25;
            //setpoint = setpoint - SPEED_STEP;

            setpointR_mutex.unlock();

            // display speed
            //bluetooth.printf("\r\n %5d", setpointR);
        }

/******************************LEFT MOTOR**************************************/        
        else if (x=='i')
        {
            setpointL_mutex.lock();

 
                //setpoint = setpoint + SPEED_STEP;
                setpointL = 25;

            setpointL_mutex.unlock();
            //bluetooth.printf("\r\n %5d", setpointL);
        }
        else if (x=='k')
        {
            setpointL_mutex.lock();

            setpointL = -25;
            //setpoint = setpoint - SPEED_STEP;
  
            setpointL_mutex.unlock();

            // display speed
            //bluetooth.printf("\r\n %5d", setpointL);
        }
/******************************END MOTOR SETPOINT******************************/
        
        // error wrong input
        else {
            bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
        }
    }   
    
    
    
    //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e);
    
    //bluetooth.printf("\r\ndP: %d, vel: %d, (Kp*e) Kp: %f, (Ki*xState) Ki: %f, e: %d, xState: %d, u: %d", dPosition, vel, Kp, Ki, e, xState , u);
    
    /*
    bluetooth.printf("\r\n %d,   ", e);
    bluetooth.printf("%d,   ", dPosition);
    bluetooth.printf("%d,   ", xState);
    bluetooth.printf("%d,   ", u);
    */
    
    //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime);
    
}

/******************************************************************************
                           User interface 3 - manual control
******************************************************************************/

void consoleUI(void)
{
    if (bluetooth.readable()) {
        x = bluetooth.getc();
        
        // if input from console is the letter 'r'
        if(x == 'r') {
            // reset watchdog timer
            WatchdogReset();
            setpointR = 0;
            setpointL = 0;
            bluetooth.printf("\r\nWatchdog has been reset");
        }
        
/******************************ROBOT FWD RVS***********************************/
        // if w is pressed increase the speed
        // by incrementing u
        else if(x == 'w') {
            mutexSetpoint.lock();
            Setpoint = -25;
            mutexSetpoint.unlock();
        
        }

        // if s is pressed decrease the speed
        // by decrementing u
        else if(x == 's') {
            mutexSetpoint.lock();
            Setpoint = 25;
            mutexSetpoint.unlock();
        
        }

/******************************ROBOT STEERING**********************************/        
        else if (x=='a')
        {
            mutexSetpoint.lock();
            SteeringError = 75;
            mutexSetpoint.unlock();
        }
        else if (x=='d')
        {
            mutexSetpoint.lock();
            SteeringError = -75;
            mutexSetpoint.unlock();
        }        
        // error wrong input
        else {
            bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog");
        }
    }
    else{
        // If no key is pressed stop the robot. 
        Setpoint = 0;
        SteeringError = 0;
           
    }
    
    //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e);
    
}