Made by Maor and Shai
Dependencies: DriveCommand HCSR04 Queue mbed-rtos mbed
Fork of Nucleo_UltrasonicHelloWorld by
Revision 2:9a4938a0475b, committed 2016-07-04
- Comitter:
- Maor_T
- Date:
- Mon Jul 04 15:28:11 2016 +0000
- Parent:
- 1:4a5586eb1765
- Commit message:
- SmartRobotCar;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/DriveCommand.lib Mon Jul 04 15:28:11 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/Maor_T/code/DriveCommand/#9b6dc5b1dc33
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HCSR04.lib Mon Jul 04 15:28:11 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/prabhuvd/code/HCSR04/#71da0dbf4400
--- a/HC_SR04_Ultrasonic_Library.lib Thu Dec 04 08:04:55 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://developer.mbed.org/users/ejteb/code/HC_SR04_Ultrasonic_Library/#e0f9c9fb4cf3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Queue.lib Mon Jul 04 15:28:11 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/Maor_T/code/Queue/#4770d82994dd
--- 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.
}
+
}
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Mon Jul 04 15:28:11 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/Maor_T/code/mbed-rtos/#78d11dd53a40
--- a/mbed.bld Thu Dec 04 08:04:55 2014 +0000 +++ b/mbed.bld Mon Jul 04 15:28:11 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89 \ No newline at end of file +http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34 \ No newline at end of file
