Add HC-SR04 into Self Balancing Robot

I want to add HC-SR04 in my self balancing robot - where it won't collide "wall" in a certain distance.

The previous code is as follows:

        if (input>150 && input<200){//If the Bot is falling
             
            if (output>0){ //Falling towards front
              Forward(); //Rotate the wheels forward
              
              }
            else if (output<0){ //Falling towards back
              Reverse(); //Rotate the wheels backward
              }
            }
        else //If Bot not falling
        Stop(); //Hold the wheels still
        }

Then I add new void Loop_SR04(){ .... } - here's the loop:

void Loop_sr04(){
   //delay(40);
   unsigned int distance = sonar.ping_cm();
   Serial.print(distance);
   Serial.println("cm");
}

Then I try to insert the Loop_sr04 into the previous code, like this:

        if (input>150 && input<200){//If the Bot is falling
             
            if (output>0){ //Falling towards front
              Forward(); //Rotate the wheels forward
              Loop_sr04();
              }
            else if (output<0){ //Falling towards back
              Reverse(); //Rotate the wheels backward
              Loop_sr04();			  
              }
            }
        else //If Bot not falling
        Stop(); //Hold the wheels still
        }

But, the mpu6050 stops working and only shows the work of HC-SR04 plus FIFO Overflow! information in COM4.

What I want is:

  1. To get distance while the robot is running.
  2. To make "IF" in 10 CMs will make the robot going backward (to avoid collision).

Note: I have declared the pinout of SR04 as in:

#define TRIGGER_PIN 3
#define ECHO_PIN 13
#define MAX_DISTANCE 200

// NewPing setup of pins and maximum distance
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

and, in void_setup() as in:

Serial.begin (9600); // sonar SR04 

ADD (Full code is like this below):

/*Arduino Self Balancing Robot
 * Code by: B.Aswinth Raj
 * Build on top of Lib: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
 * Website: circuitdigest.com 
 */
#include <NewPing.h>
#include "I2Cdev.h"
#include <PID_v1.h> //From https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
#include "MPU6050_6Axis_MotionApps20.h" //https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

MPU6050 mpu;

#define TRIGGER_PIN 3
#define ECHO_PIN 13
#define MAX_DISTANCE 200

// NewPing setup of pins and maximum distance
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

/*********Tune these 4 values for your BOT*********/
double setpoint= 177.9;
double Kp = 21;
double Kd = 1;
double Ki = 60;
/******End of values setting*********/

double input, output;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady()
{
    mpuInterrupt = true;
}

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
  #endif

  Serial.begin (9600); // sonar SR04  
  Serial.begin(115200); // MPU6050

  // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

     // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

    // load and configure the DMP
    devStatus = mpu.dmpInitialize();

    
    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1688); 

      // make sure it worked (returns 0 if so)
    if (devStatus == 0)
    {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
        
        //setup PID
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-255, 255);  
    }
    else
    {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

//Initialise the Motor outpu pins
    pinMode (6, OUTPUT);
    pinMode (9, OUTPUT);
    pinMode (10, OUTPUT);
    pinMode (11, OUTPUT);

//By default turn off both the motors
    analogWrite(6,LOW);
    analogWrite(9,LOW);
    analogWrite(10,LOW);
    analogWrite(11,LOW);
}

void loop() {
  
 
          
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize)
    {
        //no mpu data - performing PID calculations and output to motors     
        pid.Compute();   
        
        //Print the value of Input and Output on serial monitor to check how it is working.
        Serial.print(input); Serial.print(" =>"); Serial.println(output);
               
        if (input>150 && input<200){//If the Bot is falling
             
            if (output>0){ //Falling towards front
              Forward(); //Rotate the wheels forward
              
              }
            else if (output<0){ //Falling towards back
              Reverse(); //Rotate the wheels backward
              }
            }
        else //If Bot not falling
        Stop(); //Hold the wheels still
        }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)
    {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    }
    else if (mpuIntStatus & 0x02)
    {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer); //get value for q
        mpu.dmpGetGravity(&gravity, &q); //get value for gravity
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //get value for ypr

        input = ypr[1] * 180/M_PI + 180;

   }
}

