MPU6050 with Encoder

[nilesh26bhalerao]

Hello I am combinig MPU6050 with Encoder for cycle simulation. But as I move the encoder very fastly it stopped automatically giving readings. I am little bit confused whats is the exact problem?

 

My Code

 

 

#define BAUDRATE 38400

//Encoder

//#define pinA A1

#define ARDUINOPIN A2

#define STATE_UNINIT 0

#define STATE_INIT 1

byte systemState;

volatile long count;

int pinA = 2;

int pinB = 3;

int limitSW_1 = 4;

boolean sigA = 0;

boolean sigB = 0;

boolean status_L1 = 0;

//Ends Encoder

 

//Start MPU

 

#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

#include "PinChangeInt.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include "Wire.h"

#endif

#define OUTPUT_READABLE_QUATERNION

//#define OUTPUT_READABLE_YAWPITCHROLL

//Ends MPU

 

MPU6050 mpu;

// MPU control/status vars

bool dmpReady = false;  

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

VectorInt16 aa;         // [x, y, z]            accel sensor measurements

VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements

VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements

VectorFloat gravity;    // [x, y, z]            gravity vector

float euler[3];         // [psi, theta, phi]    Euler angle container

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

 

// packet structure for InvenSense teapot demo

uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

 

 

 

// ================================================================

// ===               INTERRUPT DETECTION ROUTINE                ===

// ================================================================

 

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady() 

{

mpuInterrupt = true;

}

 

 

 

// ================================================================

// ===                      INITIAL SETUP                       ===

// ================================================================

 

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(BAUDRATE);

    //while (!Serial); // 

      mpu.initialize();

      devStatus = mpu.dmpInitialize();

       mpu.setXGyroOffset(52);

      mpu.setYGyroOffset(18);

      mpu.setZGyroOffset(-24);

      mpu.setZAccelOffset(1030); // 1688 factory default for my test chip

 

    if (devStatus == 0) 

{

mpu.setDMPEnabled(true);

      pinMode(ARDUINOPIN, INPUT_PULLUP);

// attachInterrupt(ARDUINOPIN, dmpDataReady, RISING);

      attachPinChangeInterrupt(ARDUINOPIN, dmpDataReady, RISING);

mpuIntStatus = mpu.getIntStatus();

dmpReady = true;

packetSize = mpu.dmpGetFIFOPacketSize();

} 

else { } 

     

    systemState = STATE_UNINIT;

   // pinMode(pinA, INPUT_PULLUP);

  //  attachPinChangeInterrupt(pinA, interruptA, CHANGE);

    // pinMode(pinB, INPUT_PULLUP);

   // attachPinChangeInterrupt(pinB, interruptB, CHANGE);

    attachInterrupt(0, interruptA, CHANGE);

    attachInterrupt(1, interruptB, CHANGE);

}

// ================================================================

// ===                    MAIN PROGRAM LOOP                     ===

// ================================================================

 

void loop() 

{

    if (!dmpReady) return;

    while (!mpuInterrupt && fifoCount < packetSize) {}

    mpuInterrupt = false;

    mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount();

    if ((mpuIntStatus & 0x10) || fifoCount == 1024) 

{

mpu.resetFIFO();

} 

    else if (mpuIntStatus & 0x02) 

         {

              while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

              mpu.getFIFOBytes(fifoBuffer, packetSize);

              fifoCount -= packetSize;

              //YPR

                #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(",");

                 // Serial.print(ypr[1] * 180/M_PI);

                 // Serial.print(",");

                 // Serial.println(ypr[2] * 180/M_PI);

                #endif    

              //Quaternion

   #ifdef OUTPUT_READABLE_QUATERNION

// display quaternion values in easy matrix form: w x y z

mpu.dmpGetQuaternion(&q, fifoBuffer);

  // Serial.print("quat\t");

Serial.print(q.w);

Serial.print(",");

Serial.print(q.x);

   Serial.print(",");

Serial.print(q.y);

Serial.print(",");

Serial.print(q.z);

Serial.print(",");

#endif

 Serial.print(count);   

            Serial.print(',');

            Serial.print(status_L1);

            Serial.println();

           // Stream.flush();

          } 

 status_L1 = digitalRead(limitSW_1);

            if (status_L1 == 1 && false)

                  {

                    count = 0;

                    systemState = STATE_INIT;    

                  }

          

           // delay(50);

}

 

//interupt for PinA

void interruptA()

{

 sigA = digitalRead(pinA);

 switch(sigA)

 {

case 0:

 if(sigB == 1)

 count++;

 else

 count--;

 break;

case 1:

 if(sigB == 0)

 count++;

 else

 count--;

 break;

 }

}

 

//interupt for PinA

 

   void interruptB()

{

 sigB = digitalRead(pinB);

 switch(sigB)

 {

case 0:

 if(sigA == 0)

 count++;

 else

 count--;

 break;

case 1:

 if(sigA == 1)

 count++;

 else

 count--;

 break;

 }

}