我需要帮助。我制作了一个机器人，该机器人可以找到从A点到B点的最短路径，并且可以完美运行。现在，我想在此机器人上安装一个超声波传感器，以便它可以检测到它前面的障碍物。当它检测到障碍物时，机器人应停止移动并等待直到障碍物被清除。我正在使用TCP服务器/客户端通信来控制机器人（机器人是client）。现在，像这样，机器人只执行第一个动作，然后即使前面没有障碍物也停下来。控制和所有算法都在服务器（PC）上执行，而客户端（Raspberry Pi）仅执行命令。现在，想法是当机器人从A点移动到B点时，如果它检测到前方有任何障碍物，则机器人应停止并等待障碍物被清除，但它还应向服务器发送一些信息以停止程序即不要执行，要等到机器人发出另一个信号表明障碍物已被清除。
希望您能明白。在这里，您有客户端代码和服务器代码的一部分，我在其中进行所有这些控制工作（这些是函数），但是它不能正常运行。而且，我不知道如何调试它。
这是客户端代码：
#include <iostream>
#include <sys/types.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <string.h>
#include <string>
#include <wiringPi.h>
#include <softPwm.h>
#include <chrono>
#include <thread>
#include "libSonar.h"
enum class PacketType : char {
ROBOT_STUCK = '1',
ROBOT_UNSTUCK = '2'
};
using namespace std;
using namespace std::this_thread; // sleep_for()
using namespace std::chrono;
int trigger = 28;
int echo = 29;
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;
}
string appendHeader(PacketType type, const string& packet) {
string header = string(1, static_cast<char>(type));
return header + packet;
}
bool checkForObstacles(int socket, int instructionNumber)
{
wiringPiSetup();
// Controlling the motors from here
softPwmCreate(0, 0, 255);
softPwmCreate(4, 0, 255);
string packetContents = appendHeader(PacketType::ROBOT_STUCK, to_string(instructionNumber));
constexpr int MIN_DISTANCE = 5;
int distance = measureDistance();
//cout << "Distance: " << distance << endl;
if (distance >= MIN_DISTANCE)
return false;
softPwmWrite(0, LOW);
softPwmWrite(4, LOW);
int sendRes = send(socket, packetContents.c_str(), packetContents.size() + 1, 0);
delay(1000);
while(distance < MIN_DISTANCE)
{
delay(10); // re-measure after 10ms. Adjust to what you prefer
distance = measureDistance();
}
packetContents = appendHeader(PacketType::ROBOT_UNSTUCK, "");
sendRes = send(socket, packetContents.c_str(), packetContents.size() + 1, 0);
delay(1000);
return true;
}
int main()
{
// Create a socket
int sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock == -1)
{
return 1;
}
// Create a hint structure for the server we're connecting with
int port = 54000;
string ipAddress = "192.168.1.3";
sockaddr_in hint;
hint.sin_family = AF_INET;
hint.sin_port = htons(port);
inet_pton(AF_INET, ipAddress.c_str(), &hint.sin_addr);
// Connect to the server on the socket
int connectRes = connect(sock, (sockaddr*)&hint, sizeof(hint));
if (connectRes == -1)
{
return 1;
}
char buf[4096];
memset(buf, 0, 4096);
int CountReceived = 0;
while(true)
{
int bytesReceived = recv(sock, buf, 4096, 0);
string received = string(buf, bytesReceived); // receiving string from the Server
CountReceived++;
cout << "String: " << received << endl;
string a = received.substr(0,3); // Getting substrings from the string
string b = received.substr(3,1);
string c = received.substr(4,1);
string d = received.substr(5,3);
string e = received.substr(8,1);
string f = received.substr(9,1);
string g = received.substr(10,4);
int enA = stoi(a); // converting substrings to integers so I can control the motors
int in1 = stoi(b);
int in2 = stoi(c);
int enB = stoi(d);
int in3 = stoi(e);
int in4 = stoi(f);
int t = stoi(g); // Time needed to work motors
cout << ">>> " << enA << endl;
cout << ">>> " << in1 << endl;
cout << ">>> " << in2 << endl;
cout << ">>> " << enB << endl;
cout << ">>> " << in3 << endl;
cout << ">>> " << in4 << endl;
cout <<">>> " << t << endl;
// Pins for controlling the motors
int enableA = 0;
int input1 = 2;
int input2 = 3;
int enableB = 4;
int input3 = 5;
int input4 = 6;
wiringPiSetup();
// Controlling the motors from here
softPwmCreate(enableA, 0, 255);
pinMode(input1, OUTPUT);
pinMode(input2, OUTPUT);
softPwmCreate(enableB, 0, 255);
pinMode(input3, OUTPUT);
pinMode(input4, OUTPUT);
softPwmWrite(enableA, enA);
digitalWrite(input1, in1);
digitalWrite(input2, in2);
softPwmWrite(enableB, enB);
digitalWrite(input3, in3);
digitalWrite(input4, in4);
//delay(t);
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();
int remaining = t - (int) elapsed;
if (remaining < 0)
break;
if (checkForObstacles(sock, CountReceived))
continue;
delay(min(remaining, 25)); // replace 25 with how often you want to check the distance
}
softPwmWrite(enableA, LOW);
softPwmWrite(enableB, LOW);
delay(1000);
received.clear();
}
// Close the socket
close(sock);
return 0;
}
这是服务器代码：
enum class PacketType : char {
ROBOT_STUCK = '1',
ROBOT_UNSTUCK = '2'
};
std::string receivePacket(SOCKET socket)
{
constexpr int bufLen = 4096;
static char buf[bufLen];
int bytesReceived = recv(socket, buf, bufLen, 0);
Sleep(100);
return std::string(buf, bytesReceived);
}
PacketType getPacketHeader(const std::string& packet)
{
return static_cast<PacketType>(packet[0]);
}
void waitForUnstuck(SOCKET socket)
{
while (true)
{
std::string packet = receivePacket(socket);
if (getPacketHeader(packet) == PacketType::ROBOT_UNSTUCK)
return;
}
}
void receivePackets(SOCKET socket)
{
std::string packet = receivePacket(socket);
cout << "Received: " << packet << endl;
switch (getPacketHeader(packet))
{
case PacketType::ROBOT_STUCK:
waitForUnstuck(socket);
break;
}
}
void move(SOCKET clientSocket, std::string str)
{
send(clientSocket, str.c_str(), str.size() + 1, 0);
Sleep(4000);
str.clear();
}
希望您能理解这个主意，如果您愿意并有时间帮助我，我将不胜感激。