Как заставить робота останавливаться при обнаружении препятствия (C ++, Raspberry PI, TCP-связь) - PullRequest
Новая
       70

Как заставить робота останавливаться при обнаружении препятствия (C ++, Raspberry PI, TCP-связь)

0 голосов
/ 08 апреля 2019

Мне нужна помощь.Я сделал робота, который находит кратчайший путь из точки А в точку Б, и он отлично работает.Теперь я хочу установить ультразвуковой датчик на этом роботе, чтобы он мог обнаруживать препятствия перед ним.Когда он обнаруживает препятствие, робот должен прекратить движение и ждать, пока препятствие не будет устранено.Я управляю роботом по протоколу TCP-сервер / клиент (робот - это клиент).Теперь, как это, робот делает только первое движение и затем останавливается, даже если перед ним нет препятствий.Управление и весь алгоритм выполняются на сервере (ПК), клиент (Raspberry Pi) просто выполняет команды.Идея заключается в том, что пока робот движется из точки А в точку Б, ЕСЛИ он обнаруживает любое препятствие перед ним, робот должен остановиться и подождать, пока препятствие будет удалено, но он также должен отправить что-то на сервер, чтобы остановить программу.т.е. не должен выполняться, чтобы ждать, пока робот не подаст другой сигнал, что препятствие было удалено.

Я надеюсь, что вы поняли идею.Здесь у вас есть клиентский код и часть серверного кода, где я делаю все эти контрольные вещи (это функции), но это не работает, как я сказал правильно.Кроме того, я не знаю, как его отладить.

Это код клиента: 

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

НадеюсьВы понимаете идею, и я буду признателен, если вы хотите, и у вас есть время, чтобы помочь мне.

...