Freeze randomly during programme using MPU-6050

Hi,
I’ve built a game for my nephews which detects movement using an MPU-6050. The code runs as follows:

  1. Choose sensitivity value (using rotary enocoder showing values on LCD screen)
  2. Run stabilising loop - basically waits for the MPU-6050 values to stabilise before starting
  3. Play the game. When motion is detected, a sound is played and the player is out.

Now I realise the MPU-6050 can be pretty complicated to control, so I’ve used a simplified bit of code without interupts. Yes, I’m new to this. And yes, my code probably looks like an unholy mess.

By some miracle it basically works… almost. It’s working fine, but then freezes from time to time, and I don’t know why. By freeze, I mean it seems to stop running the loop and gets stuck at one of the stages, and stops detecting movement or doing anything. There’s a good chance I’m missing something simple so if anyone has any ideas I’d much appreciate it. Code is below,
Many thanks,
Paul

#include <LiquidCrystal.h> // include the library code for LCD
#include "pitches.h" // include library for buzzer
LiquidCrystal lcd(7, 8, 9, 10, 11, 12); // initialize the library with the numbers of the interface pins

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
MPU6050 mpu;
uint16_t packetSize;
uint16_t fifoCount;
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
float ypr[3];
float YAWnew;
float ROLLnew;
float PITCHnew;
float YAW = 10;
float PITCH = 10;
float ROLL = 10;
int numberstablereads = 0;
int detector = 1;
int buildstart = 0;
int rotaryswitch;
int firstlooponly = 0;
float sensitivity = 0.04; // most sensitive amount possible. Lower it is, the more sensitive
float sensitivityincrease = 0.02; // increments the sensitivity goes up in on difficulty scale
int cyclescounter = 0;


//This bit is for the rotary encoder
#define CLK 6
#define DT 4
#define SW 14

int counter = 10; // Used to track the rotary encoder count in terms of sensitivity level

int state; //This bit is for the rotary encoder
int Laststate; //This bit is for the rotary encoder

int startskip = 0;

void setup() {

  pinMode(CLK, INPUT_PULLUP); //This bit is for the rotary encoder
  pinMode(DT, INPUT_PULLUP); //This bit is for the rotary encoder
  pinMode(SW, INPUT_PULLUP); //This bit is for the rotary encoder
  Laststate = digitalRead(CLK); //This bit is for the rotary encoder

  Wire.begin();
  TWBR = 24;
  mpu.initialize();
  mpu.dmpInitialize();
  mpu.setXAccelOffset(-1343);
  mpu.setYAccelOffset(-1155);
  mpu.setZAccelOffset(1033);
  mpu.setXGyroOffset(19);
  mpu.setYGyroOffset(-27);
  mpu.setZGyroOffset(16);
  mpu.setDMPEnabled(true);
  packetSize = mpu.dmpGetFIFOPacketSize();
  fifoCount = mpu.getFIFOCount();

  Serial.begin(115200);

  lcd.begin(16, 2); // set up the LCD's number of columns and rows:

  lcd.clear();
  lcd.setCursor(3, 0);
  lcd.print("DIFFICULTY");
  lcd.setCursor(0, 1);
  lcd.print("easy");
  lcd.setCursor(5, 1);
  lcd.print("-");
  lcd.setCursor(12, 1);
  lcd.print("hard");
  lcd.setCursor(10, 1);
  lcd.print("+");
  lcd.setCursor(7, 1);
  lcd.print(counter);

}

