Mpu6050+encoder

jaewoo1 · August 9, 2021, 4:33am

We are currently building a balancing device using a momentum wheel. To this end, the slope was obtained using gyro and acceleration sensors called mpu6050, and an algorithm was implemented to control the rotation speed of the motor based on this slope. Also, we used a motor with a hall sensor encoder to get the exact speed of rotation of the motor. Whenever the motor rotates and a signal is generated in the encoder, an interrupt was used to count, but suddenly the value of the tilt sensor changed irregularly as it operated. The Mpu6050 hypothesized that the problem would occur if an interrupt occurred while reading a value, and tried to use the detachInterrupts function to avoid encoders interrupts, but the problem was not resolved. Our goal is to control this system with one computer, and I would like to ask you if it is possible to solve this problem.

...
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

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

MPU6050 mpu;

#define OUTPUT_READABLE_YAWPITCHROLL

//#define INTERRUPT_PIN 2
#define LED_PIN 13

#define encoder0PinA 2
#define encoder0PinB 3

#define encoder1PinA 18
#define encoder1PinB 19

int encoder0Pos = 0;
int encoder1Pos = 0;

unsigned long count_0 = 0;
unsigned long count_1 = 0;

unsigned long start = 0;

unsigned long dt= 0;

int now_0 = 0;
int now_1 = 0;

bool blinkState = false;

bool dmpReady = false;
uint8_t mpuIntStatus;
uint8_t devStatus;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];

Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorInt16 aaWorld;
VectorFloat gravity;
float euler[3];
float ypr[3];

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

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

const int dir_0 = 4;
const int pwm_0 = 5;

const int dir_1 = 6;
const int pwm_1 = 7;

int spin_0 = 0;
int power_0 = 0;

int spin_1 = 0;
int power_1 = 0;

float pitch = 0;
float roll = 0;

void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000);
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif

pinMode(4,OUTPUT);
pinMode(5,OUTPUT);

pinMode(6,OUTPUT);
pinMode(7,OUTPUT);

pinMode(encoder0PinA, INPUT); 
pinMode(encoder0PinB, INPUT); 

pinMode(encoder1PinA, INPUT); 
pinMode(encoder1PinB, INPUT); 

attachInterrupt(digitalPinToInterrupt(encoder0PinA), [](){count_0++;}, CHANGE); 
attachInterrupt(digitalPinToInterrupt(encoder0PinB), [](){count_0++;}, CHANGE); 

attachInterrupt(digitalPinToInterrupt(encoder1PinA), [](){count_1++;}, CHANGE); 
attachInterrupt(digitalPinToInterrupt(encoder1PinB), [](){count_1++;}, CHANGE); 

Serial.begin(115200);
while (!Serial); 

Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
//pinMode(INTERRUPT_PIN, INPUT);

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

Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available());                 // wait for data
while (Serial.available() && Serial.read()); // empty buffer again

Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();

mpu.setXGyroOffset(110);
mpu.setYGyroOffset(37);
mpu.setZGyroOffset(7);
mpu.setXAccelOffset(286); 
mpu.setYAccelOffset(-1527); 
mpu.setZAccelOffset(664); 

if (devStatus == 0) {
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.PrintActiveOffsets();
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

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

    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    packetSize = mpu.dmpGetFIFOPacketSize();
} else {
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
}
pinMode(LED_PIN, OUTPUT);

}

