Arduino freezing- serial stops and motors keep running indefinitely

Hello! I have been trying to build a 2-wheeled balancing robot for a while now (big thanks to Kas, a lot of my code and ideas come from his posts here: Prevas Parekring).

Currently, I'm having trouble running the robot because it tends to "freeze" after a few seconds. When this happens, all serial communication with the computer stops and the motors continue running indefinitely with the same direction and speed as they had right before the freeze.

I am taking angle measurements using an IMU near the bottom of the robot. The wheels are each powered by a DC motor, which are being controlled by a motor driver board. The board is being powered by an 11.1V battery, and the Arduino will be powered by a separate battery (though during testing I am just powering it through the computer).

Before it freezes, the data reported to the serial plotter shows there is significant noise in the IMU readings (and therefore the angle the robot thinks it is at). I believe this issue is related to the freezing. When either the motors or the 11.1V battery are not plugged into the motor driver (basically, when the motors aren't running), the robot is not affected by the noise or the freezing.

Before posting I talked to a friend of mine who knows a bit more about electrical engineering, and he suggested I add toroids to reduce noise (which can be seen in the attached pictures). They didn't seem to have any effect.

Please let me know if you have any ideas about how I could fix this issue. Thanks for reading! Below I've attached the code (which I doubt will be helpful, but just in case), some information about the parts I used, and a few pictures which might be helpful.

Code:

#include "MPU6050.h"

int stdTime=10;    //Varies +/- 1ms: expected
int lastTimeReal=stdTime;
int lastTime=stdTime;
unsigned long startTime=0;

MPU6050 accelGyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

int senValue[3]={0,0,0};
int senZero[3]={0,0,0};
#define gyrY 0
#define accZ 1
#define accX 2
float actAngle;
float accAngle;
float gyroRate;

const int motor1Dir=2;
const int motor2Dir=4;
const int motor1Speed=5;
const int motor2Speed=6;

float drive=0;
float setPoint=0;

void setup(){
  Serial.begin(115200);

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
      Wire.begin();
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
      Fastwire::setup(400, true);
  #endif
  accelGyro.initialize();

  Wire.setClock(1000000);

  pinMode(motor1Dir, OUTPUT);
  pinMode(motor2Dir, OUTPUT);
  pinMode(motor1Speed, OUTPUT);
  pinMode(motor2Speed, OUTPUT); 

  delay(10);
  calibrateSensors();
}

void loop(){
  updateSensors();    //Averages 5 readings of each sensor 
  accAngle=getAccAngle();    //Calculates angle using tangent of x and z
  gyroRate=getGyroRate();
  actAngle=kalmanCalculate(accAngle,gyroRate,lastTime);

  drive=updatePID(setPoint, actAngle);    //Uses PID to calculate speed and direction of drive
  if (actAngle>(setPoint-45) && actAngle<(setPoint+45))
    driveMotor(drive);
  else
    driveMotor(0);    //If the robot falls over, stop driving

  serialOutPlotter();
  
  lastTimeReal=millis()-startTime;
  if (lastTimeReal<stdTime)
    delay(stdTime-lastTimeReal);
  lastTime=millis()-startTime;
  //serialTiming();
  startTime=millis();
}

//All functions below generate outputs for testing/ debugging
void serialTiming(){    //Verifies the loop runs at a constant rate    
  static int skip=0;    
  if (skip++==5){
    skip=0;
    Serial.print(lastTimeReal); Serial.print(",");
    Serial.println(lastTime);
  }
}

void serialOutPlotter(){    //Sends raw and kalman angle values to serial plotter
  Serial.print(actAngle); Serial.print(" ");
  Serial.println(accAngle);
}

void serialOutSensor(){
  static int skip=0;
  if (skip++==20){
    skip=0;
    Serial.print(accAngle); Serial.print(", ");
    Serial.println(gyroRate);
  }
}

void serialOutRaw(){
  static int skip=0;
  if (skip++==5){
    skip=0;
    Serial.print("accX:"); Serial.print(senValue[accX]);
    Serial.print(" accZ:"); Serial.print(senValue[accZ]);
    Serial.print(" gyrY:"); Serial.println(senValue[gyrY]); 
  }
}

//***KALMAN FILTERING TAB***
float qAngle=0.001;
float qGyro=0.003;
float rAngle=0.03;

float xAngle=0;
float xBias=0;
float P00=0, P01=0, P10=0, P11=0;
float dt, y, S;
float K0, K1;

float kalmanCalculate(float newAngle, float newRate, int looptime){
  dt=float(looptime)/1000;
  xAngle+=dt*(newRate-xBias);
  P00+=-dt*(P10+P01)+qAngle*dt;
  P01+=-dt*P11;
  P10+=-dt*P11;
  P11+=qGyro*dt;

  y=newAngle-xAngle;
  S=P00+rAngle;
  K0=P00/S;
  K1=P10/S;

  xAngle+=K0*y;
  xBias+=K1*y;
  P00-=K0*P00;
  P01-=K0*P01;
  P10-=K1*P00;
  P11-=K1*P01;

  return xAngle;
}

//***MOTOR TAB***
int driveMotor(float torque){
  //Serial.println(torque);
  if (torque>=0){    //Forward
    digitalWrite(motor1Dir, HIGH);
    digitalWrite(motor2Dir, HIGH);
    //torque+=5;
  } else {
    digitalWrite(motor1Dir, LOW);
    digitalWrite(motor2Dir, LOW);
    torque=abs(torque);
    //torque-=5;
  }

  analogWrite(motor1Speed, torque);
  analogWrite(motor2Speed, torque);
}

//***PID TAB***
float guard=20.0;
float K=1;
int Kp=27;
int Ki=0;
int Kd=0;

int lastError=0;
int intError=0;
int pTerm=0, iTerm=0, dTerm=0;

float updatePID(float targetPos, float currentPos){
  //Serial.println(pTerm);
  float error=targetPos-currentPos;
  pTerm=Kp*error;
  intError+=error;
  intError=constrain(intError, -255, 255);
  iTerm=Ki*intError;
  dTerm=Kd*(error-lastError);
  lastError=error;
  return constrain(K*(pTerm+iTerm+dTerm),-255,255);
}

//***SENSORS TAB***
void calibrateSensors(){    //Original didn't work unless robot was held perfectly upright when program started
  /*long v;    
  for (int n=0; n<3; n++){
    v=0;
    for (int i=0; i<50; i++)
      v+=readSensor(n);
    senZero[n]= v/50;
  }
  senZero[accZ]-=16384;*/

  senZero[accZ]=366;    //16750-16384
  senZero[accX]=1400;
  senZero[gyrY]=0;    //Values when robot is held up against wall (perfectly upright)
}    //Current balance point at 0.44 degrees left

int readSensor(int channel){
  //gyrY=0, accZ=1, accX=2
  accelGyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  if (channel==0)
    return gy;
  if (channel==1)
    return az;
  if (channel==2)
    return ax;
}

void updateSensors(){
  long v;
  for (int n=0; n<3; n++){
    v=0;
    for (int i=0; i<5; i++)
      v+=readSensor(n);
    senValue[n]=v/5-senZero[n];
  }
}

float getAccAngle(){
  return (atan2(-senValue[accZ], -senValue[accX])*180/3.141592+90);
}

float getGyroRate(){
  return senValue[gyrY]*1/131;    //131 lsb/deg/sec, mult by 1/131
}

Part information:
Using Arduino Uno
Motors: Pololu - 30:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Spur Pinion)
Motor controller: https://www.robotshop.com/media/files/pdf/user-manual-mdd10a.pdf
MPU6050 IMU: https://datasheet.octopart.com/MPU-6050-InvenSense-datasheet-21631037.pdf
MPU6050 library: i2cdevlib/Arduino/MPU6050 at master · jrowberg/i2cdevlib · GitHub

