MPU6050 (Self balancing Robot)

Hey
I am making a self balancing robot. I connected everything properly but when upload the program and open serial monitor. This comes

Initializing I2C devices...

Testing device connections...

MPU6050 connection failed

Send any character to begin DMP programming and demo:

Initializing DMP...

DMP Initialization failed (code 1)

I check my mpu6050 with "Who I am " Program and it is coming as 0×3F.

Does this mean my mpu is fake. Anybody who can help me out on this one

I also connected ena and enb to Arduino .



#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

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

/*********Tune these 4 values for your BOT*********/
double setpoint= 176; //set the value when the bot is perpendicular to ground using serial monitor. 
//Read the project documentation on circuitdigest.com to learn how to set these values
double Kp = 21; //Set this first
double Kd = 0.8; //Set this secound
double Ki = 140; //Finally set this 
/******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() {
  Serial.begin(115200);



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

The problem may be in the sketch that you didn't post or in the schematic that you didn't post

Please follow the advice given in the link below when posting code, in particular the section entitled 'Posting code and common code problems'

Use code tags (the </> icon above the compose window) to make it easier to read and copy for examination

1 Like

I attached the image and code please give a check

At which voltage does the MPU6050 work and which voltage your Arduino? I2C connection requires modules use the same operating voltage.

Mpu6050 is getting powered by Arduino and Arduino by cable

This does not answer my question. My MPU6050 module has a 3.3V voltage regulator on board.

1 Like

Please edit your post to add code tags ("</>" post editor button).

1 Like

Mpu6050 is getting 5 volts by Arduino 5volt pin

Done

Very likely, since the original manufacturer stopping making them several years ago.

But for reliable communications, and to avoid destroying the sensor, you must use a 5V to 3.3V level adapter for the SCL and SDA lines, with a 5V Arduino.

@jremington The MPU6050 breakout board requires 5V and has a voltage regulator to 3.3V. Note that the pullups are on the 3.3V side. The Arduino is only pulling to the ground so there are no issues.

@arvinderkatoch The fake MPU6050's work fine for a balancing bot.
My Balancing Bot: https://youtu.be/JI7Pp7kWgmg
I use an MPU6050 I got from Aliexpress (China) and a custom H Bridge UNO Arduino at heart I designed.

libraries are here: https://github.com/ZHomeSlice/Simple_MPU6050
and Here: GitHub - jrowberg/i2cdevlib: I2C device library collection for AVR/Arduino or other C++-based MCUs

Please let me know if you struggle with this example. it is similar to yours.
My Code for your viewing experience:

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#include "PID_v3.h"
//#include <RC_V1r2.h>
#include <EEPROM.h>
#include "EEPROMAnything.h"
MPU6050 mpu;
//RC RCRadio;
#include "Interrupts.h"
InterruptsClass Interrupt;
int R9;
int R10;
volatile int Radio12;
volatile int Radio10;
volatile int8_t   RCIsAlive = 0;
volatile bool     FailSafe = true;
volatile uint32_t AliveBits = 0;                // a reading Since Last Test,
volatile long rotarySteps = 0;
volatile int sw = 0;

// PID Tuning Settings
double consKp = 6, consKi = 5, consKd = .02; // PID values for Balancinb these should be good!
double YconsKp = 1, YconsKi = 0, YconsKd = .05; // PID values for turning these should be good!
int MinPower = 50;
// supply your own gyro offsets here, scaled for min sensitivity use MPU6050_calibration.ino
// -4232	-706	1729	173	-94	37
//                       XA      YA      ZA      XG      YG      ZG
//int MPUOffsets[6] = {  -4232,  -706,   1729,    173,    -94,     37};

// 2471  -563  1628  8 -23 53
int MPUOffsets[6] = {  2471,  -563,  1628,  8, -23, 53};

#define PWM_A     (6)   // 0-255             Arduino Digital pin 6
#define PWM_B     (5)   // 0-255             Arduino Digital pin 3
#define HBgA1HighEn (8) // Enable/Disable    Arduino Digital pin 8
#define HBgA2HighEn (7) // Enable/Disable    Arduino Digital pin 7
#define HBgB1HighEn (4) // Enable/Disable    Arduino Digital pin 4
#define HBgB2HighEn (3) // Enable/Disable    Arduino Digital pin 5
#define Int0_D2     (2)// intreupt 0 or Digital in/out  Pin 2 Breakout on Board D2

#define Disable 0x0   // also defined as LOW
#define Enable  0x1   // also defined as HIGH
#define Forward (1)   // Value for the Forward action
#define Backward (2)  // Value for the Backward Action
#define AllStop (0)   // Value for the all stop action

#define LED_PIN 13 // 



// ================================================================
// ===                      MPU DMP SETUP                       ===
// ================================================================
int FifoAlive = 0; // tests if the interrupt is triggering
int IsAlive = -20;     // counts interrupt start at -20 to get 20+ good values before assuming connected
// MPU control/status vars
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
byte StartUP = 100; // lets get 100 readings from the MPU before we start trusting them (Bot is not trying to balance at this point it is just starting up.)
static void nothingFunction(void) {}
typedef void (*voidFuncPtr)(void);// Create a type to point to a funciton.
voidFuncPtr GetDMP_CB = nothingFunction; // Set the Call Back functin to nothing
voidFuncPtr GetDMPFail_CB = nothingFunction;
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high


void MPU6050Connect() {
  static int MPUInitCntr = 0;
  // initialize device
  mpu.initialize(); // same
  // load and configure the DMP
  devStatus = mpu.dmpInitialize();// same

  if (devStatus != 0) {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)

    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};

    MPUInitCntr++;

    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[devStatus]);
    Serial.println(F(")"));

    if (MPUInitCntr >= 10) return; //only try 10 times
    delay(1000);
    MPU6050Connect(); // Lets try again
    return;
  }


  mpu.setXAccelOffset(MPUOffsets[0]);
  mpu.setYAccelOffset(MPUOffsets[1]);
  mpu.setZAccelOffset(MPUOffsets[2]);
  mpu.setXGyroOffset(MPUOffsets[3]);
  mpu.setYGyroOffset(MPUOffsets[4]);
  mpu.setZGyroOffset(MPUOffsets[5]);

  Serial.println(F("Enabling DMP..."));
  mpu.setDMPEnabled(true);
  // enable Arduino interrupt detection
  Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
  Serial.print("mpu.getInterruptDrive=  "); Serial.println(mpu.getInterruptDrive());

  //mpuIntStatus = mpu.getIntStatus(); // Same
  packetSize = mpu.dmpGetFIFOPacketSize(); // get expected DMP packet size for later comparison
  delay(1000); // Let it Stabalize
  mpu.resetFIFO(); // Clear fifo buffer
  mpu.getIntStatus(); // Clears the Status Buffer
  mpuInterrupt = false; // wait for next interrupt
  /* attachInterrupt(0, []() {
     mpuInterrupt = true;
    }, RISING); //pin 2 on the Uno on interrupt set mpuInterrupt to ture (using Lambda anonymouns funciotn)
  */
}

