Out of two logically similar sketches, one works while the other does not

Question

I am currently making a project involving a self balancing bot & found this code online:

#include "PID_v1.h"
#include "LMotorController.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"

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

#define MIN_ABS_SPEED 20

MPU6050 mpu;

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

//PID
double originalSetpoint = 178.57;
double setpoint = 178.57;
double movingAngleOffset = 0.1;
double input, output;
double Kp = 50;
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.5;
//MOTOR CONTROLLER
int ENA = 5;
int IN1 = 6;
int IN2 = 7;
int IN3 = 8;
int IN4 = 9;
int ENB = 10;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

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

    // initialize serial communication
    // (115200 chosen because it is required for Teapot Demo output, but it's
    // really up to you depending on your project)
    Serial.begin(115200);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    // 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
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(262);
    mpu.setYGyroOffset(-29);
    mpu.setZGyroOffset(-10);
    mpu.setXAccelOffset(-2109);
    mpu.setYAccelOffset(394);
    mpu.setZAccelOffset(1341); // 1688 factory default for my test chip

    // 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(")"));
    }
}


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();
        motorController.move(output, MIN_ABS_SPEED);

    }

    // 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);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        #ifdef LOG_INPUT
            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
        input = ypr[1] * 180/M_PI + 180;
        Serial.println(input);
   }
}

The repo is linked to https://github.com/kurimawxx00/arduino-self-balancing-robot

My code is as below:

    /* For documentation purposes
 *
 * #define MPU6050_INTERRUPT_FF_BIT            7
 * #define MPU6050_INTERRUPT_MOT_BIT           6
 * #define MPU6050_INTERRUPT_ZMOT_BIT          5
 * #define MPU6050_INTERRUPT_FIFO_OFLOW_BIT    4
 * #define MPU6050_INTERRUPT_I2C_MST_INT_BIT   3
 * #define MPU6050_INTERRUPT_PLL_RDY_INT_BIT   2
 * #define MPU6050_INTERRUPT_DMP_INT_BIT       1
 * #define MPU6050_INTERRUPT_DATA_RDY_BIT      0
 *
  */

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "PID_v1.h"
#include "LMotorController.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include <Wire.h>
#endif

#define MIN_SPEED 20

#define DEBUG_BUILD

MPU6050 mpu;

// Variables for MPU status and control
bool isDMPReady = false;
uint8_t dmpStatus; // 0 if initialization succeeds
uint16_t dmpPacketSize; // default = 42
uint16_t fifoCount;
uint8_t fifoBuffer[64];
uint8_t mpuIntStatus;

// Variables for calculated data from DMP
Quaternion q; // // holds QUAT_W, QUAT_X, QUAT_Y, QUAT_Z from FIFO
int16_t gyro[3]; // Holds rotation
VectorFloat gravity; // holds gravity vector calculated from DMP
float ypr[3]; // 0 = yaw, 1 = pitch, 2 = roll

// PID data
double setPoint = 178.57;
double input = setPoint, output;
double Kp = 50;
double Kd = 1.4;
double Ki = 60;
PID pid(&input, &output, &setPoint, Kp, Ki, Kd, DIRECT);

// Motor config
const int inA = 6;
const int inB = 7;
const int inC = 8;
const int inD = 9;
const int enA = 5;
const int enB = 10;
const double motorCorrectionA = 0.6;
const double motorCorrectionB = 0.5;
LMotorController motorController(inA, inB, inC, inD, enA, enB, motorCorrectionA, motorCorrectionB);

// =================================================
// ===         Interrupt Service Routine         ===
// =================================================
volatile bool mpuInterrupt = false;
void dmpDataReady() {
    mpuInterrupt = true;
}

