Why is my Arduino code for running a Bluetooth controlled robot with obstacle sensor not working as it should be?

Question

I have created a Bluetooth based smartphone controlled robot (a 4 wheel car) earlier using Arduino and an obstacle avoidance robot. Now, I wanted to combine both of them. So, I combined their functions and codes in such a manner that felt right to me. But, it kinda seems wrong to my robot. My Bluetooth Controlled robot was running smoothly. My obstacle avoiding robot was running flawless. But, when I tried to join them, both of them started crying.

I tried to change orders of the functions, adding some functions twice or thrice removing them from some places that didn't feel right then. Nothing was worthy.

I need to upload whole code because I'm not sure where am I doing it wrong

#include <SoftwareSerial.h> 

#include <RemoteXY.h> 

// RemoteXY connection settings  
#define REMOTEXY_SERIAL_RX A2 
#define REMOTEXY_SERIAL_TX A3 
#define REMOTEXY_SERIAL_SPEED 9600 


// RemoteXY configurate   
#pragma pack(push, 1) 
uint8_t RemoteXY_CONF[] = 
  { 255,8,0,54,0,176,0,8,228,4,
  5,48,44,26,30,30,0,31,8,1,
  6,0,-84,-142,20,20,0,2,26,129,
  0,6,37,23,8,0,64,78,97,118,
  101,100,0,131,3,51,1,20,5,1,
  2,31,82,111,98,111,116,32,67,97,
  114,0,131,0,62,5,19,5,2,2,
  31,67,111,110,116,114,111,108,108,101,
  114,0,129,0,10,45,13,7,0,16,
  84,72,69,0,129,0,5,52,25,8,
  0,136,83,104,101,105,107,104,0,1,
  0,-34,-111,12,12,1,2,31,88,0,
  1,4,79,44,12,12,0,37,151,240,
  159,147,162,0,65,4,87,9,7,7,
  0,65,1,87,17,7,7,0,65,2,
  87,25,7,7,0,67,5,3,3,25,
  14,0,94,24,51,2,0,86,1,11,
  5,0,135,26,31,31,79,78,0,79,
  70,70,0 }; 

// this structure defines all the variables of your control interface  
struct { 

    // input variable
  int8_t joystick_1_x; // =-100..100 x-coordinate joystick position 
  int8_t joystick_1_y; // =-100..100 y-coordinate joystick position 
  uint8_t rgb_1_r; // =0..255 Red color value 
  uint8_t rgb_1_g; // =0..255 Green color value 
  uint8_t rgb_1_b; // =0..255 Blue color value 
  uint8_t button_1; // =1 if button pressed, else =0 
  uint8_t button_2; // =1 if button pressed, else =0 
  uint8_t switch_1; // =1 if switch ON and =0 if OFF 

    // output variable
  uint8_t red_led_r; // =0..255 LED Red brightness 
  uint8_t blue_led_b; // =0..255 LED Blue brightness 
  uint8_t green_led_g; // =0..255 LED Green brightness 
  char text_indicator[51];  // string UTF8 end zero 

    // other variable
  uint8_t connect_flag;  // =1 if wire connected, else =0 

} RemoteXY; 
#pragma pack(pop) 

///////////////////////////////////////////// 
//           END RemoteXY include          // 
///////////////////////////////////////////// 

#define PIN_BUTTON_2 A4
#define PIN_SWITCH_1 A5



#include<AFMotor.h>
#include <NewPing.h>
#include <Servo.h> 

#define TRIG_PIN A0 
#define ECHO_PIN A1 
#define MAX_DISTANCE 250
//#define MAX_SPEED 150 // sets speed of DC  motors
//#define MAX_SPEED_OFFSET 20

NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); 

AF_DCMotor left_motor_A(1, MOTOR12_64KHZ); 
AF_DCMotor left_motor_B(2, MOTOR12_64KHZ);
AF_DCMotor right_motor_A(3, MOTOR34_64KHZ);
AF_DCMotor right_motor_B(4, MOTOR34_64KHZ);
Servo myservo;   

int distance = 100;

int right_motor_speed = 0;
int left_motor_speed = 0;
//define two arrays with a list of pins for each motor
AF_DCMotor RightMotor[2] = {right_motor_A, right_motor_B};
AF_DCMotor LeftMotor[2] = {left_motor_A, left_motor_B};


