user67602: Running a DC motor(6.5A max input), motor driver (MD10C 7Amp peak output), and arduino Mega.
Basically, I am creating a stabilized object based on the accelerometer's input. It works great but all of a sudden, very randomly, it glitches and gets stuck in whatever mode it was in last, so if it was motor A HIGH and motor B LOW at speed 200, itll just do that and I have to unplug it, put in a blank code, then my code again, then start it, itll work great then do it all over again. The time it takes for it to glitch out is random, I can't seem to find any patterns to it.
here is my code
//Code for accelerometer controlling an encoded DC
#include <Encoder.h>
#include <Wire.h>
#include <MPU6050.h>
#include <QueueArray.h>

//Class to read the encoder 
Encoder myEnc(19,18);

//I2C address of the MPU-6050
const int MPU_addr=0x68;

const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 200 Hz              = 1250
const uint16_t t1_comp = 1250;

// Buffered data for sending/receiving to/from the ISR 
volatile bool isr_flag = false;  //Used to flag when the ISR when triggered 
volatile unsigned long isr_time = 0;  //Input updated by ISR
volatile byte isr_motor_direction; //Output calculated and updated in loop()
volatile byte isr_motor_speed = 0; // Output calculated and updated in loop()

//initializing the MPU6050 (accel/gyro input)
MPU6050 sensor;

//accerleromter in the y direction
int16_t AcY;
//not using
int16_t AcX, AcZ, GyX, GyY, GyZ, Tmp;

//new varible for mapping the accelerometer to degrees
int yAng;

//Variables to create PID method
int currentTheta;
float error;
int dt = 5;
long integral=0;
long deriv;
int previousError;

//accel on light, always want it at 0
int setpoint = 0;

//change speeds of the motor 
#define MOTOR_SPEED_PIN    3
//change direction of the motor 
#define MOTOR_DIRECTION_PIN   2 
#define MOTOR_DIRECTION_CW    LOW
#define MOTOR_DIRECTION_CCW   HIGH






void setup() 
{
  //Disable global interrupt 
  cli();
  
  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &= ~(1 << WGM13);
  TCCR1B |= (1 << WGM12);

  
  // Set to prescalar of 64
  TCCR1B &= ~(1 << CS12);
  TCCR1B |= (1 << CS11);
  TCCR1B |= (1 << CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 << OCIE1A);

  // Enable global interrupt
  sei();

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  
  Serial.begin(9600);

  pinMode(MOTOR_SPEED_PIN, OUTPUT);
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);

  sensor.initialize();
}







void loop()
{  
    //Begin critical section 
    //

    cli();
    bool flag = isr_flag; //Copy the ISR flag
    isr_flag = false; //Reset the ISR flag
    sei();

    //
    //End critical section 

    if (flag == true)
    {
      DebugMsgTime("PID algorithm begin");

      //Begin critical section
      //

      //Getting a copy of the data that has been modified by the ISR
      cli();
      unsigned long time = isr_time;
      sei();

      //
      //End critical section 

      static unsigned long previous_time = 0;
      previous_time = time;

      //Wire reading
      
      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr,14,true);
    
      //reading each of the 6 axis's of the MPU6050 and the temp
      AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
      //Only using AcY
      AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
      Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
      GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
      GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
      GyZ=Wire.read()<<8|Wire.read();
  
      //mapping AcY to yAng from accel units to degrees
      yAng = map(AcY, -17000, 17000, -90, 90);

      //Calculate motor parameters locally
      long pos = PID(yAng);
      byte motor_direction = pos > 0 ? MOTOR_DIRECTION_CW : MOTOR_DIRECTION_CCW //make sure that this is correct
      pos = abs(pos);
      pos = constrain(pos, 0, 100);
      byte motor_speed = map(pos, 0, 100, 0, 200);

      //Begin Critical Section
      //

      // Update the ISR data ready for output at the next interrupt.
      // This keeps the outputs locked to each other and synchronised to the interrupt.
      cli();
      isr_motor_direction = motor_direction;
      isr_motor_speed = motor_speed;
      sei();

      //
      //End Critical Section

      DebugMsgTime("PID algorithm end");
    }  
}







