Made by Maor and Shai
Dependencies: DriveCommand HCSR04 Queue mbed-rtos mbed
Fork of Nucleo_UltrasonicHelloWorld by
main.cpp
- Committer:
- Maor_T
- Date:
- 2016-07-04
- Revision:
- 2:9a4938a0475b
- Parent:
- 1:4a5586eb1765
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. } }