title: How to make robot stops when detects obstacle (C++, Raspberry PI, TCP communication)
body: <p>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.</p>
  
  <p>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.</p>
  
  <p>This is the client code:</p>
  
  <pre class="lang-cpp prettyprint-override"><code>#include &lt;iostream&gt;
  #include &lt;sys/types.h&gt;
  #include &lt;unistd.h&gt;
  #include &lt;sys/socket.h&gt;
  #include &lt;netdb.h&gt;
  #include &lt;arpa/inet.h&gt;
  #include &lt;string.h&gt;
  #include &lt;string&gt;
  #include &lt;wiringPi.h&gt;
  #include &lt;softPwm.h&gt;
  #include &lt;chrono&gt;
  #include &lt;thread&gt;
  #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 &lt;&lt; "Initialization problem - measureDistance() " &lt;&lt; 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&amp; packet) {
      string header = string(1, static_cast&lt;char&gt;(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 &lt;&lt; "Distance: " &lt;&lt; distance &lt;&lt; endl;
      if (distance &gt;= 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 &lt; 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(), &amp;hint.sin_addr);
      //  Connect to the server on the socket
      int connectRes = connect(sock, (sockaddr*)&amp;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 &lt;&lt; "String: " &lt;&lt; received &lt;&lt; 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 &lt;&lt; "&gt;&gt;&gt; " &lt;&lt; enA &lt;&lt; endl;  
          cout &lt;&lt; "&gt;&gt;&gt; " &lt;&lt; in1 &lt;&lt; endl;
          cout &lt;&lt; "&gt;&gt;&gt; " &lt;&lt; in2 &lt;&lt; endl;
  
          cout &lt;&lt; "&gt;&gt;&gt; " &lt;&lt; enB &lt;&lt; endl;
          cout &lt;&lt; "&gt;&gt;&gt; " &lt;&lt; in3 &lt;&lt; endl;
          cout &lt;&lt; "&gt;&gt;&gt; " &lt;&lt; in4 &lt;&lt; endl;
  
          cout &lt;&lt;"&gt;&gt;&gt; " &lt;&lt; t &lt;&lt; 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&lt;chrono::milliseconds&gt;(now-start).count();
              int remaining = t - (int) elapsed;
  
              if (remaining &lt; 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;
  
  }
  </code></pre>
  
  <p>And here's the servers code:</p>
  
  <pre class="lang-cpp prettyprint-override"><code>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&amp; packet) 
  {
      return static_cast&lt;PacketType&gt;(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 &lt;&lt; "Received: " &lt;&lt; packet &lt;&lt; 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();
  }
  </code></pre>
  
  <p>I hope you understand the idea and I will appreciate if you want and have time to help me.</p>