ISR(TIMER1_COMPA_vect)
{
  //Output
  digitalWrite(MOTOR_DIRECTION_PIN, isr_motor_direction);
  analogWrite(MOTOR_SPEED_PIN, isr_motor_speed);

  //Input
  isr_time = micros();
  isr_flag = true;
}







//PID loop to find error between setpoint and currentTheta
long PID(int currentTheta)
{
    error = setpoint - currentTheta;
    integral = integral + error*dt;
    deriv = (error-previousError)/dt;
    long output = 1.9 * error; + .8 * integral + 40 * deriv; //2.8,.7,70 //1.3,.8,40
    previousError = error;
    return output; 
}







void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = micros();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}


user68310: What happens if the while condition fails? i.e. if millis() - time is greater than or equal to 20. Is this what you intended? If there's too much other stuff in the while loop, it may quit it and never re-enter it.
loop()
I've added a call to a function that prints debug information.
unsigned long time = 0;

void loop()
{
  DebugMsgTime("loop() begin");

  while (millis() - time < 20)
  {
    DebugMsgTime("while begin");

    time = millis();

    //delay(10);  // Short delay OK.
    delay(20);  // Long delay causes while loop to quit.

    DebugMsgTime("while end");
  }
  DebugMsgTime("loop() end");
}

DebugMsgTime()
This function prints a message along with the current time and the delta time since it was last called.
void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = millis();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}

Debug Output

It works great but all of a sudden, very randomly, it glitches and
gets stuck in whatever mode it was in last

Basic Encoder Test:
loop() begin
0
0
while begin  // It works great...
1
1
while end
2
1
while begin
12
10
while end
34
22           // but all of a sudden, very randomly, it glitches...
loop() end   // and gets stuck in whatever mode it was in last
54
20
loop() begin
74
20
loop() end
97
23
loop() begin
118
21
loop() end

As you can see, using this while loop is a very inaccurate way to generate regular sampling times.
loop() FIX
As @FerryBig pointed out in the comments, you need to change the time comparison < at the start of the loop. You also don't need the while loop because loop() is already called cyclically. Instead, put the time comparison in an if statement, something like this which gives you a rough 20 ms:
void loop()
{
  static unsigned long previous_time = 0;
  unsigned long time = millis();
  unsigned long delta_time = time - previous_time;

  if (delta_time >= 20)
  {
    previous_time = time;

    // Do PID algorithm.
  }
}

However, for a PID controller you may need more accuracy.
Timer Interrupt
A better method would be to use a timer interrupt to call an Interrupt Service Routine (ISR) at regular intervals. This ISR would do two simple things in this order:

Output: digitalWrite(motor_direction); and analogWrite(3, mtr2);
Input: set a flag that indicates that the gyro Wire needs reading.

Then, when the flag is set, the main loop() would read the gyro Wire, and calculate motor_direction and mtr2 in readiness for the next interrupt.
const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 10 Hz              = 25000
const uint16_t t1_comp = 25000;

volatile bool isr_flag = false;       // Used to flag when the ISR has updated the data.
volatile unsigned long isr_time = 0;  // The data.

void setup()
{
  // Disable global interrupt
  cli();

  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &= ~(1 << WGM13);
  TCCR1B |= (1 << WGM12);

  // Set to prescalar of 64
  TCCR1B &= ~(1 << CS12);
  TCCR1B |= (1 << CS11);
  TCCR1B |= (1 << CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 << OCIE1A);

  // Enable global interrupt
  sei();

  Serial.begin(9600);
}

void loop()
{
  //
  // BEGIN CRITICAL SECTION (Atomic)
  //

  cli();
  bool flag = isr_flag;  // Copy the ISR flag.
  isr_flag = false;      // Reset the ISR flag.
  sei();

  //
  // END CRITICAL SECTION
  //

  Serial.println(flag);

  if (flag == true)
  {
    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Get a copy of the data that has been modified by the ISR.
    cli();
    unsigned long time = isr_time;
    sei();

    //
    // END CRITICAL SECTION
    //

    static unsigned long previous_time = 0;
    Serial.println(time - previous_time);
    previous_time = time;
  }

  delay(25);
}