void loop() {
detachInterrupt(digitalPinToInterrupt(encoder0PinA));
detachInterrupt(digitalPinToInterrupt(encoder0PinB));

detachInterrupt(digitalPinToInterrupt(encoder1PinA));
detachInterrupt(digitalPinToInterrupt(encoder1PinB));

if (!dmpReady) return;

//while (!mpuInterrupt && fifoCount < packetSize) {
//    if (mpuInterrupt && fifoCount < packetSize) {
//      fifoCount = mpu.getFIFOCount();
//    }  
//}

mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

fifoCount = mpu.getFIFOCount();


if(fifoCount < packetSize){
}
else if ((mpuIntStatus & (0x01 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));

} else if (mpuIntStatus & (0x01 << MPU6050_INTERRUPT_DMP_INT_BIT)) {
  

while(fifoCount >= packetSize){
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
}
    #ifdef OUTPUT_READABLE_YAWPITCHROLL
        
        count_0 = 0;
        count_1 = 0;
        
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        
        pitch = ypr[2] * 180/M_PI;
        roll = ypr[1] * 180/M_PI;
        
        attachInterrupt(digitalPinToInterrupt(encoder0PinA), [](){count_0++;}, CHANGE); 
        attachInterrupt(digitalPinToInterrupt(encoder0PinB), [](){count_0++;}, CHANGE); 

        attachInterrupt(digitalPinToInterrupt(encoder1PinA), [](){count_1++;}, CHANGE); 
        attachInterrupt(digitalPinToInterrupt(encoder1PinB), [](){count_1++;}, CHANGE); 

        start = millis();
       
        Serial.print(ypr[2] * 180/M_PI); 
        Serial.print("\t");
        Serial.print(ypr[1] * 180/M_PI);
        Serial.print("   ");

        delay(50);
        
        dt = millis();
        now_0 = count_0;
        now_1 = count_1;
        dt -=  start;
        
        int rpm_0 = now_0 * ((60000/68) / dt);
        int rpm_1 = now_1 * ((60000/68) / dt);
        
        Serial.print(spin_0);
        Serial.print("   ");
        Serial.print(power_0);
        Serial.print("   ");
        Serial.print(rpm_0);
        Serial.print("   ");

        Serial.print(spin_1);
        Serial.print("   ");
        Serial.print(power_1);
        Serial.print("   ");
        Serial.println(rpm_1);
        
        power_0 = map(pitch, -80, 80, -255, 255);
        power_1 = map(roll, -80, 80, -255, 255);

        if(pitch>=0 and pitch<=80){
          spin_0 = 0;
          
          digitalWrite(4, spin_0);
          analogWrite(5, power_0);
         }

         else if(pitch<0 and pitch>=-80){
          spin_0 = 1;
          
          digitalWrite(4, spin_0);
          analogWrite(5, -power_0);
         }

         else if(pitch>80){
          spin_0 = 0;
          
          digitalWrite(4, spin_0);
          analogWrite(5, 255);
         }

         else if(pitch<-80){
          spin_0 = 1;
          
          digitalWrite(4, spin_0);
          analogWrite(5, 255);
         }

         
         if(roll>=0 and roll<=80){
          spin_1 = 0;
          
          digitalWrite(6, spin_1);
          analogWrite(7, power_1);
         }

         else if(roll<0 and roll>=-80){
          spin_1 = 1;
          
          digitalWrite(6, spin_1);
          analogWrite(7, -power_1);
         }

         else if(roll>80){
          spin_1 = 0;
          
          digitalWrite(6, spin_1);
          analogWrite(7, 255);
         }

         else if(roll<-80){
          spin_1 = 1;
          
          digitalWrite(6, spin_1);
          analogWrite(7, 255);
         }

    #endif

}
}
...

KASSIMSAMJI · August 9, 2021, 5:04am

you can go around using encoder with no interrupt functions being involved

see this

well the guy used delay() for de-bouncing, but you can replace it with millis()

get rid of delay() functions from your code too, use millis()

Koepel · August 9, 2021, 12:34pm

Could you edit your top post and put the sketch between lines with three backslash-single-quotes ?

```
Your sketch
```

Interrupts used by:

millis() runs with a interrupt
Serial reads and writes data with interrupts
the Wire library uses interrupts
the encoder uses interrupts
the MPU-6050 signals that something is in the FIFO with an interrupt

That should work together.

How do you power your servo motors ?

jaewoo1 · August 9, 2021, 2:05pm

I'm not using servo motors, but DC motors.(with mdd10a motor driver and 12V) The whole system is like, the more the device tilts, the faster the motor rotates, and the encoder measures rpm of the motor. The problem is that, while operating well, the slope value suddenly changes even if the device is not tilted; it constantly gets wrong slope value.

system · December 7, 2021, 2:05pm

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.