Arduino Uno Freezes when Program is Running

I have written a program that controls a brush-less motor (attached to an arm, hinged at one end), and uses PID control to keep the arm at a certain angle. I am using a Turnigy 1000kv motor and Plush 30A ESC, and displaying the measured angle as well as setpoint to an LCD. The angle is calculated from values I get from an MPU6050 IMU.

But for some reason, the program will sometimes freeze. It happens both when the motor is running and when it is not. When this happens, the motor keeps running at the last written value (which is bad if it was increased to balance out). I have included the code, does anybody have a clue as to why this would happen? I have tried a different Arduino, and have not had this problem with other projects.

#include <PID_v1.h>
#include <math.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include <LiquidCrystal.h>


//-----------------------------------------------Variable Declarations------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------

const byte mPin = 3;                                                                                                
const int switchPin = 4;  

LiquidCrystal lcd(7, 8, 9, 10, 11, 12);

double angleSetpoint, angleIn, angleOut;
double kp = 1.75, ki = 0.50, kd = 0.05;
PID anglePID(&angleIn, &angleOut, &angleSetpoint, kp, ki, kd, DIRECT);

double rateSetpoint, rateIn, rateOut;
double kpr = 0.30, kir = 0.00, kdr = 0.01;
PID ratePID(&rateIn, &rateOut, &rateSetpoint, kpr, kir, kdr, DIRECT);

double motorW;                                                                                                    //MotorWrite variables
int switchCounter = 0;
long int dispTimer = 0;   

MPU6050 accelgyro(0x68);
int16_t ax, ay, az;
int16_t gx, gy, gz;
double aY, aZ, gX, accelAngle, gyroAngle, calcAngle, gyroX;

const int MOTOR_MIN_VAL = 150;
const int MOTOR_MAX_VAL = 255;
const int MOTOR_INIT_VAL = 120;

double ntg;                                                                                                       //To hold cycle time for gyroscope
int armMotor = 0;

const double alpha = 0.1;                                                                                         //Constant for accelerometer Low Pass Filter - The smaller alpha, the lower the cutoff frequency (and the more lag)

const double radToDeg = 180/3.1415926535;

//--------------------------------Setup/Loop Procedures----------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------------------------------------

void setup(){
  delay(200);

  pinMode(mPin, OUTPUT);                                                                                                     //Motor pins as OUTPUTS
  pinMode(switchPin, INPUT);

  Wire.begin();

  anglePID.SetSampleTime(10);
  anglePID.SetOutputLimits(-50, 50);
  anglePID.SetMode(AUTOMATIC);

  ratePID.SetSampleTime(10);
  ratePID.SetOutputLimits(-100,100);
  ratePID.SetMode(AUTOMATIC);

  motorInit();
  accelgyro.initialize();
  writeTo(0x68, 0x1A, 0x05);

  lcd.begin(16, 2);
  lcd.clear();

  delay(200);
  Serial.begin(9600);   
}

void loop(){

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  gX = gx / 65.5;
  double dt = (micros() - ntg)/1000000.00;
  gyroX = -(double)gX * dt;
  ntg = micros();
  accelAngle = -(atan2(ay, az))*radToDeg;
  calcAngle = ((1-alpha)*(calcAngle + gyroX) + alpha * accelAngle);

  if (digitalRead(switchPin) == 1) {
    armMotor = 1;
    switchCounter = 1;
  }
  else armMotor = 0;

  if (armMotor == 0 && switchCounter == 1){
    for (int i = 156; i > 154; i--){
      analogWrite(mPin, i);
      delay(4000);     
    }   
    switchCounter = 0;
  }


  if (armMotor == 1) writeMotorValue();
  else analogWrite(mPin, MOTOR_INIT_VAL);

  if( millis() - dispTimer > 200){
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Angle SP =");
    lcd.print(angleSetpoint);

    lcd.println("    ");
    lcd.setCursor(0,1);
    lcd.print("Meas Angle = ");
    lcd.print(calcAngle);
    dispTimer = millis();
  }
}

//--------------------------------------------Flight Control Code-----------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------