void MPUCheck() {
  if (mpuInterrupt) 
    if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) 
      GetDMP_CB(); // Callback Function <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success
  mpuInterruptTest();
}

void onGetMPUDMPSuccess(void (*CB)(void)) {
  GetDMP_CB = CB;
}


byte mpuInterruptTest() {
  static unsigned long FailTimer;
  if ((unsigned long)(millis() - FailTimer) >= 15) { //15 MS timer
    FailTimer = millis();
    IsAlive = (FifoAlive > 0) ?   IsAlive + 1 : IsAlive - 1  ;
    IsAlive = constrain(IsAlive, -15, +15); // Must have 15 good or bad test for IsAlive to change states Test every 15ms
    FifoAlive = 0; // Set FifoAlive to zero waiting for the next DMP retreaval
  }
  if (IsAlive <= 0 ) {
    GetDMPFail_CB(); // Callback Function <<<<<<<<<<<<<<<<<<<<<<<<<<<< On Failure
  }
  return (IsAlive > 0);
}

void onGetMPUDMPFail(void (*CB)(void)) {
  GetDMPFail_CB = CB;
}
// ================================================================
// ===                      i2c SETUP Items                     ===
// ================================================================
void i2cSetup() {
  // 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
}



// ================================================================
// ===                         PID Setup                        ===
// ================================================================
double InputABS;
bool Mode = false;
double SPZero = 0;// Setpoint trim adjustment in Degrees
double SPAdjust = 0, Setpoint = 0, Input, Output;