// =================================================
// ===              Program Setup                ===
// =================================================
void setup() {
    // join the I2C bus
    #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(115200);
    while(!Serial);

    // Initialize mpu
    Serial.println(F("Initializing I2C Devices: "));
    mpu.initialize();
    dmpStatus = mpu.dmpInitialize();

    // Gyro and accel offsets
    mpu.setXGyroOffset(262);
    mpu.setYGyroOffset(-29);
    mpu.setZGyroOffset(-10);
    mpu.setXAccelOffset(-2109);
    mpu.setYAccelOffset(394);
    mpu.setZAccelOffset(1341);

    // Verify initialization
    #ifdef DEBUG_BUILD
        Serial.println(F("Testing MPU Status: "));
        Serial.println(mpu.testConnection() ? F("MPU6050 initialization success!") : F("MPU6050 initialization failed!"));
        Serial.println(dmpStatus == 0 ? F("DMP initialization successful!") : F("DMP initialization failed!"));
    #endif

    if(dmpStatus == 0){
        // Enable DMP
        mpu.setDMPEnabled(true);

        attachInterrupt(0, dmpDataReady, RISING);

        // Get interrupt byte
        mpuIntStatus = mpu.getIntStatus();

        // Get DMP packet size ; default = 42B
        dmpPacketSize = mpu.dmpGetFIFOPacketSize();

        // Verify DMP initialization
        #ifdef DEBUG_BUILD
            Serial.println(mpu.getFIFOEnabled() ? F("FIFO Enabled!") : F("FIFO Not Enabled!"));
            Serial.println(mpu.getXGyroFIFOEnabled() ? F("FIFO X Gyro Enabled!") : F("FIFO X Gyro Not Enabled!"));
            Serial.println(mpu.getYGyroFIFOEnabled() ? F("FIFO Y Gyro Enabled!") : F("FIFO Y Gyro Not Enabled!"));
            Serial.println(mpu.getZGyroFIFOEnabled() ? F("FIFO Z Gyro Enabled!") : F("FIFO Z Gyro Not Enabled!"));
            Serial.println(mpu.getAccelFIFOEnabled() ? F("FIFO Accel Enabled!") : F("FIFO Accel Not Enabled!"));
            Serial.print(F("DMP Packet Size: "));
            Serial.println(dmpPacketSize);
        #endif

        isDMPReady = true;

        // Setup PID
        pid.SetMode(AUTOMATIC);
        pid.SetSampleTime(10);
        pid.SetOutputLimits(-160, 160);
    }
    else{
        Serial.println(F("DMP Error!!!"));
    }
}

// =================================================
// ===            Main Program Loop              ===
// =================================================
void loop() {
    // Do nothing if DMP initialization failed
    if(!isDMPReady) return;

    while(!mpuInterrupt && fifoCount < dmpPacketSize){
        pid.Compute();
        motorController.move(output, MIN_SPEED);
    }

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

    fifoCount = mpu.getFIFOCount();

    if(mpuIntStatus & 0x10 || fifoCount == 1024){
        mpu.resetFIFO();
        Serial.println("FIFO Overflow");
    }
    else if(mpuIntStatus & 0x02){
        while(fifoCount < dmpPacketSize) fifoCount = mpu.getFIFOCount();

        mpu.getFIFOBytes(fifoBuffer, dmpPacketSize);
        fifoCount -= dmpPacketSize;

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        // TODO: implement complementary filter
        input = ypr[1] * 180 / M_PI + 180; // Supposing at upright position, pitch is 180 degrees

        #ifdef DEBUG_BUILD
            Serial.print("Yaw/Pitch/Roll: ");
            Serial.print(ypr[0] * RAD_TO_DEG);
            Serial.print("\t");
            Serial.print(ypr[1] * RAD_TO_DEG);
            Serial.print("\t");
            Serial.println(ypr[2] * RAD_TO_DEG);
        #endif
    }
}

I am using the exact same libraries and classes as in the repo. Yet my code does not work at all, the motors move erratically. All the physical connections are ok since the code from the repo works flawlessly. I've no idea why this is happening. Help is very much appreciated.

Answer 1

Given that both libraries are the same.

The two sketches seem to set up the motorcontroller quite differently. Changing an "input pin" and "enable pin" around, would lead to errratic motor behaviour.

``` LMotorController motorController(ENA, IN1

LMotorController motorController(inA, inB ```

To me, an input pin would be an PWM signal to drive the speed of the motor. An enable pin, would enable/disable the whole driver (both left/right).

But it's hard to tell, also, if the above sketch is working, why not use the above sketch and adjust that?

Answer 2

Another thing I noticed while tinkering, interrupts from the MPU6050 can only be caught by the Uno when serial comm is set to 115200 baud rate or higher. Anything lower causes the MPU interrupts to be never caught. Maybe this is a particular problem of my board (it's a cheap chinese one), but maybe this will help someone out somehow. :)