How to make robot stops when detects obstacle (C++, Raspberry PI, TCP communication)

manpreet Best Answer 2 years ago

I need help. I've made a robot that finds the shortest path from point A to point B and it works perfectly. Now I want to implement an ultrasonic sensor on this robot so it can detect obstacles in front of it. When it detects the obstacle the robot should stop moving and wait until the obstacle is removed. I'm controlling the robot using TCP server/client communication (the robot is the client ). Now, like this, the robot does just the first movement and then stops even if there's no obstacle in front of it. The controlling and all the algorithm is done on the Server (PC) the client (Raspberry Pi) just executes the commands. Now, the idea is while the robot is travelling from point A to point B IF it detects any obstacle in front of him the robot should stop and wait until the obstacle is removed, but it should send also something to the server to stop the program i.e not to be executed, to wait until the robot sends another signal that obstacle has been removed.

I hope you get the idea. Here you have the client code and part of the server code where I do all this controlling stuff (these are the functions), but It doesn't work as I said properly. Also, I don't know how to debug it.

This is the client code:

#include 
#include types.h>
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#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(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
                                                

                                                                                                        
                                                    
                                                
                                                

                                                    

                                                        
                                                            0
                                                            views 

                                                            0
                                                                likes 
                                                            
                                                            

                                                            0
                                                            shares
                                                            


    

         Facebook
        
         Twitter
        
        
         Linked In 
        
          WhatsApp