@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.
https://europe1.discourse-cdn.com/arduino/original/4X/7/4/6/74631f82563988409a5cddf32fdacbfee3e4707f.jpeg
@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();
}
}*/
}