void motorInit(){
  analogWrite(mPin, MOTOR_INIT_VAL);
}


void writeMotorValue(){

  angleIn = calcAngle;
  angleSetpoint = 0.0;
  anglePID.Compute();

  rateSetpoint = angleOut;
  rateIn = -gX;
  ratePID.Compute();

  motorW = MOTOR_MIN_VAL + rateOut;                         
  if (motorW < MOTOR_MIN_VAL) motorW = MOTOR_MIN_VAL;
  if (motorW > MOTOR_MAX_VAL) motorW = MOTOR_MAX_VAL;

  analogWrite(mPin, motorW);
}

void writeTo(int device, byte address, byte val) {
  Wire.beginTransmission(device);                                   //start transmission to device 
  Wire.write(address);                                              // send register address
  Wire.write(val);                                                  // send value to write
  Wire.endTransmission();                                           //end transmission
}

Your code has no useful comments in it, so it is hard to guess what it is supposed to be doing.

  gX = gx / 65.5;

Variable names that differ only in case are a nightmare to keep straight. I'm sure that, with a little thought, you could come up with better names.

You should have some Serial.print() statements at strategic places in your code, to figure out what "it freezes" means. Does the Arduino really get stuck somewhere? Seems unlikely. Does it reset, and start setup() and loop() over again, which might make it appear to have frozen?

I'd be more than happy to go through it and comment lines of code (it's a simple program, didn't think it was necessary as nobody else was going to read it initially). But I do know that it seems to simply stop.... although I do not having it output on the serial port right now, when I did the data would simply stop flowing. Whatever had been previously output onto the serial monitor never changed.

Are you implying that it is likely a section of the code that makes it happen? I can certainly try that. As it is, I have the angle calculated being updated on the LCD constantly. And, as soon as it "freezes," the values stop being updated. I will work on commenting the code and have it up soon.

Thanks for the help!

Here is the code with comments in the places that seemed necessary. As always, any pointers/help/comments are appreciated.

#include <PID_v1.h>
#include <math.h>
#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include <LiquidCrystal.h>


//-----------------------------------------------Variable Declarations------------------------------------------------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------

const byte mPin = 3;                                                                                                
const int switchPin = 4;  

LiquidCrystal lcd(7, 8, 9, 10, 11, 12);

double angleSetpoint, angleIn, angleOut;                                        //PID Library variables and constants
double kp = 1.75, ki = 0.50, kd = 0.05;
PID anglePID(&angleIn, &angleOut, &angleSetpoint, kp, ki, kd, DIRECT);

double rateSetpoint, rateIn, rateOut;
double kpr = 0.30, kir = 0.00, kdr = 0.01;
PID ratePID(&rateIn, &rateOut, &rateSetpoint, kpr, kir, kdr, DIRECT);

double motorW;                                                                   //MotorWrite variables
int switchCounter = 0;
long int dispTimer = 0;   

MPU6050 accelgyro(0x68);
int16_t ax, ay, az;
int16_t gx, gy, gz;
double aY, aZ, gX, accelAngle, gyroAngle, calcAngle, gyroX;

const int MOTOR_MIN_VAL = 150;
const int MOTOR_MAX_VAL = 255;
const int MOTOR_INIT_VAL = 120;

double ntg;                                                                                                       //To hold cycle time for gyroscope
int armMotor = 0;

const double alpha = 0.1;                                                                                         //Constant for accelerometer Low Pass Filter - The smaller alpha, the lower the cutoff frequency (and the more lag)

const double radToDeg = 180/3.1415926535;

//--------------------------------Setup/Loop Procedures----------------------------------------------------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------------------------------------

void setup(){
  delay(200);

  pinMode(mPin, OUTPUT);                                                                                                   
  pinMode(switchPin, INPUT);

  Wire.begin();

  anglePID.SetSampleTime(10);
  anglePID.SetOutputLimits(-50, 50);
  anglePID.SetMode(AUTOMATIC);

  ratePID.SetSampleTime(10);
  ratePID.SetOutputLimits(-100,100);
  ratePID.SetMode(AUTOMATIC);

  motorInit();
  accelgyro.initialize();
  writeTo(0x68, 0x1A, 0x05);

  lcd.begin(16, 2);
  lcd.clear();

  delay(200);
  Serial.begin(9600);   
}

