c++ - 如何让机器人在检测到障碍物时停止（C++、Raspberry PI、TCP 通信）

Question

我需要帮助。我制作了一个机器人，它可以找到从 A 点到 B 点的最短路径，并且运行良好。现在我想在这个机器人上实现一个超声波传感器，这样它就可以检测到它前面的障碍物。当它检测到障碍物时，机器人应该停止移动并等待障碍物被移除。我正在使用 TCP 服务器/客户端通信控制机器人（机器人是客户端）。现在，像这样，机器人只做第一个动作，然后即使前面没有障碍物也会停下来。控制和所有算法都在服务器（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();
}

我希望你能理解这个想法，如果你愿意并且有时间帮助我，我将不胜感激。

标签： c++tcpraspberry-pitcpclienttcpserver