//speed control of motors
void Wheel (AF_DCMotor * motor, int v) // v = motor speed, motor = pointer to an array of pins 
{
  if (v > 100) v=100;
  if (v < -100) v=-100;
  if (v > 0){

    motor[0].run(FORWARD);
    motor[1].run(FORWARD);
    motor[0].setSpeed(v * 1.75);
    motor[1].setSpeed(v * 1.75);
  }
  else if ( v<0 ){

    motor[0].run(BACKWARD);
    motor[1].run(BACKWARD);
    motor[0].setSpeed(v * 1.75);
    motor[1].setSpeed(v * 1.75);
/*    //digitalWrite (motor [1], FORWARD);
    analogWrite (motor [2], (v) * 0.75);
    //analogWrite (motor [2], (-v) * 0.75); */
  }
  else{
    motor[0].run(RELEASE);
    motor[1].run(RELEASE);
    motor[0].setSpeed(0);
    motor[1].setSpeed(0);
  }
}

 int lookRight()
 {
    RemoteXY.blue_led_b = 255;
    RemoteXY.red_led_r = 0;
    RemoteXY.green_led_g = 0;
    sprintf(RemoteXY.text_indicator, "CHECKING THE RIGHT SIDE.");
    myservo.write(50); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115); 
    return distance;
 }

 int lookLeft()
 {
    RemoteXY.blue_led_b = 255;
    RemoteXY.red_led_r = 0;
    RemoteXY.green_led_g = 0;
    sprintf(RemoteXY.text_indicator, "CHECKING THE LEFT SIDE.");
    myservo.write(170); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115); 
    return distance;
    delay(100);
 }

 int readPing() 
 { 
  delay(100);
  int cm = sonar.ping_cm();
   if(cm==0)
  {
    cm = MAX_DISTANCE ;
  } 
  return cm;
 }

 void moveStop() 
 {
  left_motor_A.run(RELEASE); 
  left_motor_B.run(RELEASE);
  right_motor_A.run(RELEASE);
  right_motor_B.run(RELEASE);

 } 



void setup()  
{ 
  RemoteXY_Init ();  

  pinMode (PIN_BUTTON_2, OUTPUT);
  pinMode (PIN_SWITCH_1, OUTPUT);

  myservo.attach(10);  
  myservo.write(115); 
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);


} 

void loop()  
{  
  RemoteXY_Handler (); 

  digitalWrite(PIN_BUTTON_2, (RemoteXY.button_2==0)?LOW:HIGH);
  digitalWrite(PIN_SWITCH_1, (RemoteXY.switch_1==0)?LOW:HIGH);

    int distanceR = 0;
    int distanceL =  0;
    delay(40);

    if(distance<=27)
    {
      moveStop();
      RemoteXY.blue_led_b = 0;
      RemoteXY.red_led_r = 255;
      RemoteXY.green_led_g = 0;
      sprintf(RemoteXY.text_indicator, "AN OBSTACLE HAS COME IN FRONT OF YOUR ROBOT!!!");
      digitalWrite(PIN_BUTTON_2, HIGH);
      delay(1500);
      distanceR = lookRight();
      delay(250);
      distanceL = lookLeft();
      delay(250);

      if(distanceR>=distanceL)
      {
        RemoteXY.blue_led_b = 255;
        RemoteXY.red_led_r = 0;
        RemoteXY.green_led_g = 0;

        sprintf(RemoteXY.text_indicator, "IT IS GOOD TO GO ON THE RIGHT SIDE.");
      }

      else
      {
        sprintf(RemoteXY.text_indicator, "IT IS GOOD TO GO ON THE LEFT SIDE.");
      } 
    }
    else
    {
      RemoteXY.blue_led_b = 0;
      RemoteXY.red_led_r = 0;
      RemoteXY.green_led_g = 255;
      //manage the right motor
      Wheel (LeftMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
      Wheel (RightMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);

    }
    distance = readPing();

    //manage the right motor
    //Wheel (LeftMotor, RemoteXY.joystick_1_y - RemoteXY.joystick_1_x);
    //Wheel (RightMotor, RemoteXY.joystick_1_y + RemoteXY.joystick_1_x);


  }

I want my Robot to work on my directions and when he senses something on its front end then he should stop there and tell me through a RemoteXY.text_string and then check for obstacles at the left and right side and tell me on which side it is available to go. After that, it should again take my orders via my mobile phone.

Firstly, it became impossible to connect the robot to the Smartphone using Bluetooth because the robot was very busy in doing some other stuffs (only he knows what was he doing). Then, I applied a "RemoteXY Button" on my code (that is not in the code that I'm posting here) with an "if" block; such that if this switch is pressed via the Smartphone, only then the robot will start moving. So, doing this thing helped me to connect my robot to my smartphone via Bluetooth but, when I was connection then the Servo Motor was regularly spinning (again, I don't know why) after secure connection, when I turned that switch on, then my robot took only my first order (which is usually moving forward) and it performed on my first order for almost first 3 to 5 seconds and then the Robot got his system hanged and my smartphone was automatically disconnected from the robot.

I am unable to figure out where is the filth in my code or my code is itself fully filthy. This is the matter, where I need you guys' help

Other information that you might need for better Support/help, I am using: 1). RemoteXY app for Android to Robot Communication 2). Arduino UNO as Microcontroller 3). Arduino IDE for Programming Arduino 4). One SG 90 Servo Motor. 5). Four Geared DC- Motors. 6). L293D Motor Driver Shield for driving Dc motors and the Servo 7). HC SR-04 ultrasonic sensor for obstacle sensing 8). A 12V battery 9). HC-05 Module for Bluetooth Communication 10). A buzzer for external indicator/horn. At pin A4 11). A filthy code for Arduino Programming 😜.