// Yaw PID Control
bool TurnControlEnable = true; // To Enable Turn control = true
double DegSetpoint = 0;        // Degree Setpoint should always be Zero!
double YawInput;               //(YawInput = YawMPU + YawOffset) with rollover at +-180°
double Turn;                   // Outout to drive motors
double YawOffset;              // YawOffset adds to YawImput to get an alternate reading for 0° (YawInput = YawMPU + YawOffset) with rollover at +-180°
double YawMPU;                 // Actual reading from the MPU
PID myTurnPID(&YawInput, &Turn, &DegSetpoint);
PID myPID(&Input, &Output, &Setpoint); //, consKp, consKi, consKd, REVERSE

void PIDSetup() {
  myPID.SetOutputLimits(-255, 255)
  .SetSampleTime(10)
  .SetControllerDirection(REVERSE)
  .SetDriveMin(MinPower)
  .SetDriveMax(255)
  .SetTunings(consKp, consKi, consKd); //Load the tunings

  //  myPID.onManual([]() {
  // Tank Drive Controls

  //  })
  myPID.onCostA([]() {
    Drive(PWM_A, HBgA1HighEn, HBgA2HighEn, 0, LOW, LOW);
  });
  myPID.onCostB([]() {
    Drive(PWM_B, HBgB1HighEn, HBgB2HighEn, 0, LOW, LOW);
  });
  myPID.onForwardA([ = ](double Speed) {
    Drive(PWM_A, HBgA1HighEn, HBgA2HighEn, Speed, HIGH, LOW);
  });
  myPID.onReverseA([ = ](double Speed) {
    Drive(PWM_A, HBgA1HighEn, HBgA2HighEn, Speed, LOW, HIGH );
  });
  myPID.onForwardB([ = ](double Speed) {
    Drive(PWM_B, HBgB1HighEn, HBgB2HighEn, Speed, HIGH, LOW);
  });
  myPID.onReverseB([ = ](double Speed) {
    Drive(PWM_B, HBgB1HighEn, HBgB2HighEn, Speed, LOW, HIGH);
  });

  myTurnPID.SetOutputLimits(-1000, 1000)
  .SetSampleTime(10)
  .SetControllerDirection(REVERSE)
  .SetTunings(YconsKp, YconsKi, YconsKd); //Load the tunings
}

// ================================================================
// ===                     Motor SETUP Items                    ===
// ================================================================
void MotorSetup() {
  pinMode(PWM_A, OUTPUT); // A PWM
  pinMode(HBgA1HighEn, OUTPUT); // A1 Enable/Disable
  pinMode(HBgA2HighEn, OUTPUT); // A2 Enable/Disable
  pinMode(PWM_B, OUTPUT); // B PWM
  pinMode(HBgB1HighEn, OUTPUT); // B1 Enable/Disable
  pinMode(HBgB2HighEn, OUTPUT); // B2 Enable/Disable
  STOP();
  Output = 0;
  FullBreaking (); // freezes the motors by setting both sides of the H-Bridge to LOW Side on 100%
}
void FullBreaking () {
  Drive(PWM_B, HBgA1HighEn, HBgA2HighEn, 255, LOW, LOW);
  Drive(PWM_B, HBgB1HighEn, HBgB2HighEn, 255, LOW, LOW);
}

void Drive(int PPin, int xPin1, int xPin2, double Speed, int High1En, int High2En) {
  digitalWrite(xPin1, High1En);
  digitalWrite(xPin2, High2En);
  analogWrite(PPin, (int) Speed);
}

// ================================================================
// ===                    RC Radio SETUP Items                  ===
// ================================================================
// static void nothing(void){}                 // Z added Create an empty function
typedef void (*voidFuncPtr)(void);                 // Z added Create a type to point to a funciton.
volatile voidFuncPtr onRCIsAliveFuncPtr = nothing;    // Z Callback for onRepeat
volatile voidFuncPtr onRCIsNotAliveFuncPtr = nothing;    // Z Callback for onRepeat


