Diff: main.cpp

Revision:

2:9a4938a0475b

Parent:

1:4a5586eb1765

--- a/main.cpp	Thu Dec 04 08:04:55 2014 +0000
+++ b/main.cpp	Mon Jul 04 15:28:11 2016 +0000
@@ -1,5 +1,166 @@
 #include "mbed.h"
-#include "ultrasonic.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)
 {
@@ -7,17 +168,646 @@
     printf("Distance changed to %dmm\r\n", distance);
 }
 
-ultrasonic mu(D8, D9, .1, 1, &dist);    //Set the trigger pin to D8 and the echo pin to D9
-                                        //have updates every .1 seconds and a timeout after 1
-                                        //second, and call dist when the distance changes
+
+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
+ //   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.
+   //     mu.checkDistance();     //call checkDistance() as much as possible, as this is where  //the class checks if dist needs to be called.
     }
+       
 }
+
+
+