c++ - raspberry pi - 使用超声波传感器(C++ )检测到物体时停止机器人汽车

最近我一直试图让机器人在检测到前方有障碍物时停止。

下面是代码:


int measureDistance()


{


 if (wiringPiSetup() == -1)


 cout <<"Initialization problem - measureDistance()" << endl;



 Sonar sonar;


 sonar.init(trigger, echo);



 int distance = 0;



 distance = sonar.distance(30000);


 sleep_for(nanoseconds(10));



 return distance;


}



bool checkForObstacles() 


{


 wiringPiSetup();



 // Controlling the motors from here


 softPwmCreate(0, 0, 255);


 softPwmCreate(4, 0, 255);



 constexpr int MIN_DISTANCE = 20;



 int distance = measureDistance();


 cout <<"Distance:" << distance << endl;



 if (distance >= MIN_DISTANCE)


 return false;



 softPwmWrite(0, LOW);


 softPwmWrite(4, LOW);



 while(distance < MIN_DISTANCE) 


 {


 delay(10); // re-measure after 10ms. Adjust to what you prefer


 distance = measureDistance();


 cout <<"Measuring:" << distance <<"cm" << endl;


 }



 return false;


}



void move(int t)


{


 // Pins where the motors are connected


 int ena = 0;


 int in1 = 2;


 int in2 = 3;



 int enb = 4;


 int in3 = 5;


 int in4 = 6;


 // Pins setup


 wiringPiSetup ();



 softPwmCreate(ena, 0, 255);


 pinMode(in1, OUTPUT);


 pinMode(in2, OUTPUT);



 softPwmCreate(enb, 0, 255);


 pinMode(in3, OUTPUT);


 pinMode(in4, OUTPUT);



 // Control


 softPwmWrite(ena, 50);


 digitalWrite(in1, 1);


 digitalWrite(in2, 0);



 softPwmWrite(enb, 50);


 digitalWrite(in3, 1);


 digitalWrite(in4, 0);



 cout <<"TIME:" << t << endl;


 auto start = chrono::high_resolution_clock::now();


 while(true) 


 {


 auto now = chrono::high_resolution_clock::now();


 auto elapsed = chrono::duration_cast<chrono::milliseconds>(now-start).count();



 cout <<"ELAPSED:" << elapsed << endl;



 int remaining = t - (int) elapsed;



 cout <<"REMAINING:" << remaining <<"ms" << endl;


 if (remaining < 0)


 break;



 if (checkForObstacles())


 {


 continue;


 } 



 delay(min(remaining, 25)); // replace 25 with how often you want to check the distance


 }



 softPwmWrite(ena, LOW);


 softPwmWrite(enb, LOW);


 delay(200);


}



P.S :所有内容都在树莓派上运行。

时间:

这是伪代码,因为我不知道你的机器人和Pi的工作原理如何,但是希望这段代码为你提供一点帮助:measureDistance似乎毫无用处,所以我将它删除(由于某种原因,它每次都初始化),


int main()


{


 // only initialize everything once, in your main


 if (wiringPiSetup() == -1)


 return -1;



 Sonar sonar;


 sonar.init(trigger, echo); // declare these somewhere first, I don't know the values



 while(1){


 sleep_for(nanoseconds(10)); // or delay(200) or whatever


 // if there are obstacles skip the move() part and go back to sleeping


 // or delay again, and try again


 if (checkForObstacles(sonar))


 continue;


 // No obstacle = move


 move();


 }


}



// this is only responsible for checking obstacles, no pin writing and moving happening


bool checkForObstacles(Sonar sonar) // pass in a reference to sonar


{


 constexpr int MIN_DISTANCE = 20;



 int distance = sonar.distance(30000);


 cout <<"Distance:" << distance << endl;



 if (distance >= MIN_DISTANCE)


 return false;



 return true;


}



void move() {


 /*


 * only move logic goes here


 * like the digitalWrites


 */


}



...