File content as of revision 2:9a4938a0475b:

#include "mbed.h"
#include "hcsr04.h"
#include "rtos.h"
#include <string>

#include "DriveCommand.h"
#include "queue.h"

#define ULTRASONIC_TRIG_PIN p8
#define ULTRASONIC_ECHO_PIN p9
#define MOTOR_A_PIN1        p23
#define MOTOR_A_PIN2        p24
#define MOTOR_B_PIN1        p21 
#define MOTOR_B_PIN2        p22
#define BLUETOOTH_RX        p27
#define BLUETOOTH_TX        p28
#define SERVO_PWM_PIN       p25

#define BT_CHECK_DELAY_MS 50

#define ULTRASONIC_CHECK_DELAY_MS 50
#define ULTRASONIC_EMERGENCY_DELAY_MS 20
#define AUTODRIVE_CHECK_DELAY_MS 100



//------DEBUG OPTIONS ----------
//#define DEBUG_BT_COMMANDS
#define DEBUG_ULTRASONICE

//------------------------------

PwmOut motorA1(MOTOR_A_PIN2); //left motor
PwmOut motorA2(MOTOR_A_PIN1); //left motor
PwmOut motorB1(MOTOR_B_PIN1); //right motor
PwmOut motorB2(MOTOR_B_PIN2); //right motor

PwmOut servo(SERVO_PWM_PIN);
Serial bt(BLUETOOTH_TX,BLUETOOTH_RX);
Serial pc(USBTX,USBRX);

HCSR04  usensor(p8,p9);
float lastDistance = 0; // save the last ultrasonic val
bool moveForward_flag = false; // determined when vehicle moving forward - using to stop and avoid obascales
bool turnRight_Flag = false;
bool turnLeft_Flag = false;
bool obstacleAlert = false;
float vehicleSpeed = 1;
int rotateRightMillis  = 650;
int rotateLeftMillis = 650;
float turnSpeed = 0.3;
int obstacleDistance = 30;

Timeout stopper;
Queue autoDriverQueue(sizeof(DriveCommand),10);


//string globalString =" ";
char lastCommandChar ='x';
char incomingMsg[1024];
int incomingMsg_iter = 0;
bool succesReadFlag = false;
bool autoDriveFlag = false;
bool autoDriveRun = false;
int autoDriveCounter = 0;
bool autoDrivePartOk = true; // check if every part of auto drive is whitout obstacles

//Prototype
void Drive_Stop();
void AutoDrive_Stop();
void ExecuteNextCommand();
void Rotate_Left();
void MotorRight_Run(float speedPercent,bool forwardDir);
void MotorLeft_Run(float speedPercent,bool forwardDir);
void Drive_Force_Stop();
int Get_Min_Distance();
void Servo_Look_Left();
void Servo_Look_Right();
void Servo_Look_Center();


void FlagController(char dir)
{
    switch(dir)
    {
        case 'f':
        moveForward_flag = true; 
        turnRight_Flag = false;
        turnLeft_Flag = false;
        break;
        case 'l':
        moveForward_flag = false; 
        turnRight_Flag = false;
        turnLeft_Flag = true;
        break; 
        case 'r':
        moveForward_flag = false; 
        turnRight_Flag = true;
        turnLeft_Flag = false;
        break;    
        case 'b':
        moveForward_flag = false; 
        turnRight_Flag = false;
        turnLeft_Flag = false;
        break;     
    }
}

void StopAfter(float sec)
{
   //  printf("StopAfter Got %f\n",sec);
     stopper.attach(&AutoDrive_Stop,sec);  
}

float GetDistance()
{
    float sum = 0;
    
    for(int i=i ; i< 7 ; i++)
    {
         sum += usensor.get_dist_cm();      
    }
    
    sum /= 7.0;   
    return sum;    
    }



void Reset_IncomingMsg()
{
    for(int i=0 ; i<1024 ; i++)
        incomingMsg[i] = '*'; 
    incomingMsg_iter  =0;    
}