void onRCIsAlive(void (*CallBack)(void)) {
  onRCIsAliveFuncPtr = CallBack;

}
void onRCIsNotAlive(void (*CallBack)(void)) {
  onRCIsNotAliveFuncPtr = CallBack;

}
uint8_t RCRadioAlive() {
  if (RCIsAlive > 0) {
    if (onRCIsAliveFuncPtr) {
      onRCIsAliveFuncPtr(); // call the function we assigned to the once empty function pointer
    }
  } else {
    if (onRCIsNotAliveFuncPtr) {
      onRCIsNotAliveFuncPtr(); // call the function we assigned to the once empty function pointer
    }
  }
  return (RCIsAlive > 0);
}
void RCRadioSetup() {
  onRCIsAlive([]() {
    R10 = Radio10;
    SPAdjust = (double) map(R10, -500, 500, -10, 10);
    runAfter(10) {
      R9 = Radio12;
      if ( abs(YawInput) < 90) { // let the bot catch up
        YawOffset -=  (double) - R9 * .013 ;
        if (YawOffset > 180) YawOffset -= 360; // rollover to negative
        if (YawOffset < -180) YawOffset += 360; // rollover to positive
      }
    }
  });
  onRCIsNotAlive([]() {
    SPAdjust = 0;
  });
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);
  pinMode(A1, INPUT_PULLUP);
  Interrupt.onInterrupt([ = ](uint32_t Time, uint32_t PinsChanged, uint32_t Pins) {
    sei(); // re enable other interrupts at this point,
    Interrupt.PinCallBack(Time, PinsChanged, Pins);
  });
  Interrupt.onPin(2, INPUT, [ = ](uint32_t Time, uint32_t PinsChanged, uint32_t Pins) {
    if (Interrupt.CheckPin(2)) mpuInterrupt = true;
  }).onPin(12, INPUT, [ = ](uint32_t Time, uint32_t PinsChanged, uint32_t Pins) {
    uint16_t RCTime =  Interrupt.RCRemote(12, Time, Pins, true);
    if (RCTime) {
      Radio12 = RCTime - 1500;
      AliveBits = AliveBits | 1 << 12;
    }
  }).onPin(10, INPUT, [ = ](uint32_t Time, uint32_t PinsChanged, uint32_t Pins) {
    uint16_t RCTime =  Interrupt.RCRemote(10, Time, Pins, true);
    if (RCTime) {
      Radio10 = RCTime - 1500;
      AliveBits = AliveBits | 1 << 10;
    }
  }).onPin(11, INPUT_PULLUP, [ = ](uint32_t Time, uint32_t PinsChanged, uint32_t Pins) {
    if (Interrupt.CheckPin(11)) sw++;
  }).onPin(9, INPUT_PULLUP, [ = ](uint32_t Time, uint32_t PinsChanged, uint32_t Pins) {
    rotarySteps = rotarySteps - Interrupt.Encoder(9, A1, Pins, false);
  });
  Interrupt.onTimer0([]() { // Timer to check for signal loss
    sei();
    static int MSec = 0;
    if (MSec++ < 20)return; // Check every 10 Miliseconds for change
    MSec = 0;
    RCIsAlive = (AliveBits > 0) ?   RCIsAlive + 1 : RCIsAlive - 1  ;
    RCIsAlive = constrain(RCIsAlive, -20, +20); // Must have 20 tests good or 20 tests bad for RCIsAlive to change states
    if ((RCIsAlive <= 0) && FailSafe) {
      Radio12 = 0;
      Radio10 = 0;
    }
    AliveBits = 0;
  }).Timer0Enable();
}

// ================================================================
// ===                      EEPROM SETUP                        ===
// ================================================================
#if !defined( Mega ) // Not Mega Remove it because you don't need this code
struct config_t {
  double Setpoint;
  double consKp;
  double consKi;
  double consKd;
  int MinPower;
  byte Saved;
} configuration;

void EEPROMLoad() {
  EEPROM_readAnything(0, configuration);
  if (configuration.Saved != 111)return;
  SPZero = configuration.Setpoint;
  consKp = configuration.consKp;
  consKi = configuration.consKi;
  consKd = configuration.consKd;
  MinPower = configuration.MinPower;
  // Setpoint = 0.00;
}

void EEPROMSave() {
  configuration.Setpoint = SPZero;
  configuration.consKp = consKp;
  configuration.consKi = consKi;
  configuration.consKd = consKd;
  configuration.MinPower = MinPower;
  configuration.Saved = 111;
  EEPROM_writeAnything(0, configuration);

}
#endif

// ================================================================
// ===                      RC Check Timer SETUP                       ===
// ================================================================






// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================
void setup() {
  //  EEPROMLoad();
  // pinMode(LED_PIN, OUTPUT);
  Serial.begin(115200); //115200
  Serial.println(F("Alive"));
  PIDSetup();
  MotorSetup();
  i2cSetup();
  MPU6050Connect();
  onGetMPUDMPSuccess(MPUMath); // Call Back void MPUMath(void) function When Data is retrieved from the MPU6050
  onGetMPUDMPFail(STOP);// Call Back void STOP(void) function When the MPU6050 Fails to Communicate
  RCRadioSetup();

  STOP();
}





// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
  MPUCheck();
  if (InputABS < 1) { // 1°
    Mode = true;
    myPID.SetMode(AUTOMATIC);
    myTurnPID.SetMode(AUTOMATIC);
  }
  Settings();
}

void STOP() {
  Mode = false;
  myPID.SetMode(MANUAL);
  myTurnPID.SetMode(MANUAL);
  Output = 0;
  Turn = 0;
  FullBreaking ();
  return;
}

void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Input = ((ypr[1] * 180 / M_PI) - SPZero);
  InputABS = abs(Input);
  if (InputABS > 55) { // 45.0 degrees
    STOP();
    StartUP = 10;
  } else if (StartUP) StartUP -= 1;
  if (!StartUP) {
    RCRadioAlive();
    Setpoint = SPAdjust; // SPAdjust is for offsetting the balance for driving the robot
    myPID.Compute();
    myPID.BalanceBotDrive(BotTurnControl());
  }
}
float BotTurnControl() {
  if (TurnControlEnable) {
    YawMPU = (ypr[0] * 180 / M_PI) ;
    if (StartUP) YawOffset = - YawMPU;
    YawInput = YawMPU + YawOffset;
    //    course_correction = (desired_heading-current_heading+180+720) % 360 - 180;
    if (YawInput > 180) YawInput -= 360; // rollover to negative
    if (YawInput < -180) YawInput += 360; // rollover to positive
    //  YawInput = YawInput;
    if (!StartUP) myTurnPID.Compute();
  } else Turn = 0;
  return Turn;
}





void Settings() {
  static char s[4] = {'/', '-', '\\', '|'};
  if (!Mode) {
    static int sx = 0;
    char c[7] = {' ', ' ', ' ', ' ', ' ', ' ', ' '};

    int R = rotarySteps;
    switch (sw) {
      case 0:
        if (rotarySteps == 0) break;
        Serial.println("Saveing");
        EEPROMSave();
        Serial.println("Saved");
        delay(1000);
        break;
      case 1:
        consKp = max(0, consKp + (double) (rotarySteps * .1));
        rotarySteps = 0;
        break;
      case 2:
        consKi = max(0, consKi + (double) (rotarySteps ));
        rotarySteps = 0;
        break;
      case 3:
        consKd = max(0, consKd + (double) (rotarySteps * .02));
        rotarySteps = 0;
        break;
      case 4:
        MinPower = MinPower + (int) (rotarySteps);
        rotarySteps = 0;
        break;
      case 5:
        SPZero = SPZero + (double) (rotarySteps * .1);
        rotarySteps = 0;
        break;

      default:
        sw = 0;
        rotarySteps = 0;
        break;
    }
    static int Lsw;
    if (R || sw != Lsw ) {
      Lsw = sw;


      //   for (static long QTimer = millis(); (long)( millis() - QTimer ) >= 100; QTimer = millis() ) {
      if (sx > 3)sx = 0;
      c[sw] = '*';
      Serial.print(s[sx++]);
      Serial.print(c[0]);
      Serial.print("\t Kp"); Serial.print(c[1]); Serial.print(consKp);
      Serial.print("\t Ki"); Serial.print(c[2]); Serial.print(consKi);
      Serial.print("\t Kd"); Serial.print(c[3]); Serial.print(consKd);
      Serial.print("\t M"); Serial.print(c[4]); Serial.print(MinPower);
      Serial.print("\t S"); Serial.print(c[5]); Serial.print(SPZero);
      Serial.print("\t Input"); Serial.print(Input * .1);
      Serial.print("\t YawInput "); Serial.print(YawInput);
      Serial.print("\t Turn "); Serial.print(Turn);
      Serial.println();
      myPID.SetTunings(consKp, consKi, consKd);
      c[sw] = ' ';
      //   }
      rotarySteps = 0;
    }
  } /*else {
    static int sx = 0;
    for (static long QTimer = millis(); (long)( millis() - QTimer ) >= 1000; QTimer = millis() ) {
      if (sx > 3)sx = 0;
      Serial.print(s[sx++]);
      Serial.print("\t S");  Serial.print(SPZero);
      Serial.print("\t Input"); Serial.print(Input*.1);
      Serial.print("\t YawInput "); Serial.print(YawInput);
      Serial.print("\t Turn "); Serial.print(Turn);
      Serial.println();
    }
  }*/
}