Balancing Robot Angle.jpg756×1008 317 KB

Balancing Robot.jpg756×1008 426 KB

Balancing Robot Angle.jpg756×1008 317 KB

Balancing Robot.jpg756×1008 426 KB

Wiring Sketch.PDF (45.4 KB)

Your wiring diagram shows neither bypass capacitors not toroids on the motor wiring. How about a drawing showing what you really are doing.

Paul

Sorry about that! It looks like I attached an older version of the wiring sketch. This one shows the toroids as well.

I am not using bypass capacitors because I wasn't aware they would be useful (and frankly I know almost nothing about capacitors in general). I think I have some lying around, so I'll work on understanding and adding those.

Thanks!

Wiring Sketch.PDF (46.3 KB)

Please post images as PNGs or JPGs and make them visible in your Posts so we don't have to download them. See this Simple Image Guide

...R

You seem to call the function serialOutPlotter(); in every iteration of loop() - which is far too frequent. The Serial data will fill up the Serial Output Buffer and then block the Arduino until it is all sent.

It may help to use a higher baud rate (say 500000) but ideally, just print data once per second or maybe twice.

If you really need to see the data for every iteration write the values to an array (maybe 30 or 50 values) and print the array when it is full. And accept that printing the array will interfere with things.

...R

Thanks for the heads up about the images! I've attached a PNG of the wiring diagram to this message so I can display it inline.