ISR(TIMER1_COMPA_vect)
{
  //TCNT1 = t1_load;  // No need to reload here because it auto reloads.
  isr_time = micros();
  isr_flag = true;  // Indicate that new data is available.
}

PID Controller
This is the full implementation of the PID controller. It should give you a solid foundation to develop the PID algorithm for your motor, and to filter the gyro inputs.
//Code for accelerometer controlling an encoded DC
#include <Encoder.h>
#include <Wire.h>
#include <MPU6050.h>
#include <QueueArray.h>

//Class to read the encoder
Encoder myEnc(19, 18);

//I2C address of the MPU-6050 idk if i need
const int MPU_addr = 0x68;

const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 10 Hz              = 25000
//const uint16_t t1_comp = 25000;

// If the PID algorithm takes less than 5 ms, it's possible to run Timer1 at a maximum of about 200 Hz.
// 16 MHz          / 64        / 200 Hz             = 1250
const uint16_t t1_comp = 1250;

// Buffered data for sending/receiving to/from the ISR.
volatile bool          isr_flag = false;            // Used to flag when the ISR has triggered.
volatile unsigned long isr_time = 0;                // Input updated by ISR.
volatile byte          isr_motor_direction = LOW;   // Output calculated and updated in loop().
volatile byte          isr_motor_speed = 0;         // Output calculated and updated in loop().

//initializing the MPU6050 (accel/gyro input)
MPU6050 sensor;

//accerleromter in the y direction
int16_t AcY;
//not using
int16_t AcX, AcZ, GyX, GyY, GyZ, Tmp;

//new varible for mapping the accelerometer to degrees
int yAng;

//Variables to create PID method
int currentTheta;
float error;
int dt = 5;
long integral = 0;
long deriv;
int previousError;

//accel on light, always want it at 0
int setpoint = 0;

//this is the enable pin (PWM) to run speeds to the motor
#define MOTOR_SPEED_PIN         3

//change direction of the motor
// TODO: Check whether LOW and HIGH are correct.
#define MOTOR_DIRECTION_PIN     2
#define MOTOR_DIRECTION_CW      LOW
#define MOTOR_DIRECTION_CCW     HIGH

void setup()
{
  // Disable global interrupt.
  cli();

  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &= ~(1 << WGM13);
  TCCR1B |= (1 << WGM12);

  // Set to prescalar of 64
  TCCR1B &= ~(1 << CS12);
  TCCR1B |= (1 << CS11);
  TCCR1B |= (1 << CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 << OCIE1A);

  // Enable global interrupt
  sei();

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.begin(9600);
  Serial.println("Basic Encoder Test:");

  //pinMode(MOTOR_SPEED_PIN, OUTPUT);  // Not needed for analogWrite().
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);

  sensor.initialize();
}

void loop()
{
  //
  // BEGIN CRITICAL SECTION (Atomic)
  //

  cli();
  bool flag = isr_flag;  // Copy the ISR flag.
  isr_flag = false;      // Reset the ISR flag.
  sei();

  //
  // END CRITICAL SECTION
  //

  //Serial.println(flag);

  if (flag == true)
  {
    DebugMsgTime("PID algorithm begin");

    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Get a copy of the data that has been modified by the ISR.
    cli();
    unsigned long time = isr_time;
    sei();

    //
    // END CRITICAL SECTION
    //

    static unsigned long previous_time = 0;
    //Serial.println(time);
    //Serial.println(time - previous_time);
    previous_time = time;

    //
    // Read gyro Wire.
    //

    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14, true);

    //reading each of the 6 axis's of the MPU6050 and the temp
    AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
    //Only using AcY
    AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
    AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
    Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
    GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    GyZ = Wire.read() << 8 | Wire.read();

    //mapping AcY to yAng from accel units to degrees
    yAng = map(AcY, -17000, 17000, -90, 90);

    //
    // Calculate motor parameters locally.
    //

    long pos = PID(yAng);
    byte motor_direction = pos > 0 ? MOTOR_DIRECTION_CW : MOTOR_DIRECTION_CCW;  // TODO: Check whether defines are correct.
    pos = abs(pos);
    pos = constrain(pos, 0, 100);
    byte motor_speed = map(pos, 0, 100, 0, 200);

    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Update the ISR data ready for output at the next interrupt.
    // This keeps the outputs locked to each other and synchronised to the interrupt.
    cli();
    isr_motor_direction = motor_direction;
    isr_motor_speed = motor_speed;
    sei();

    //
    // END CRITICAL SECTION
    //

    DebugMsgTime("PID algorithm end");
  }

  //delay(1);
}