void loop(){

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);              //obtain raw accelerometer/gyroscope values
  gX = gx / 65.5;                                                  //gyroscope sensitivity is 65.5 LSB/deg*s
  double dt = (micros() - ntg)/1000000.00;                          //calculate differential time element
  gyroX = -(double)gX * dt;                                        //gyroscope angle = rate * time elapsed
  ntg = micros();
  accelAngle = -(atan2(ay, az))*radToDeg;                          //accelerometer angle, inverse tangent of acceleration vectors
  calcAngle = ((1-alpha)*(calcAngle + gyroX) + alpha * accelAngle);      //complementary filter

  if (digitalRead(switchPin) == 1) {
    armMotor = 1;
    switchCounter = 1;
  }
  else armMotor = 0;

  if (armMotor == 0 && switchCounter == 1){                        //when true, motor slows down to land
    for (int i = 156; i > 154; i--){
      analogWrite(mPin, i);
      delay(4000);     
    }   
    switchCounter = 0;
  }


  if (armMotor == 1) writeMotorValue();
  else analogWrite(mPin, MOTOR_INIT_VAL);

  if( millis() - dispTimer > 200){                              //displaying setpoint and calculated angle on LCD
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Angle SP =");
    lcd.print(angleSetpoint);

    lcd.println("    ");
    lcd.setCursor(0,1);
    lcd.print("Meas Angle = ");
    lcd.print(calcAngle);
    dispTimer = millis();
  }
}

//--------------------------------------------Flight Control Code-----------------------------------------------------------
//--------------------------------------------------------------------------------------------------------------------------

void motorInit(){
  analogWrite(mPin, MOTOR_INIT_VAL);
}


void writeMotorValue(){

  angleIn = calcAngle;                                            //attitude and rate PID controllers
  angleSetpoint = 0.0;
  anglePID.Compute();

  rateSetpoint = angleOut;
  rateIn = -gX;
  ratePID.Compute();

  motorW = MOTOR_MIN_VAL + rateOut;                         
  if (motorW < MOTOR_MIN_VAL) motorW = MOTOR_MIN_VAL;
  if (motorW > MOTOR_MAX_VAL) motorW = MOTOR_MAX_VAL;

  analogWrite(mPin, motorW);
}

void writeTo(int device, byte address, byte val) {
  Wire.beginTransmission(device);                                   //start transmission to device 
  Wire.write(address);                                              // send register address
  Wire.write(val);                                                  // send value to write
  Wire.endTransmission();                                           //end transmission
}

I’ve no idea what your code is doing, but I’m curious, does it pause for about 12 seconds?

for (int i = 156; i > 154; i--){
  analogWrite(mPin, i);
  delay(4000);
}

Yes it does, but only when I flip a switch. Then it no longer runs based off of the PID controller, and the motor slows down by 1 PWM value every 4 seconds. It just happens to land it well with the setup that I have.

if (armMotor == 1) writeMotorValue();
  else analogWrite(mPin, MOTOR_INIT_VAL);

You are missing some brackets here.

As far as I know, brackets are not necessary when the "if" and "else" statements are one-liners. The IDE does not have a problem with them, and the code runs fine except, of course, when it freezes. Any thoughts about why the freezing might happen?

franduino505:
As far as I know, brackets are not necessary when the "if" and "else" statements are one-liners.

Using brackets rather than "one-liners" can make it much easier to see the structure of a program IMHO

The IDE does not have a problem with them, and the code runs fine except, of course, when it freezes. Any thoughts about why the freezing might happen?

It is not possible to say why or where a program occasionally freezes without trying it and that requires all the hardware that you have.

Have you put in liberal quantities of Serial.println() statements that show you how the logic is progressing through the program. That way you can pinpoint whereabouts in the code the freezing is happening.

It may also be freezing when some variable gets to a particular value so Serial.println() statements to monitor key variables can also be useful.

...R