void Forward() //Code to rotate the wheel forward 
{
    analogWrite(6,output);
    analogWrite(9,0);
    analogWrite(10,output);
    analogWrite(11,0);
    Serial.print("F"); //Debugging information 
}

void Reverse() //Code to rotate the wheel Backward  
{
    analogWrite(6,0);
    analogWrite(9,output*-1);
    analogWrite(10,0);
    analogWrite(11,output*-1); 
    Serial.print("R");
}

void Stop() //Code to stop both the wheels
{
    analogWrite(6,0);
    analogWrite(9,0);
    analogWrite(10,0);
    analogWrite(11,0); 
    Serial.print("S");
}

void Loop_sr04(){
   //delay(40);
   unsigned int distance = sonar.ping_cm();
   Serial.print(distance);
   Serial.println("cm");
}

I love copying other code (for some), but not all. I have tried to learn it but many makes me dizzy to understand.

Any help, please.

    // check for overflow (this should never happen unless our code is too inefficient)

There is the answer, I think. You made to code too inefficient by the changes you made.

Copying code can only get you so far. Quite soon you will reach the point where copying code without understanding it will not work. You have reached that point.

It is very very difficult to learn coding by copying. It would be like going to a foreign city and trying to learn the local language by listening to conversations on busy streets. This would make you dizzy! You need to take a class, where the teacher speaks your language and the foreign language and can introduce you to the basics, beginning at a slow pace. Once you have learned the basics, then you can learn more quickly by talking with the locals.

Okay, thanks then, but I need it to work as expected.

Do I have to make difference of both serial as in:

  sr04.begin (9600); // sonar SR04  
  mpu6050.begin(115200); // MPU6050

or, should I give the same signal 115200 for both of them?
Give me the CLUE (at least), please.

The question you asked makes no sense as you ask it. Neither SR04 or MPU6050 sensors possess a serial (UART) interface. MPU6050 has an i2c interface and SR04 has a simple 2-pin trigger/echo interface. You would only use the commands you asked about with sensors or other components which have a UART interface.

Right now, your project uses only one serial/UART interface. That is to connect with the serial monitor on your PC/laptop. One serial interface, so one serial.begin() in your code.

1 Like

Oh I see :grin: I will try it again and post it the result right here again. Thanks

@joe_kdw, your topic has been moved to a more suitable location on the forum. Installation and Troubleshooting is not for problems with (nor for advise on) your project :wink: See About the Installation & Troubleshooting category.

I can't help much you with the code.

One odd thing is the two Serial.begin() statements in setup(). You only need one and they have no relationship with the SR04 or the MPU6050.

And the second one is that if you call Loop_SR04() from loop(), the only thing is that it will print the distance.

I would start by timing how long Loop_sr04() takes. I think that the FIFO overflow is an indication that it takes too long. I however have no experience with NewPing nor with the MPU6050.

1 Like

Thanks, your expl. is necessary for me to run with the code. I am now writing the code and try to understand the flow of HC-SR04 when integrated in self-balancing robot. What I want is to let mpu work without being interfered with the sensor SR04, but the sensor works as I expect without interfering (even stopping its own program). That's the point.

Really, I love this tech as I write others for back-end and front-end web development. LOL. Thanks. Really appreciate.

sterretje & PaulRB - well, (I smile by myself. In fact, it works finally by removing the "serial" and I let Serial.begin(115200);LOL ...

By the way, thanks soooo much :*

Here's the result by omitting the "alien serial" LOL. thanks again. I just want to get it and time to use "IF" to "avoid obstacle". YIiihaaa :slight_smile:

I will use: second serial for BLUETOOTH code later hihihi ..

The self-balancing robot kit I bought uses millis() to pulse the trigger pin every 50 milliseconds and uses a Pin Change Interrupt to measure the echo pulse. If you don't need Pin 2 or Pin 3 for anything else you could move the echo pin to one of those and use a 'CHANGE' interrupt. Otherwise, install the PinChangeInterrupt library.

1 Like

Thanks @johnwasser, very nice information. I will do trial and error for it. It's really helpful. Thanks again.