当机器人车用超声波传感器(C++）检测到物体时，停止机器人车

Stop robot car when it detects object with ultrasound sensor (C++)

本文关键字：机器人 超声波传感器 C++ 检测      更新时间：2023-10-16

最近，当我的机器人检测到前方有障碍物时，我一直试图让它停下来。接下来的情况是：当我检测到物体时，汽车会停下来，这没关系，但当物体消失时，汽车不会继续移动到最终目的地。与其这样做，汽车应该从A点到B点行驶10秒(10秒只是一个例子，每次移动的时间都不同(，当它检测到物体时，它应该停下来等待，直到物体消失，然后它必须继续到达最终目的地(B点(。

我有一个想法，当机器人不移动时，计算时间，并将该时间添加到到达终点所需的时间中。但我正在为此而挣扎。

这是代码：

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);
}

继续：当汽车行驶时，如果我在它前面放了一个障碍物，它就会停止，但当我清除障碍物时，程序结束——之后汽车就不动了这不应该发生

第页。S： 一切都在树莓派上运行。

这是一种伪代码，因为我不知道你的机器人和wiringPi是如何工作的，但我希望这个代码流能帮助你走上正轨：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
*/
}
相关文章：