Any help with the code will be appreciated.

Thank You So Much in advance. ❤️

Answer 1

My guess is, that the function RemoteXY_Handler() has to be called very regularly (without long pauses in between) to communicate with the app on your smartphone, or the app will think, that the communication is lost, and disconnect from the robot. This means, that you cannot use long delay() calls in your code, since that is just busy waiting. In your code in case of an obstacle in front of the robot, you wait 3.6s in total. That is too much. The robot is doing nothing in this time. Instead you should use a non-blocking coding style. That means, that the code will not block, waiting (and doing nothing) until an event happends (for example a specific amount of time passed), but instead will execute other functions, until the event occurs. That is a generally better way to program a microcontroller, since it enables you to add more functionalities easily without disturbing the program flow.

Here you try to do timed things with delay(). Instead you should use a coding style, like the one found in the BlinkWithoutDelay example, that comes with the Arduino IDE. It is explained various times throughout the internet and this site. Basically it is like regularily looking at your watch (the millis() function here) and only doing things, when the time has come. You should look at the example and maybe go through some of the tutorials about this, so that you really understand, what happens there. Then you can start to rewrite your code and eliminate all delay() calls. That will need some restructuring of the code.

---

The BlinkWithoutDelay example is an easy structure, since it contains only few logic. Your goal is a bit more complex. So I suggest, that you use a Finite State Machine (FSM). It is rather easy to implement and a powerful tool to structure your code. It's definitely worth learning for every programmer.

A FSM consists of multiple states, that are connected by transitions. In each state there can be code executed. Also there might be code, that is only executed on transitions. First you have to model your project into states. One state here could be REMOTE_CONTROLLED, which would just execute the commands coming from the app. Based on input from sensors and the app you can then transition between the states. If you see, that you might need a delay() inside a state, that might be a hint, that you can further divide the state into substates. For example I would make an extra state, which gives the servo the time to move. I would suggest, that you really draw a graph for your project with all your states (state name in a bubble). Then draw arrows as state transitions between them. This will give you a good overview of the structure to implement.

In code this is often implemented with a switch() case structure and a state variable:

int state=0;
int distance = 100;

void loop(){
    RemoteXY_Handler ();
    distance = readPing();

    switch(state){
        case 0:
            // Do whatever the remote demands here
            if(distance <= 27){
                // Execute code, that should happen once at transition here
                state = 1; // This makes the transition to the next state
            }
        case 1:
            // Code of the next state
    }
}

With this structure at every time only the code of the current state will be executed. The transitions are made simply by setting the state variable to the next desired state. To make the states more speaking in the code, you can use an enum, so that every state can be referenced with an enum name. The remote handler and the reading of the distance is done outside the FSM here, since you always need the input from those. Based on them, you can do state transitions.

To do timed things in an FSM, you can put the millis() code (like in the BlinkWithoutDelay example) into the corresponding state.

---

Note, that this is only one way to implement a FSM. The video, that you linked, does this a bit different by using functions pointers (the state() function pointer is the state variable here). The principle is the same, only the programming structure is different. You can decide yourself, what works better for you, and what you understand better (pointers can be a bit difficult to understand for beginners).