ISR(TIMER1_COMPA_vect)
{
  // Output.
  digitalWrite(MOTOR_DIRECTION_PIN, isr_motor_direction);
  analogWrite(MOTOR_SPEED_PIN, isr_motor_speed);

  // Input.
  isr_time = micros();
  isr_flag = true;
}

//PID loop to find error between setpoint and currentTheta
long PID(int currentTheta)
{
  error = setpoint - currentTheta;
  integral = integral + error * dt;
  deriv = (error - previousError) / dt;
  long output = 1.9 * error + .8 * integral + 40 * deriv; //2.8,.7,70 //1.3,.8,40
  previousError = error;
  return output;
}

void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = micros();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}

And I think there might be a typo in your PID controller:
long PID(int currentTheta)
{
    ...
    long output = 1.9*error; + .8*integral + 40*deriv;
                           |
                         typo?

This is equivalent to:
    long output = 1.9*error;

Which is not a PID controller.

67602: <p>Running a DC motor(6.5A max input), motor driver (MD10C 7Amp peak output), and arduino Mega.
Basically, I am creating a stabilized object based on the accelerometer's input. It works great but all of a sudden, very randomly, it glitches and gets stuck in whatever mode it was in last, so if it was motor A HIGH and motor B LOW at speed 200, itll just do that and I have to unplug it, put in a blank code, then my code again, then start it, itll work great then do it all over again. The time it takes for it to glitch out is random, I can't seem to find any patterns to it.
here is my code</p>
<pre><code>//Code for accelerometer controlling an encoded DC
#include &lt;Encoder.h&gt;
#include &lt;Wire.h&gt;
#include &lt;MPU6050.h&gt;
#include &lt;QueueArray.h&gt;

//Class to read the encoder 
Encoder myEnc(19,18);

//I2C address of the MPU-6050
const int MPU_addr=0x68;

const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 200 Hz              = 1250
const uint16_t t1_comp = 1250;

// Buffered data for sending/receiving to/from the ISR 
volatile bool isr_flag = false;  //Used to flag when the ISR when triggered 
volatile unsigned long isr_time = 0;  //Input updated by ISR
volatile byte isr_motor_direction; //Output calculated and updated in loop()
volatile byte isr_motor_speed = 0; // Output calculated and updated in loop()

//initializing the MPU6050 (accel/gyro input)
MPU6050 sensor;

//accerleromter in the y direction
int16_t AcY;
//not using
int16_t AcX, AcZ, GyX, GyY, GyZ, Tmp;

//new varible for mapping the accelerometer to degrees
int yAng;

//Variables to create PID method
int currentTheta;
float error;
int dt = 5;
long integral=0;
long deriv;
int previousError;

//accel on light, always want it at 0
int setpoint = 0;

//change speeds of the motor 
#define MOTOR_SPEED_PIN    3
//change direction of the motor 
#define MOTOR_DIRECTION_PIN   2 
#define MOTOR_DIRECTION_CW    LOW
#define MOTOR_DIRECTION_CCW   HIGH






void setup() 
{
  //Disable global interrupt 
  cli();
  
  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &amp;= ~(1 &lt;&lt; WGM13);
  TCCR1B |= (1 &lt;&lt; WGM12);

  
  // Set to prescalar of 64
  TCCR1B &amp;= ~(1 &lt;&lt; CS12);
  TCCR1B |= (1 &lt;&lt; CS11);
  TCCR1B |= (1 &lt;&lt; CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 &lt;&lt; OCIE1A);

  // Enable global interrupt
  sei();

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  
  Serial.begin(9600);

  pinMode(MOTOR_SPEED_PIN, OUTPUT);
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);

  sensor.initialize();
}