string Get_last_Msg()
{
    int i=0;
    string temp = "";
    while(incomingMsg[i] != '~' && incomingMsg[i] != '*')
    {
        temp+=incomingMsg[i++];         
    }
       
       if(incomingMsg[i] == '~')
       {
            succesReadFlag = true;  
            Reset_IncomingMsg();  
        }
            
       else
       {
           succesReadFlag = false;       
        }                         
        return temp;
}

float map(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


 void dist(int distance)
{
    //put code here to happen when the distance is changed
    printf("Distance changed to %dmm\r\n", distance);
}


void MotorLeft_Run(float speedPercent,bool forwardDir)
{
    if(forwardDir)
    {
      motorA1.write(0);
      motorA2.write(speedPercent);      
    }
    else
    {
      motorA1.write(speedPercent);
      motorA2.write(0);          
    } 
}

void MotorLeft_Stop()
{
    motorA1.write(0);
    motorA2.write(0); 
}

void MotorRight_Run(float speedPercent,bool forwardDir)
{
    if(forwardDir)
    {
      motorB1.write(0);
      motorB2.write(speedPercent);      
    }
    else
    {
      motorB1.write(speedPercent);
      motorB2.write(0);          
    } 
}

void MotorRight_Stop()
{
    motorB1.write(0);
    motorB2.write(0); 
}

void Drive_Forward(float speedPercent)
{
    FlagController('f');
    MotorLeft_Run(speedPercent,true);
    MotorRight_Run(speedPercent,true) ;  
}

void Drive_Left(float leftPercent)
{
    FlagController('f');
    MotorRight_Run(leftPercent,true);
    MotorLeft_Run(1-leftPercent,true) ; // left motor reversed
}

void Reverse_Left(float leftPercent)
{
    MotorRight_Run(leftPercent,false);
    MotorLeft_Run(1-leftPercent,false) ; // left motor reversed
}

void Turn_Left(float leftPercent)
{
    FlagController('l');
    Servo_Look_Left();
    wait_ms(100);
    MotorRight_Run(leftPercent,true);
    MotorLeft_Run(leftPercent,false) ; // left motor reversed
}

void Drive_Right(float rightPercent)
{
      MotorLeft_Run(rightPercent,true) ;
      MotorRight_Run(1-rightPercent,true); // right motor reversed
}

void Reverse_Right(float rightPercent)
{
      MotorLeft_Run(rightPercent,false) ;
      MotorRight_Run(1-rightPercent,false); // right motor reversed
}

void Turn_Right(float rightPercent)
{
      FlagController('r');
      Servo_Look_Right();
      wait_ms(200);
      MotorLeft_Run(rightPercent,true) ;
      MotorRight_Run(rightPercent,false); // right motor reversed
}

void Drive_Backwards(float speedPercent)
{
    FlagController('b');
    MotorLeft_Run(speedPercent,false);
    MotorRight_Run(speedPercent,false) ; 
    moveForward_flag = false; 
}


void Drive_Stop()
{
    FlagController('s');
    MotorLeft_Stop();
    MotorRight_Stop();
    moveForward_flag = false;
    autoDriveRun = false;
}
void Drive_Force_Stop()
{
    MotorRight_Run(turnSpeed,false);
    MotorLeft_Run(turnSpeed,false) ;
    wait_ms(500);
    MotorLeft_Stop();
    MotorRight_Stop();
    moveForward_flag = false;
    autoDriveRun = false;
    
}


void AutoDrive_Stop()
{
//    if(autoDriveFlag) // stop only if autoDrive mode is running
//    {
        MotorLeft_Stop();
        MotorRight_Stop();
        moveForward_flag = false;        
//    }
    autoDriveRun = false;  
}


void Rotate_Left()
{
    MotorRight_Run(turnSpeed,true);
    MotorLeft_Run(turnSpeed,false) ; // left motor reversed
    wait_ms(rotateLeftMillis);
    Drive_Stop(); 
}

void Rotate_Right()
{
    MotorRight_Run(turnSpeed,false);
    MotorLeft_Run(turnSpeed,true) ; // left motor reversed
    wait_ms(rotateRightMillis);
    Drive_Stop(); 
}


void ServoMove(int pos)
{
    if(pos < 0 || pos >180)
        return;
    float outVal = map(pos,0,180,1000,2000);
    servo.pulsewidth_us(outVal);   
}


int Get_Min_Distance()
{
    int min;
    
    Servo_Look_Left();
    wait_ms(250);
    usensor.start();
    int left = usensor.get_dist_cm();
        min=left;
        
    if(min <  obstacleDistance )
    {
        autoDrivePartOk = false;
        return min;   
     }
       
    Servo_Look_Right();
    wait_ms(250);
    usensor.start();
    int right = usensor.get_dist_cm();
        if(right<min)
        min=right;
        
      if(min <  obstacleDistance ) 
      {
          autoDrivePartOk = false;
          return min;  
          
      }

    Servo_Look_Center();
    wait_ms(250);
    usensor.start();
    int center = usensor.get_dist_cm();
        if(center<min)
        min=center;
   
     if(min <  obstacleDistance )
     {
        autoDrivePartOk = false;
        return min;   
      }  
      //  printf("Min Distance %d\n",min);
    
    return min;  
} 

  

void Servo_Look_Left()
{
    servo.pulsewidth_us(2000);   
}

void Servo_Look_Right()
{
    servo.pulsewidth_us(1000);   
}

void Servo_Look_Center()
{
    servo.pulsewidth_us(1500);   
}

void DataReceived_ISR(){
   //  char globalChar = LPC_UART2->RBR;
   //  lastCommandChar = LPC_UART2->RBR;
     incomingMsg[incomingMsg_iter++] = LPC_UART2->RBR ;
     
    // globalString += globalChar; 
}

void DataReceived_PCISR(){
   //  char globalChar = LPC_UART2->RBR;
   //  lastCommandChar = LPC_UART2->RBR;
     incomingMsg[incomingMsg_iter++] = LPC_UART0->RBR ;
     
    // globalString += globalChar; 
}


void Servo_NoNo(int count)
{
    for(int i=0 ; i< count ; i++) {
        Servo_Look_Left();
        wait(0.2);
        Servo_Look_Right();
        wait(0.2);
    } 
   Servo_Look_Center();
}

float getCommandValue(string cmd){
    string cmdValue;   
    std::size_t sharpPosition = cmd.find("#");  
    cmdValue = cmd.substr(sharpPosition+1);
 //   printf("cmdvalue %s\n",cmdValue); // - Print the incoming command
    return atof(cmdValue.c_str());
}




void BTCommands(string cmd)
{
   #ifdef DEBUG_BT_COMMANDS
   printf("Incoming BT command : %s\n",cmd);  // - Print the incoming command
   #endif
   if (cmd.find("forward#")!= string::npos)
   {
       float time = getCommandValue(cmd);
       DriveCommand* dc;
       dc = (DriveCommand*)malloc(sizeof(DriveCommand));
       dc->SetTime(time);
       dc->SetDir('f');
       autoDriverQueue.Put(dc);
   }
   else if(cmd.find("backwards#")!= string::npos)
   {
       float time = getCommandValue(cmd);
       DriveCommand* dc;
       dc = (DriveCommand*)malloc(sizeof(DriveCommand));
       dc->SetTime(time);
       dc->SetDir('b');
       autoDriverQueue.Put(dc);
   }
    else if(cmd.find("obstacle#")!= string::npos)
   {
       obstacleDistance = getCommandValue(cmd);
      
   }  
   
   else if (cmd.find("rotleftms#")!= string::npos)
   {
       rotateLeftMillis = getCommandValue(cmd);
       Rotate_Left();  // todo remove after testing
       
   } 
    else if (cmd.find("rotrightms#")!= string::npos)
   {
       rotateRightMillis = getCommandValue(cmd);
       Rotate_Right(); // todo remove after testing
   } 
    else if (cmd.find("turnspeed#")!= string::npos)
   {
       turnSpeed = getCommandValue(cmd);
      
   }    
   else if(cmd.find("rotleft")!= string::npos)
   {    
       Rotate_Left();
   }
   else if(cmd.find("rotright")!= string::npos)
   {    
       Rotate_Right();
   }  
   else if(cmd.find("left#")!= string::npos)
   {
       float time = getCommandValue(cmd);
       DriveCommand* dc;
       dc = (DriveCommand*)malloc(sizeof(DriveCommand));
       dc->SetTime(time);
       dc->SetDir('l');
       autoDriverQueue.Put(dc);
   }  
         else if(cmd.find("right#")!= string::npos)
   {
       float time = getCommandValue(cmd);
       DriveCommand* dc;
       dc = (DriveCommand*)malloc(sizeof(DriveCommand));
       dc->SetTime(time);
       dc->SetDir('r');
       autoDriverQueue.Put(dc);
   }   
   else if (cmd.find("autodrive")!= string::npos)
   {
       autoDriveFlag = true;

   } 
   else if (cmd.find("forward")!= string::npos)
   {
      if(lastDistance < obstacleDistance)
      {
        Servo_NoNo(2);
        return;
       } 
       Drive_Forward(vehicleSpeed);
   }   
   else if (cmd.find("stop")!= string::npos)
   {
       Drive_Stop();
   } 
   else if (cmd.find("backwards")!= string::npos)
   {
       Drive_Backwards(vehicleSpeed);
   }
   else if (cmd.find("tleft")!= string::npos)
   {
       Turn_Left(vehicleSpeed);
   }
   else if (cmd.find("tright")!= string::npos)
   {
       Turn_Right(vehicleSpeed);
   }
   else if (cmd.find("dright")!= string::npos)
   {
       Drive_Right(vehicleSpeed);
   }
   else if (cmd.find("dleft")!= string::npos)
   {
       Drive_Left(vehicleSpeed);
   }
      else if (cmd.find("rleft")!= string::npos)
   {
       Reverse_Left(vehicleSpeed);
   }
      else if (cmd.find("rright")!= string::npos)
   {
       Reverse_Right(vehicleSpeed);
   }
    else if (cmd.find("speed1")!= string::npos)
   {
       vehicleSpeed = 0.25; 
       
   }
      else if (cmd.find("speed2")!= string::npos)
   {
      vehicleSpeed = 0.50; 
       
   }
       else if (cmd.find("speed3")!= string::npos)
   {
      vehicleSpeed = 0.75; 
       
   }
       else if (cmd.find("speed4")!= string::npos)
   {
      vehicleSpeed = 1; 
       
   }  
   else
   {
       Drive_Stop();
    }
    
}

void BTCommands2(char cmd)
{
   #ifdef DEBUG_BT_COMMANDS
   printf("Incoming BT command : %c\n",cmd);  // - Print the incoming command
   #endif

    switch(cmd)
    {
        case 'F' : Drive_Forward(1);    break;
        case 'B' : Drive_Backwards(1);    break;
        case 'S' : Drive_Stop();        break;
        case 'L' : Drive_Left(1);       break;
        case 'R' : Drive_Right(1);      break;  
}
}


void init()
{
    bt.baud(115200);
    bt.attach(&DataReceived_ISR,Serial::RxIrq);  
    pc.attach(&DataReceived_PCISR,Serial::RxIrq);  
    Reset_IncomingMsg();
}


void UltraSonic_Thread_Func(void const *args)
{
    while(1)
    {
      
    //  lastDistance = GetDistance();
    usensor.start(); 
    lastDistance = usensor.get_dist_cm();
      
      if(moveForward_flag)
      { 
        lastDistance = Get_Min_Distance();    
      }

      
      #ifdef DEBUG_ULTRASONICE
      pc.printf("Distance : %f\n",lastDistance );
      #endif
      
        if(lastDistance < obstacleDistance && moveForward_flag ) { // todo : minimize code here
            autoDrivePartOk = false;
            Drive_Force_Stop();
            moveForward_flag = false; 
            Servo_NoNo(1);
        }
        else if((turnRight_Flag || turnLeft_Flag) && lastDistance < obstacleDistance  ) { // todo : minimize code here
            autoDrivePartOk = false;
            Drive_Stop();
            
           // moveForward_flag = false;
        } 

        
      Thread::wait(ULTRASONIC_CHECK_DELAY_MS);     
    }  
}

void UltraSonic_Thread_Emergency(void const *args)
{
    while(1)
    {
    usensor.start(); 
    lastDistance = usensor.get_dist_cm();
    
      #ifdef DEBUG_ULTRASONICE
      pc.printf("Distance : %f\n",lastDistance );
      #endif
      
        if(lastDistance < obstacleDistance && moveForward_flag ) { // todo : minimize code here
            Drive_Force_Stop();
            moveForward_flag = false;
            Servo_NoNo(1);
        }     
      Thread::wait(ULTRASONIC_EMERGENCY_DELAY_MS);     
    }  
}



void ExecuteNextCommand()
{
    DriveCommand* dc = (DriveCommand*)malloc(sizeof(DriveCommand)) ;
  //  printf("Before get from queue\n");
    autoDriverQueue.Get(dc);
  //  printf("after get from queue\n");
    float time = dc->GetTime();
    char dir = dc->GetDir();
  //  printf("AutoDrive Runing at Dir:%c, Time:%d\n",dir,time);
    autoDriveRun = true;
    autoDrivePartOk = true;
    switch(dir)
    {
        case 'f': 
        Drive_Forward(vehicleSpeed);
        break;
        case 'b':
        Drive_Backwards(vehicleSpeed);
        break;
        case 'l':
        Turn_Left(turnSpeed);
        wait_ms(rotateLeftMillis);
        Drive_Forward(vehicleSpeed);
        break;
        case 'r':
        Turn_Right(turnSpeed);
        wait_ms(rotateRightMillis);      
        Drive_Forward(vehicleSpeed);
        break;        
    } 
    
    StopAfter(time);
    free(dc);
    
    while(autoDriveRun)
        wait_ms(50);
        
    if(autoDrivePartOk)
        bt.printf("ok%d~",autoDriveCounter);
    else
        bt.printf("fail%d~",autoDriveCounter);    
 
    autoDriveCounter++;

    
}





void AutoDrive_Thread_Func(void const *args)
{
        while(true) { 
        
        // todo add check for autoDriveFlag
         if(autoDriveFlag && autoDriverQueue.GetNumberOfItems() >0 )
         {
            // printf("Autodrive Commands  before commit\n");
            if(!autoDriveRun) //check that there is no running command right now
                ExecuteNextCommand(); 
            // printf("Autodrive Commands  after commit\n");
             
         }
         else
         {
            autoDriveFlag = false;
            autoDriveCounter =0;            
         }
            
        //printf("Checking for Autodrive Commands\n");
        Thread::wait(AUTODRIVE_CHECK_DELAY_MS);
    }
}




void BT_Thread_Func(void const *args)
{
//    while(true) { 
//        if(globalString.length() > 0 )
//        {
//            BTCommands(globalString);
//            globalString = "";
//        } 
//        Thread::wait(100);
//    }
//    
        while(true) { 
//        if(lastCommandChar != 'x' )
//        {
//            BTCommands2(lastCommandChar);
//            lastCommandChar = 'x';
//        }
        
         string cmd = Get_last_Msg();
         if(succesReadFlag)
         {
             
             BTCommands(cmd);
             succesReadFlag = false;
         }        
        
        Thread::wait(BT_CHECK_DELAY_MS);
    }
}


int main()
{
 //   mu.startUpdates();//start mesuring the distance
    
    init();
    printf("Hello\n");
    Thread btThread(BT_Thread_Func); 
    btThread.set_priority(osPriorityNormal);
    Thread ultraSonicThread(UltraSonic_Thread_Func);
    ultraSonicThread.set_priority(osPriorityNormal);
    Thread ultraSonicEmergencyThread(UltraSonic_Thread_Emergency);
    ultraSonicEmergencyThread.set_priority(osPriorityNormal); 
    Thread AutoDriveThread(AutoDrive_Thread_Func);
    AutoDriveThread.set_priority(osPriorityNormal);
    
    
    
         
   Servo_Look_Left();
   wait(2);   
   Servo_Look_Right();
   wait(2);    
   Servo_Look_Center();
   
      
 //  Drive_Forward(0.5);
   
 //  StopAfter(3.0);
    
       
  //     autoDriverQueue.Put(&DriveCommand('f',120)); 
                 
    while(1)
    {
    //   mu._checkDistance();   
        //Do something else here
   //     mu.checkDistance();     //call checkDistance() as much as possible, as this is where  //the class checks if dist needs to be called.
    }
       
}