void loop() {

  mpu.resetFIFO();

  if (startskip == 0) {
    state = digitalRead(CLK);
    int rotaryswitch = digitalRead(SW);
    if (rotaryswitch == 0) {
      startskip++; // when button pressed on switch, it starts the game
    }
    cyclescounter = 0; // re-set cycles count

    if (state != Laststate)
    {
      if (digitalRead(DT) != state)
      {
        counter--;
      }
      else
      {
        counter++;
      }

      if (counter > 20) {
        counter = 20; // limiting the range of the counter
      }
      if (counter < 1) {
        counter = 1; // limiting the range of the counter
      }

      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("DIFFICULTY");
      lcd.setCursor(0, 1);
      lcd.print("easy");
      lcd.setCursor(5, 1);
      lcd.print("-");
      lcd.setCursor(12, 1);
      lcd.print("hard");
      lcd.setCursor(10, 1);
      lcd.print("+");
      lcd.setCursor(7, 1);
      lcd.print(counter);

      delay (2);
    }
  }

  else {

    cyclescounter++;

    if (firstlooponly == 0) {
      int counterinvert = 20 - counter;
      sensitivity = sensitivity + sensitivityincrease * counterinvert; // setting sensitivity based on amount selected at start
      lcd.clear();
      lcd.print("** GET READY! **"); // Print a message to the LCD.
     
      Serial.print("Counter=");
      Serial.print(counter);
      Serial.print("\t");
      Serial.print("Sensitivity=");
      Serial.print(sensitivity);
      Serial.print("\t");
      Serial.print("counterinvert=");
      Serial.print(counterinvert);
      Serial.print("\t");

    }


    mpu.resetFIFO();

    while (fifoCount < packetSize) {


      fifoCount = mpu.getFIFOCount();
    }

    if (fifoCount == 1024) {

      mpu.resetFIFO();
      Serial.println(F("FIFO overflow!"));

    }
    else {

      if (fifoCount % packetSize != 0) {

        mpu.resetFIFO();

      }
      else {

        while (fifoCount >= packetSize) {

          mpu.getFIFOBytes(fifoBuffer, packetSize);
          fifoCount -= packetSize;

        }

        Serial.print("YAW=");
        Serial.print(YAW);
        Serial.print("\t");

        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); // This is where the data get read

        YAW = (ypr[0] * 180 / M_PI);
        PITCH = (ypr[1] * 180 / M_PI);
        ROLL = (ypr[2] * 180 / M_PI);

        if (YAW == YAWnew) {
          numberstablereads++; Serial.print("STABLEREAD");
          Serial.print(numberstablereads);
          Serial.print('\n');
        }

        if (numberstablereads > 300 && YAWnew > YAW + sensitivity) {
          detector = 1;
        }
        if (numberstablereads > 300 && YAWnew < YAW - sensitivity) {
          detector = 1;
        }
        if (numberstablereads > 300 && PITCHnew > PITCH + sensitivity) {
          detector = 1;
        }
        if (numberstablereads > 300 && PITCHnew < PITCH - sensitivity) {
          detector = 1;
        }
        if (numberstablereads > 300 && ROLLnew > ROLL + sensitivity) {
          detector = 1;
        }
        if (numberstablereads > 300 && ROLLnew < ROLL - sensitivity) {
          detector = 1;
        }

        firstlooponly++;

        if (numberstablereads > 300 && buildstart == 0) {
          lcd.clear();

          lcd.setCursor(0, 0);
          lcd.print("START BUILDING!!");
          lcd.setCursor(0, 1);
          lcd.print("[Difficulty=");
          lcd.setCursor(12, 1);
          lcd.print(counter);
          lcd.setCursor(14, 1);
          lcd.print("]");

          buildstart++;

          tone (3, NOTE_F5, 100);
          delay (100);
          tone (3, NOTE_C6, 800);
          delay (800);

          mpu.resetFIFO();
        }

        if (detector == 1) {
          Serial.print ("MOTION DETECTED!" );

          lcd.clear();
          lcd.setCursor(1, 0);
          lcd.print("MOTION DETECTED!");
          lcd.setCursor(0, 1);
          lcd.print("!!!PLAYER OUT!!!");

          tone (3, NOTE_D5, 100);
          delay (50);
          tone (3, NOTE_C5, 400);
          delay (5000);
          mpu.resetFIFO();
          lcd.clear();

          detector = 0;
          numberstablereads = 250;
          buildstart = 0;

          lcd.clear();
          lcd.print("** GET READY! **"); // Print a message to the LCD.
        
        }

        YAWnew = YAW;
        PITCHnew = PITCH;
        ROLLnew = ROLL;

        Serial.print("YAWnew=");
        Serial.print(YAW);
        Serial.print("\t");

        Serial.print("PITCH=");
        Serial.print(PITCH);
        Serial.print("\t");

        Serial.print("ROLL=");
        Serial.print(ROLL);
        Serial.print('\n');

        Serial.print("CyclesCounter=");
        Serial.print(cyclescounter);
        Serial.print('\n');

       // when button pressed on switch, it re-sets back to the start
        rotaryswitch = digitalRead(SW);
        if (rotaryswitch == 0 && cyclescounter > 100) {
          startskip = 0;
          firstlooponly = 0;
          lcd.clear();
          lcd.setCursor(2, 0);
          lcd.print("RESTARTING!!");
          lcd.setCursor(2, 1);
          lcd.print("RESTARTING!!");
          delay (1500);
          lcd.clear();
          lcd.setCursor(3, 0);
          lcd.print("DIFFICULTY");
          lcd.setCursor(0, 1);
          lcd.print("easy");
          lcd.setCursor(5, 1);
          lcd.print("-");
          lcd.setCursor(12, 1);
          lcd.print("hard");
          lcd.setCursor(10, 1);
          lcd.print("+");
          lcd.setCursor(7, 1);
          lcd.print(counter);
        }
      }
    }
  }
}