void loop()
{  
    //Begin critical section 
    //

    cli();
    bool flag = isr_flag; //Copy the ISR flag
    isr_flag = false; //Reset the ISR flag
    sei();

    //
    //End critical section 

    if (flag == true)
    {
      DebugMsgTime(&quot;PID algorithm begin&quot;);

      //Begin critical section
      //

      //Getting a copy of the data that has been modified by the ISR
      cli();
      unsigned long time = isr_time;
      sei();

      //
      //End critical section 

      static unsigned long previous_time = 0;
      previous_time = time;

      //Wire reading
      
      Wire.beginTransmission(MPU_addr);
      Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
      Wire.endTransmission(false);
      Wire.requestFrom(MPU_addr,14,true);
    
      //reading each of the 6 axis's of the MPU6050 and the temp
      AcX=Wire.read()&lt;&lt;8|Wire.read();  // 0x3B (ACCEL_XOUT_H) &amp; 0x3C (ACCEL_XOUT_L)    
      //Only using AcY
      AcY=Wire.read()&lt;&lt;8|Wire.read();  // 0x3D (ACCEL_YOUT_H) &amp; 0x3E (ACCEL_YOUT_L)
      AcZ=Wire.read()&lt;&lt;8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) &amp; 0x40 (ACCEL_ZOUT_L)
      Tmp=Wire.read()&lt;&lt;8|Wire.read();  // 0x41 (TEMP_OUT_H) &amp; 0x42 (TEMP_OUT_L)
      GyX=Wire.read()&lt;&lt;8|Wire.read();  // 0x43 (GYRO_XOUT_H) &amp; 0x44 (GYRO_XOUT_L)
      GyY=Wire.read()&lt;&lt;8|Wire.read();  // 0x45 (GYRO_YOUT_H) &amp; 0x46 (GYRO_YOUT_L)
      GyZ=Wire.read()&lt;&lt;8|Wire.read();
  
      //mapping AcY to yAng from accel units to degrees
      yAng = map(AcY, -17000, 17000, -90, 90);

      //Calculate motor parameters locally
      long pos = PID(yAng);
      byte motor_direction = pos &gt; 0 ? MOTOR_DIRECTION_CW : MOTOR_DIRECTION_CCW //make sure that this is correct
      pos = abs(pos);
      pos = constrain(pos, 0, 100);
      byte motor_speed = map(pos, 0, 100, 0, 200);

      //Begin Critical Section
      //

      // Update the ISR data ready for output at the next interrupt.
      // This keeps the outputs locked to each other and synchronised to the interrupt.
      cli();
      isr_motor_direction = motor_direction;
      isr_motor_speed = motor_speed;
      sei();

      //
      //End Critical Section

      DebugMsgTime(&quot;PID algorithm end&quot;);
    }  
}







ISR(TIMER1_COMPA_vect)
{
  //Output
  digitalWrite(MOTOR_DIRECTION_PIN, isr_motor_direction);
  analogWrite(MOTOR_SPEED_PIN, isr_motor_speed);

  //Input
  isr_time = micros();
  isr_flag = true;
}







//PID loop to find error between setpoint and currentTheta
long PID(int currentTheta)
{
    error = setpoint - currentTheta;
    integral = integral + error*dt;
    deriv = (error-previousError)/dt;
    long output = 1.9 * error; + .8 * integral + 40 * deriv; //2.8,.7,70 //1.3,.8,40
    previousError = error;
    return output; 
}







void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = micros();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}
</code></pre>
 68310: <p>What happens if the <code>while</code> condition fails? i.e. if <code>millis() - time</code> is greater than or equal to 20. Is this what you intended? If there's too much other stuff in the <code>while</code> loop, it may quit it <strong>and never re-enter it</strong>.</p>
