Balancing robot Arduino crashing

After having spent several weeks and redesigning multiple times, I have decided to go ahead and ask others for help regarding my balancing robot issue.

I initially used an Arduino R3 Motor shield for my robot, and was close to getting it to balance correctly. However, once the robot tipped too far it seemed to crash the Arduino, and the motors would simply keep spinning at the last speed they were at (or would erratically spin in different directions), forcing me to reset it. I am using a single power supply for my entire robot, and the battery is connected to the motors directly, but goes through a buck converter switching regulator to the Arduino (set at 9v). After this did not work, I then decided to seperate the power supplies completely (using a 6V battery pack for the Arduino), added 100nF capacitors to the terminals of the motors, and purchased a VNH5019 Pololu motor shield. However, now the robot barely works. Maybe the VNH5019 is not meant for rapid switching, but the wheels change direction too slowly when the PWM changes. Also, if the motors change direction a few times, the Arduino crashes almost immediately within a few seconds, even if it is only a few degrees. This seems to happen regardless of whether I separate the power supplies or use a single one with a buck converter.

Right now, my main suspicion is either my motor power supply (a 12V 6000mAh battery pack, but is only rated at 3A max), or the fact that my Arduino is located right above one of the motors, and the MPU6050 is situated between both of them. Although if the battery maxed out, if anything the motors would stop, not crash the Arduino completely. When it changes direction, the Power light on the motor shield seems to flicker a tiny bit, but all other power lights including the MPU6050 and Arduino remain solid. It was working fine before I switched to the VNH5019 though. I also have the VNH5019 Arduino and Motor power supply disconnected on the shield.

Unfortunately, I do not have an oscilloscope so I can’t check for any erratic noise behavior unless I use a simple multimeter.

Here is all the information I can think of if anyone notices anything odd:

Arduino Duemilanove
Pololu VNH5019 Dual Motor Shield
MPU6050 Accel/Gyro
Pololu 37Dx52L motor x 2 (50:1; I also have a pair of 30:1 that I tried initially)
Power Supplies: Arduino (6v 4xAAA Battery Pack), Motors (TalentCell 12V 6000mAh 3A max bat pack)
Chassis made of Aluminum and Brass support rods
HC-05 Master/Slave Bluetooth Module (For serial reading/control on phone)


#include <PID_v1.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "DualVNH5019MotorShield.h"
#include <SoftwareSerial.h>

#define MPU_INT 1

DualVNH5019MotorShield md;
SoftwareSerial serial(0, 1);
String streamData;

boolean MODE_BREAK = false;
boolean PRINT_VALS = false;

double kP = 0;
double kI = 0;
double kD = 0;
int kPin = 3;

MPU6050 mpu;

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

Quaternion q;
VectorFloat gravity;
int16_t gyro[3];
float ypr[3];

int calcOut = 0;

double incAngle, balAngle, pwmOUT, rotation;

PID pid(&incAngle, &pwmOUT, &balAngle, kP, kI, kD, DIRECT);

volatile bool mpuInterrupt = false;
void dmpDataReady() {
  mpuInterrupt = true;

void stopIfFault() {
  if(md.getM1Fault()) {
    serial.println("M1 fault");
  if(md.getM2Fault()) {
    serial.println("M2 fault");

void setup() {
  balAngle = 0;
  incAngle = 0;
  pwmOUT = 0;
  rotation = 0;

  kP = analogRead(3)/10.0;
  pid.SetTunings(kP, kI, kD);

  serial.println("Balancing robot enabled!");


  pid.SetOutputLimits(-400, 400);
  TWBR = 24;

  serial.println(F("Initializing I2C devices..."));
  serial.println(F("Testing device connections..."));
  serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));

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


  if(devStatus == 0) {
    serial.println(F("Enabling DMP..."));

    serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(MPU_INT, 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 "));

void loop() {
  if(serial.available()) {
    if( == '+') {
      char curChar;
      while(serial.available()) {
        curChar = (char);
        if(curChar != '+') {
          streamData += curChar;
        } else {
    if(streamData != "") {
      if(streamData == "stop" || streamData == "Stop" || streamData == "STOP") {
        MODE_BREAK = true;
        serial.println("Stopping Motors");
      } else if(streamData == "start" || streamData == "Start" || streamData == "START") {
        MODE_BREAK = false;
        serial.println("Starting Motors");
      } else if(streamData == "pid" || streamData == "Pid" || streamData == "PID") {
        serial.print("PID Values| kP: ");
        serial.print(" kI: ");
        serial.print(" kD: ");
      } else if(streamData == "values" || streamData == "Values" || streamData == "VALUES") {
        if(PRINT_VALS) {
          PRINT_VALS = false;
        } else {
          PRINT_VALS = true;
      } else {
        serial.println("Command Unknown");
    streamData = "";
  if(!dmpReady) return;
  while(!mpuInterrupt && fifoCount < packetSize) {
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  fifoCount = mpu.getFIFOCount();

  if((mpuIntStatus & 0x10) || fifoCount == 1024) {
  } else if(mpuIntStatus & 0x02) {
    while(fifoCount < packetSize) {
      fifoCount = mpu.getFIFOCount();
    mpu.getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGyro(gyro, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    rotation = ypr[0] * 180/M_PI;
    incAngle = ypr[1] * 180/M_PI;
    if(PRINT_VALS) {
      serial.print("r: ");
      serial.print(" a: ");
      serial.print(" o: ");

    if(kP != analogRead(3)/10.0) {
      kP = analogRead(3)/10.0;
      pid.SetTunings(kP, kI, kD);

    if(!MODE_BREAK) {
      if(incAngle > 70 || incAngle < -70) {
      } else {
    } else {

Main Circuitry of Robot

Entire Robot
View of Bottom Half
Mpu and Motors
(Another thing I am considering is that the wires are all routed too close to each other, and the motors could be interfering. It didn’t seem nearly as bad with the R3 Motor Shield though)

So if anyone has any suggestions, I am open to anything. I haven’t even begun to tweak the PID values yet :confused:.

You definitely have power supply problems. The motors are listed as 5 amperes stall, and they draw that much briefly, every time they start up. So, your 3A motor battery is severely limiting.

The VHN5019 is faster and more efficient than the Arduino R3 motor shield, which would actually worsen such problems. Keep in mind that when motors are rapidly reversed, they draw up to TWICE the stall current.

The Fritzing diagram is extremely difficult to interpret (even a hand drawn schematic would be much better), but it does not appear to show proper ground connections between all the components, especially the motor driver.

If you post another circuit diagram, please don't use Fritzing. It is only for the simplest circuits and for rank beginners.

All the high current wiring should be twisted pair and kept as far away from the logic circuitry and MPU6050 board as possible, or at least on the opposite side of a metal screen.

Star grounding at the motor driver board, ie the motor and logic supply grounds meet only at one place, the motor driver.

The Arduino voltage regulator needs at least 7V to function properly. Try a 6xAA pack or a 2s (7.4V) LiPo. A 9v battery will not work for very long.