first of all, I am not very good at English. if I have done a mistake during describe the problem apologize me. I want to combine both ultrasonic sensor code and mpu6050 sensor. can anyone tell me how to combine both codes in one code?

#include <Wire.h>
//I2C communication library for MPU6050
#include <I2Cdev.h>
//MPU6050 interfacing library
#include <MPU6050_6Axis_MotionApps20.h>
//Processing incoming serial data 
#include <CmdMessenger.h>
//Contain definition of maximum limits of various data type
#include <limits.h>

//Creating MPU6050 Object
MPU6050 mpu(0x68);
//Messenger object
CmdMessenger Messenger_Handler = CmdMessenger(Serial);

///////////////////////////////////////////////////////////////////////////////////////
//DMP options
//Set true if DMP init was successful
bool dmpReady = false;
//Holds actual interrupt status byte from MPU
uint8_t mpuIntStatus;
//return status after each device operation
uint8_t devStatus;
//Expected DMP paclet size
uint16_t packetSize;
//count of all bytes currently in FIFO
uint16_t fifoCount;
//FIFO storate buffer
uint8_t fifoBuffer[64];


#define OUTPUT_READABLE_QUATERNION
////////////////////////////////////////////////////////////////////////////////////////////////

//orientation/motion vars
Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorInt16 aaWorld;
VectorFloat gravity;

//Euler angle container
float euler[3];

//yaw/pitch/roll container and gravity vector
float ypr[3];




// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

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

/////////////////////////////////////////////////////////////////
//Motor Pin definition
//Left Motor pins

#define A_1 3
#define B_1 4

//PWM 1 pin number
#define PWM_1 6


//Right Motor
#define A_2 4 
#define B_2 5

//PWM 2 pin number
#define PWM_2 5

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Ultrasonic pins definition
const int echo = 24, Trig = 23;
long duration, cm;

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////
//Motor speed from PC
//Motor left and right speed
float motor_left_speed = 0;
float motor_right_speed = 0;
/////////////////////////////////////////////////////////////////

//Setup serial, encoders, ultrasonic, MPU6050 and Reset functions
void setup()
{

  //Init Serial port with 115200 baud rate
  Serial.begin(115200);  

  //Setup Motors
  SetupMotors();
  //Setup Ultrasonic
  SetupUltrasonic();  
  //Setup MPU 6050
  Setup_MPU6050();

  //Set up Messenger 
//  Messenger_Handler.attach(OnMssageCompleted);

}

void SetupMotors()
{

 //Left motor
 pinMode(A_1,OUTPUT);
 pinMode(B_1,OUTPUT); 


 //Right Motor
 pinMode(A_2,OUTPUT);
 pinMode(B_2,OUTPUT);  

}


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Setup UltrasonicsSensor() function
void SetupUltrasonic()
{
 pinMode(Trig, OUTPUT);
 pinMode(echo, INPUT); 

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Setup MPU6050 function

void Setup_MPU6050()
{


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

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

    //Initialize DMP in MPU 6050
    Setup_MPU6050_DMP();


}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Setup MPU 6050 DMP
void Setup_MPU6050_DMP()
{

     //DMP Initialization

   devStatus = mpu.dmpInitialize();

   mpu.setXGyroOffset(220);
   mpu.setXGyroOffset(76);
   mpu.setXGyroOffset(-85); 
   mpu.setXGyroOffset(1788);  


   if(devStatus == 0){
         Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);
    //mpu.setDMPEnabled(true);

//    pinMode(PUSH2,INPUT_PULLUP);    
//    attachInterrupt(PUSH2, dmpDataReady, RISING);

    mpuIntStatus = mpu.getIntStatus();

    dmpReady = true;

    packetSize = mpu.dmpGetFIFOPacketSize();

   }else{

     ;
     }   
}


void loop() {

 //Read from Serial port
    Read_From_Serial();

    //Send ultrasonic values through serial port
    Update_Ultra_Sonic();

    //Send MPU 6050 values through serial port
    Update_MPU6050();


}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Read from Serial Function

void Read_From_Serial()

{
   while(Serial.available() > 0)
    {

       int data = Serial.read();
//       Messenger_Handler.process(data);


    } 

}

void Update_Ultra_Sonic()
{
  digitalWrite(Trig, LOW);
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);
  // The echo pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  duration = pulseIn(echo, HIGH);


  //Sending through serial port
  Serial.print("u");
  Serial.print("\t");
  Serial.print(cm);
  Serial.print("\n");

}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Update MPU6050

void Update_MPU6050()
{



  int16_t ax, ay, az;
  int16_t gx, gy, gz;

    ///Update values from DMP for getting rotation vector
    Update_MPU6050_DMP();


}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Update MPU6050 DMP functions

void Update_MPU6050_DMP()
{

 //DMP Processing
 // 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) {

        ;    

          }


// 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 > 512) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
    }



// 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;

        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);


            Serial.print("i");Serial.print("\t");
            Serial.print(q.x); Serial.print("\t");
            Serial.print(q.y); Serial.print("\t");
            Serial.print(q.z); Serial.print("\t");
            Serial.print(q.w);
            Serial.print("\n");


        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            Serial.print("aworld\t");
            Serial.print(aaWorld.x);
            Serial.print("\t");
            Serial.print(aaWorld.y);
            Serial.print("\t");
            Serial.println(aaWorld.z);
        #endif

        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif


    }


}
  • What exactly is the problem with your combined code? And please format the code correctly by selecting it and click the {} button in the editors toolbar or press Ctrl+k – chrisl Dec 2 at 9:50
  • I want to read mpu6050 and ultrasonic sensor both. but it this is the error Initializing I2C devices... Testing device connections... MPU6050 connection successful Enabling DMP... u 0 – hasith_12 Dec 2 at 9:58
  • Have you tried to get the ultrasonic Sensor to work alone to be sure, that it works correctly? – chrisl Dec 2 at 10:06
  • yes. it worked. when both sensor test individually it work. – hasith_12 Dec 2 at 10:10
  • thank you very much for help – hasith_12 Dec 2 at 10:22
up vote 2 down vote accepted

In the function Update_Ultra_Sonic() you do the measurement and save the result in the variable duration. Then you print the variable cm, but you haven't calculated the value for this variable yet. You have to calculate the number of cm from the echos duration and save the result in cm. When you don't do this, the variable stays at zero.

  • thank you very much for help – hasith_12 Dec 2 at 10:24

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Not the answer you're looking for? Browse other questions tagged or ask your own question.