<h1>loop()</h1>
<p>I've added a call to a function that prints debug information.</p>
<pre class="lang-cpp prettyprint-override"><code>unsigned long time = 0;

void loop()
{
  DebugMsgTime(&quot;loop() begin&quot;);

  while (millis() - time &lt; 20)
  {
    DebugMsgTime(&quot;while begin&quot;);

    time = millis();

    //delay(10);  // Short delay OK.
    delay(20);  // Long delay causes while loop to quit.

    DebugMsgTime(&quot;while end&quot;);
  }
  DebugMsgTime(&quot;loop() end&quot;);
}
</code></pre>
<h1>DebugMsgTime()</h1>
<p>This function prints a message along with the current time and the delta time since it was last called.</p>
<pre class="lang-cpp prettyprint-override"><code>void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = millis();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}
</code></pre>
<h1>Debug Output</h1>
<blockquote>
<p>It works great but all of a sudden, very randomly, it glitches and
gets stuck in whatever mode it was in last</p>
</blockquote>
<pre class="lang-none prettyprint-override"><code>Basic Encoder Test:
loop() begin
0
0
while begin  // It works great...
1
1
while end
2
1
while begin
12
10
while end
34
22           // but all of a sudden, very randomly, it glitches...
loop() end   // and gets stuck in whatever mode it was in last
54
20
loop() begin
74
20
loop() end
97
23
loop() begin
118
21
loop() end
</code></pre>
<p>As you can see, using this <code>while</code> loop is a very inaccurate way to generate regular sampling times.</p>
<h1>loop() FIX</h1>
<p>As @FerryBig pointed out in the comments, you need to change the time comparison <code>&lt;</code> at the start of the loop. You also don't need the <code>while</code> loop because <code>loop()</code> is already called cyclically. Instead, put the time comparison in an <code>if</code> statement, something like this which gives you a rough 20 ms:</p>
<pre class="lang-cpp prettyprint-override"><code>void loop()
{
  static unsigned long previous_time = 0;
  unsigned long time = millis();
  unsigned long delta_time = time - previous_time;

  if (delta_time &gt;= 20)
  {
    previous_time = time;

    // Do PID algorithm.
  }
}
</code></pre>
<p>However, for a PID controller you may need more accuracy.</p>
<h1>Timer Interrupt</h1>
<p>A better method would be to use a timer interrupt to call an Interrupt Service Routine (ISR) at regular intervals. This ISR would do two simple things in this order:</p>
<ol>
<li>Output: <code>digitalWrite(motor_direction);</code> and <code>analogWrite(3, mtr2);</code></li>
<li>Input: set a flag that indicates that the gyro Wire needs reading.</li>
</ol>
<p>Then, when the flag is set, the main <code>loop()</code> would read the gyro Wire, and calculate <code>motor_direction</code> and <code>mtr2</code> in readiness for the next interrupt.</p>
<pre class="lang-cpp prettyprint-override"><code>const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 10 Hz              = 25000
const uint16_t t1_comp = 25000;

volatile bool isr_flag = false;       // Used to flag when the ISR has updated the data.
volatile unsigned long isr_time = 0;  // The data.

void setup()
{
  // Disable global interrupt
  cli();

  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &amp;= ~(1 &lt;&lt; WGM13);
  TCCR1B |= (1 &lt;&lt; WGM12);

  // Set to prescalar of 64
  TCCR1B &amp;= ~(1 &lt;&lt; CS12);
  TCCR1B |= (1 &lt;&lt; CS11);
  TCCR1B |= (1 &lt;&lt; CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 &lt;&lt; OCIE1A);

  // Enable global interrupt
  sei();

  Serial.begin(9600);
}

void loop()
{
  //
  // BEGIN CRITICAL SECTION (Atomic)
  //

  cli();
  bool flag = isr_flag;  // Copy the ISR flag.
  isr_flag = false;      // Reset the ISR flag.
  sei();

  //
  // END CRITICAL SECTION
  //

  Serial.println(flag);

  if (flag == true)
  {
    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Get a copy of the data that has been modified by the ISR.
    cli();
    unsigned long time = isr_time;
    sei();

    //
    // END CRITICAL SECTION
    //

    static unsigned long previous_time = 0;
    Serial.println(time - previous_time);
    previous_time = time;
  }

  delay(25);
}