I didn't realize the serial output buffer getting filled could be an issue. I tried limiting it to only print twice every second. Interestingly, while it didn't stop the freezing, it seemed to reduce the speed with which the Arduino freezes. Now I can run it for a minute or more before it stops rather than only a few seconds.

Edit: it seems when the Arduino is running off of the battery it still freezes about as fast as it was before, even though when connected to the computer it now lasts longer.

Wiring Sketch.PNG1668×865 4.89 KB

Wiring Sketch.PNG1668×865 4.89 KB

I would be surprised if your problem was a result from emf noise. Several years ago, I built a bot using a custom created H-bridge driver for the motors, gyro for directional awareness, and ultrasonic sensors for spacial awareness, etc - all on a PC board without a ground plane.

Since it appears the arduino did not reboot, I would look into simulating how your code handles the manual input of float values via serial monitor. I've had many an issue with float usage.

I commented out all the code related to the IMU and tried manually entering floats via the serial monitor to control how the motors were driving and it worked just fine. Is that what you were suggesting? I'm not entirely sure what would have gone wrong or what I'm looking for.

Thanks for the help!

MattSean:
Is that what you were suggesting?

Yes. I'd also be interested if the same error occurred when the motor driver board is removed; after removing the board, manually push the bot around to generate the changing values from the gyro sensor.

Also, I'd create a 2 second heartbeat (i.e. pin 13 LED) in the void loop.

I'm not sure about you, but when I encounter an error, I like to isolate components/code.

I am not using bypass capacitors because I wasn't aware they would be useful

Noise suppression caps on the motors are almost always required, as the motors inject severe electrical disturbances into the power supply leads, causing controller resets and data corruption. In especially bad cases noise spikes can actually destroy the controller.

Toroids with just a few windings as shown in your photos above won't be very effective.

Pololu has a good short tutorial on how to install motor caps. You may need to install 3 on each motor.

The wiring to the IMU seems to be two twisted pairs, one for power/ground and the other carrying
SCL and SDA. This is wrong, SCL and SDA must be run twisted with ground, its essential you don't create
a huge loop antenna as you have, that alone could crash the Arduino easily.

Each signal wire has return currents via the nearest ground, so make sure there is a ground wire
with each signal wire to avoid creating a big loop (which will act as an antenna like any open loop).
You can share the same ground wire with several signals, but keep them together, preferably twisted
together for minimum interference.