ISR(TIMER1_COMPA_vect)
{
  //TCNT1 = t1_load;  // No need to reload here because it auto reloads.
  isr_time = micros();
  isr_flag = true;  // Indicate that new data is available.
}
</code></pre>
<h1>PID Controller</h1>
<p>This is the full implementation of the PID controller. It should give you a solid foundation to develop the PID algorithm for your motor, and to filter the gyro inputs.</p>
<pre class="lang-cpp prettyprint-override"><code>//Code for accelerometer controlling an encoded DC
#include &lt;Encoder.h&gt;
#include &lt;Wire.h&gt;
#include &lt;MPU6050.h&gt;
#include &lt;QueueArray.h&gt;

//Class to read the encoder
Encoder myEnc(19, 18);

//I2C address of the MPU-6050 idk if i need
const int MPU_addr = 0x68;

const uint16_t t1_load = 0;

// Timer1 compare value:
// Clock frequency / Prescaler / Required frequency = Compare value
// 16 MHz          / 64        / 10 Hz              = 25000
//const uint16_t t1_comp = 25000;

// If the PID algorithm takes less than 5 ms, it's possible to run Timer1 at a maximum of about 200 Hz.
// 16 MHz          / 64        / 200 Hz             = 1250
const uint16_t t1_comp = 1250;

// Buffered data for sending/receiving to/from the ISR.
volatile bool          isr_flag = false;            // Used to flag when the ISR has triggered.
volatile unsigned long isr_time = 0;                // Input updated by ISR.
volatile byte          isr_motor_direction = LOW;   // Output calculated and updated in loop().
volatile byte          isr_motor_speed = 0;         // Output calculated and updated in loop().

//initializing the MPU6050 (accel/gyro input)
MPU6050 sensor;

//accerleromter in the y direction
int16_t AcY;
//not using
int16_t AcX, AcZ, GyX, GyY, GyZ, Tmp;

//new varible for mapping the accelerometer to degrees
int yAng;

//Variables to create PID method
int currentTheta;
float error;
int dt = 5;
long integral = 0;
long deriv;
int previousError;

//accel on light, always want it at 0
int setpoint = 0;

//this is the enable pin (PWM) to run speeds to the motor
#define MOTOR_SPEED_PIN         3

//change direction of the motor
// TODO: Check whether LOW and HIGH are correct.
#define MOTOR_DIRECTION_PIN     2
#define MOTOR_DIRECTION_CW      LOW
#define MOTOR_DIRECTION_CCW     HIGH

void setup()
{
  // Disable global interrupt.
  cli();

  // Reset Timer1 Control Reg A
  TCCR1A = 0;

  // Set CTC mode
  TCCR1B &amp;= ~(1 &lt;&lt; WGM13);
  TCCR1B |= (1 &lt;&lt; WGM12);

  // Set to prescalar of 64
  TCCR1B &amp;= ~(1 &lt;&lt; CS12);
  TCCR1B |= (1 &lt;&lt; CS11);
  TCCR1B |= (1 &lt;&lt; CS10);

  // Reset Timer1 and set compare value
  TCNT1 = t1_load;
  OCR1A = t1_comp;

  // Enable Timer1 compare interrupt
  TIMSK1 = (1 &lt;&lt; OCIE1A);

  // Enable global interrupt
  sei();

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Serial.begin(9600);
  Serial.println(&quot;Basic Encoder Test:&quot;);

  //pinMode(MOTOR_SPEED_PIN, OUTPUT);  // Not needed for analogWrite().
  pinMode(MOTOR_DIRECTION_PIN, OUTPUT);

  sensor.initialize();
}

void loop()
{
  //
  // BEGIN CRITICAL SECTION (Atomic)
  //

  cli();
  bool flag = isr_flag;  // Copy the ISR flag.
  isr_flag = false;      // Reset the ISR flag.
  sei();

  //
  // END CRITICAL SECTION
  //

  //Serial.println(flag);

  if (flag == true)
  {
    DebugMsgTime(&quot;PID algorithm begin&quot;);

    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Get a copy of the data that has been modified by the ISR.
    cli();
    unsigned long time = isr_time;
    sei();

    //
    // END CRITICAL SECTION
    //

    static unsigned long previous_time = 0;
    //Serial.println(time);
    //Serial.println(time - previous_time);
    previous_time = time;

    //
    // Read gyro Wire.
    //

    Wire.beginTransmission(MPU_addr);
    Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
    Wire.endTransmission(false);
    Wire.requestFrom(MPU_addr, 14, true);

    //reading each of the 6 axis's of the MPU6050 and the temp
    AcX = Wire.read() &lt;&lt; 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) &amp; 0x3C (ACCEL_XOUT_L)
    //Only using AcY
    AcY = Wire.read() &lt;&lt; 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) &amp; 0x3E (ACCEL_YOUT_L)
    AcZ = Wire.read() &lt;&lt; 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) &amp; 0x40 (ACCEL_ZOUT_L)
    Tmp = Wire.read() &lt;&lt; 8 | Wire.read(); // 0x41 (TEMP_OUT_H) &amp; 0x42 (TEMP_OUT_L)
    GyX = Wire.read() &lt;&lt; 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) &amp; 0x44 (GYRO_XOUT_L)
    GyY = Wire.read() &lt;&lt; 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) &amp; 0x46 (GYRO_YOUT_L)
    GyZ = Wire.read() &lt;&lt; 8 | Wire.read();

    //mapping AcY to yAng from accel units to degrees
    yAng = map(AcY, -17000, 17000, -90, 90);

    //
    // Calculate motor parameters locally.
    //

    long pos = PID(yAng);
    byte motor_direction = pos &gt; 0 ? MOTOR_DIRECTION_CW : MOTOR_DIRECTION_CCW;  // TODO: Check whether defines are correct.
    pos = abs(pos);
    pos = constrain(pos, 0, 100);
    byte motor_speed = map(pos, 0, 100, 0, 200);

    //
    // BEGIN CRITICAL SECTION (Atomic)
    //

    // Update the ISR data ready for output at the next interrupt.
    // This keeps the outputs locked to each other and synchronised to the interrupt.
    cli();
    isr_motor_direction = motor_direction;
    isr_motor_speed = motor_speed;
    sei();

    //
    // END CRITICAL SECTION
    //

    DebugMsgTime(&quot;PID algorithm end&quot;);
  }

  //delay(1);
}

ISR(TIMER1_COMPA_vect)
{
  // Output.
  digitalWrite(MOTOR_DIRECTION_PIN, isr_motor_direction);
  analogWrite(MOTOR_SPEED_PIN, isr_motor_speed);

  // Input.
  isr_time = micros();
  isr_flag = true;
}

//PID loop to find error between setpoint and currentTheta
long PID(int currentTheta)
{
  error = setpoint - currentTheta;
  integral = integral + error * dt;
  deriv = (error - previousError) / dt;
  long output = 1.9 * error + .8 * integral + 40 * deriv; //2.8,.7,70 //1.3,.8,40
  previousError = error;
  return output;
}

void DebugMsgTime(const char *msg)
{
  static unsigned long previous_time = 0;
  unsigned long time = micros();
  unsigned long delta_time = time - previous_time;
  previous_time = time;

  Serial.println(msg);
  Serial.println(time);
  Serial.println(delta_time);
}
</code></pre>
<p>And I think there might be a typo in your PID controller:</p>
<pre class="lang-cpp prettyprint-override"><code>long PID(int currentTheta)
{
    ...
    long output = 1.9*error; + .8*integral + 40*deriv;
                           |
                         typo?
</code></pre>
<p>This is equivalent to:</p>
<pre class="lang-cpp prettyprint-override"><code>    long output = 1.9*error;
</code></pre>
<p>Which is not a PID controller.</p>
