Self Balancing Stick

// Library Inclusions

#include <I2Cdev.h>                             // IMU IC2 Communication
#include <Wire.h>                               // IMU IC2 Communication
#include <MPU6050_6Axis_MotionApps20_mod_1.h>   // IMU,  changed value in line 305 to 1 to change update speed from every 15ms to every 10ms
#include <PinChangeInt.h>                       // Pin Change Interrupt
#include <digitalWriteFast.h>                   // Fast Digital Readings



// Pin Assignments

#define   pin_left_PMW          9
#define   pin_left_EncoderA     2
#define   pin_left_EncoderB     A2

#define   pin_right_PMW         10
#define   pin_right_EncoderA    3
#define   pin_right_EncoderB    4

#define   pin_IN1               5
#define   pin_IN2               6
#define   pin_IN3               7
#define   pin_IN4               8

#define   pin_left_Current      A0
#define   pin_right_Current     A1

#define   pin_IMU_Interrupt     12
#define   pin_IMU_SDA           A4
#define   pin_IMU_SCL           A5

#define   pin_Pot               A3
#define   pin_Pot_kp            //
#define   pin_Pot_ki            //
#define   pin_Pot_kd            //



// Interrupt Assignment

#define   interruptPin_left_Encoder    0
#define   interruptPin_right_Encoder   1


// User Variable Definitions

#define   encoder_Tick_Resolution   10                    // Defines how many ticks to 'wait' until calculating encoder positions and time measurments. Higher values lead to decreased ISR times, but decreased encoder accuracy.
#define   pmw_Length                256                   // Defines wavelength of PMW signals by modifying divisor, [ 1, 8, 64(default), 256, 1024].  Base Freq.: 31250 Hz. Lower values lead to smoother motors, but worse current readings (or better, not sure).
#define   encoder_Speed_Time_Limit  100                   // Defines how long to wait for a new tick before assuming wheel is stopped [ms]
#define   motor_Current_Sample_Size 400 //50              // Defines the number of current samples to take, should be atleast 255, [20us*400 = 8000us = PMW Length@Divisor=256]
#define   motor_Current_Sample_Delay 0                    // Delay between samples [us]
#define   time_Delay                1                     // Defines amount of time program pauses
#define   voltage_Offset            0                     // Defines amount of volateg added to compensate for motor sticktion, [0 - 255].
#define   twbr_Value                24                    // Defines clocck speed of I2C, higher value leads to slower speeds and more robust comms 'aledgledy', [1-255, 24(default)].
#define   angle_Rounding_Value      1000.                   // Defines what value to multiple by before round theta and omega. Cuts down on noise, [10 = x.1,  100 = x.x1 , 1(Default)]
//Line 305 of MPU6050_6Axis_MotionApps20.h  = 1[10ms]    // Defines frequency of FIFO, higher value leads to less frequent updates, (200Hz /(1+value)), [0x02 (default)].


#define   theta_Filter              0.7
#define   theta_Speed_Filter        0.7
#define   theta_Integral_Max        3.0
#define   left_Speed_Filter         0.8

#define   omega_Filter              0.7
#define   omega_Speed_Filter        0.7
#define   omega_Integral_Max        3.0
#define   right_Speed_Filter        0.8



float     angle_Average_Filter  = 0.970;
float     theta_Zero_Filter     = 0.995;
float     omega_Zero_Filter     = 0.986;
float     angle_Smoothed_Filter = 0.997;
float     friction_Value        = 10;

// Definitions

#define   encoder_PPR               334.                   // Encoder pulse per revolution
#define   serial_Frequency          250000 //230400               // Frequency of serial interface


// Motor Global Variables

volatile unsigned int   left_Rel_Tick_Count = 0;          // Encoder ticks untill resolution
volatile long           left_Abs_Tick_Count = 0;          // Absolute total of encoder ticks
volatile long           left_Abs_Tick_Count_Prev = 0;     // Previous Absolute total of encoder ticks
volatile int            left_Encoder_Direction = 0;       // Encoder measured direction, [-1 or 1]
volatile int            left_Encoder_Direction_Prev = 0;  // Encoder measured direction, [-1 or 1]
volatile int            left_Encoder_Direction_Now = 0;   // Encoder measured direction, [-1 or 1]
volatile int            left_Encoder_Direction_Debug = 0;   // Encoder measured direction, [-1 or 1]
volatile unsigned long  left_Time_Prev = 0;               // Time at previous absolute tick change, [us]
volatile unsigned long  left_Time_Now = 0;                // Time at current absolute tick change, [us]
volatile unsigned long  left_Time_Dif = 0;
volatile unsigned long  left_Time_Acc_Prev = 0;           // Time at previous absolute tick change, [us]
volatile unsigned long  left_Time_Acc_Now = 0;            // Time at current absolute tick change, [us]
         unsigned long  left_Time_Age = 0;                // Time at current absolute tick change, [us]
         unsigned long  left_Time_Saved = 0;              // Time at current absolute tick change, [us]

volatile unsigned int   right_Rel_Tick_Count = 0;          // Encoder ticks untill resolution
volatile long           right_Abs_Tick_Count = 0;          // Absolute total of encoder ticks
volatile int            right_Encoder_Direction = 0;       // Encoder measured direction, [-1 or 1]
volatile int            right_Encoder_Direction_Prev = 0;  // Encoder measured direction, [-1 or 1]
volatile int            right_Encoder_Direction_Now = 0;   // Encoder measured direction, [-1 or 1]
volatile unsigned long  right_Time_Prev = 0;               // Time at previous absolute tick change, [us]
volatile unsigned long  right_Time_Now = 0;                // Time at current absolute tick change, [us]
volatile unsigned long  right_Time_Dif = 0;
volatile unsigned long  right_Time_Acc_Prev = 0;           // Time at previous absolute tick change, [us]
volatile unsigned long  right_Time_Acc_Now = 0;            // Time at current absolute tick change, [us]
         unsigned long  right_Time_Age = 0;                // Time at current absolute tick change, [us]
         unsigned long  right_Time_Saved = 0;              // Time at current absolute tick change, [us]

float   left_Speed_RPM = 0;
float   left_Speed_RPM_Unfiltered = 0;
float   left_Speed_RPM_Prev = 0;
float   left_Speed_RPM_Alt = 0;
float   left_Speed_RPM_Alt_Prev = 0;
float   left_Acceleration = 0;
int     left_Current_Max = 0;
int     left_Current_Avg = 0;
long    left_Current_Total = 0;
int     left_Current;
int     left_Voltage = 0;

float   right_Speed_RPM = 0;
float   right_Speed_RPM_Prev = 0;
float   right_Acceleration = 0;
int     right_Current_Max = 0;
int     right_Current_Avg = 0;
long    right_Current_Total = 0;
int     right_Current;
int     right_Voltage = 0;


float   encoder_Resolution_Size;                          // Encoder Revolution per each encrment in resolution * 100000, calculated in setup




//  Controller Variables

unsigned long   imu_Time_Now = 0;
unsigned long   imu_Time_Prev = 0;


float           left_Speed_Const = 0;                     // Relates Voltage to Speed.  Determines how much extra voltage is added to PID Voltage.
int             left_PID_Voltage = 0;
float           left_PID_Accel = 0;
float           left_P_Accel = 0;
float           left_I_Accel = 0;
float           left_D_Accel = 0;
float           left_S_Accel = 0;

float           right_Speed_Const = 0;                     // Relates Voltage to Speed.  Determines how much extra voltage is added to PID Voltage.
int             right_PID_Voltage = 0;
float           right_PID_Accel = 0;
float           right_P_Accel = 0;
float           right_I_Accel = 0;
float           right_D_Accel = 0;
float           right_S_Accel = 0;

float           theta_Kp = 1200;
float           theta_Ki = 0;
float           theta_Kd = 130;
float           theta_Ks = -25;
float           theta_Kt = 0.6;
float           theta_Ktd = 0;
float           theta_Zero = 0;
float           theta_Zero_Initial = 0;
float           theta_Zero_Prev = 0;
float           theta_Zero_Unfiltered = 0;
float           theta_Error = 0;
float           theta_Error_Prev = 0;
float           theta_Error_Unfiltered = 0;
float           theta_Now = 0;
float           theta_Now_Unfiltered = 0;
float           theta_Prev = 0;
float           theta_Average = 0;
float           theta_Average_Prev = 0;
float           theta_Speed_Now = 0;
float           theta_Speed_Prev = 0;
float           theta_Speed_Unfiltered = 0;
float           theta_Integral = 0;
float           theta_Smoothed = 0;
float           theta_Smoothed_Prev = 0;
float           theta_Smoothed_Speed = 0;


float           omega_Kp = 1600;
float           omega_Ki = 0;
float           omega_Kd = 130;
float           omega_Ks = -25;
float           omega_Kt = 0.6;
float           omega_Ktd = 1.0;
float           omega_Zero = 0;
float           omega_Zero_Initial = 0;
float           omega_Zero_Prev = 0;
float           omega_Zero_Unfiltered = 0;
float           omega_Error = 0;
float           omega_Error_Prev = 0;
float           omega_Now = 0;
float           omega_Now_Unfiltered = 0;
float           omega_Average = 0;
float           omega_Average_Prev = 0;
float           omega_Prev = 0;
float           omega_Speed_Now = 0;
float           omega_Speed_Prev = 0;
float           omega_Speed_Error = 0;
float           omega_Integral = 0;
float           omega_Smoothed = 0;
float           omega_Smoothed_Prev = 0;
float           omega_Smoothed_Speed = 0;


// OTHERS
int     pot_Value = 0;
long    time_Now = 0;
long    time_Prev = 0;
float   hertz = 0;
int     stat = 0;                                       // Brings you to Change theta_zero mode

int     voltage_Max = 0;
String  package;


// DEBUG
volatile long l = 0;
int     m = 0;
long    o = 0;
int     p = 0;
int     p_Prev = 0;

long    time_IMU_Prev = 0;
long    time_IMU_Now = 0;
long    time_IMU_Dif = 0;

long    time_Probe_Prev = 0;
long    time_Probe_Now = 0;
long    time_Probe_Dif = 0;

// TestBed_Accleration
int left_Target_Voltage = 0;
int right_Target_Voltage = 0;



/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////




// IMU Global Variables

bool          blinkState = false;
bool          dmpReady = false;     // set true if DMP init was successful
uint8_t       mpuIntStatus;         // holds actual interrupt status byte from MPU
uint8_t       devStatus;            // return status after each device operation (0 = success, !0 = error)
uint16_t      packetSize;           // expected DMP packet size (default is 42 bytes)
uint16_t      fifoCount;            // count of all bytes currently in FIFO
uint8_t       fifoBuffer[64];       // FIFO storage buffer
float         euler[3];             // [psi, theta, phi]    Euler angle container
float         ypr[3];               // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
MPU6050       mpu;                  // Creating object 'mpu', I think
Quaternion    q;                    // [w, x, y, z]         quaternion container
VectorInt16   aa;                   // [x, y, z]            accel sensor measurements
VectorInt16   aaReal;               // [x, y, z]            gravity-free accel sensor measurements
VectorInt16   aaWorld;              // [x, y, z]            world-frame accel sensor measurements
VectorFloat   gravity;              // [x, y, z]            gravity vector

uint8_t       teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };     // packet structure for InvenSense teapot demo
volatile bool mpuInterrupt = false;                                                                 // indicates whether MPU interrupt pin has gone high

void setup() {

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Serial
  Serial.begin(serial_Frequency);
  //package.reserve(500);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Pin Modes
  pinMode(pin_IMU_Interrupt, INPUT);
  pinMode(pin_left_PMW, OUTPUT);
  pinMode(pin_right_PMW, OUTPUT);
  pinMode(pin_IN1, OUTPUT);
  pinMode(pin_IN2, OUTPUT);
  pinMode(pin_IN3, OUTPUT);
  pinMode(pin_IN4, OUTPUT);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Pull Ups
  digitalWrite(pin_IMU_Interrupt, HIGH);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Interrupts
  PCintPort::attachInterrupt(pin_IMU_Interrupt, dmpDataReady, RISING);
  attachInterrupt(interruptPin_left_Encoder, ISR_left_Encoder, RISING);
  attachInterrupt(interruptPin_right_Encoder, ISR_right_Encoder, RISING);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Setting PMW Wavelength
  byte mode;
  switch (pmw_Length) {
    case 1: mode = 0x01; break;
    case 8: mode = 0x02; break;
    case 64: mode = 0x03; break;
    case 256: mode = 0x04; break;
    case 1024: mode = 0x05; break;
    default:  ;                        // Code that is executed when pmw_Length does not match a case
  }
  TCCR1B = TCCR1B & 0b11111000 | mode;

 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Initializing Other Calculations
  encoder_Resolution_Size = (encoder_Tick_Resolution * 1000000. / encoder_PPR);
  voltage_Max = 255 - voltage_Offset;

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // IMU Setup
  Wire.begin();       // Join I2C bus
  TWBR = twbr_Value;  // 12 = 400kHz I2C clock (200kHz if CPU is 8MHz)
  mpu.initialize();   // Initialize IMU

  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // verify connection
//  Serial.println(F("\nSend any character to begin: "));                           // wait for ready
//  while (Serial.available() && Serial.read());                                    // empty buffer
//  while (!Serial.available());                                                    // wait for data
//  while (Serial.available() && Serial.read());                                    // empty buffer again
  devStatus = mpu.dmpInitialize();                                                // load and configure the DMP

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);                  // 1688 factory default for my test chip

  if (devStatus == 0) {                       // make sure it worked (devStatus returns 0 if so)
    mpu.setDMPEnabled(true);                  // turn on the DMP, now that it's ready
    mpuIntStatus = mpu.getIntStatus();        // enable Arduino interrupt detection (Remove attachInterrupt function here)
    dmpReady = true;                          // set our DMP Ready flag so the main loop() function knows it's okay to use it
    packetSize = mpu.dmpGetFIFOPacketSize();  // get expected DMP packet size for later comparison
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }


  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  Serial.println(F("Setup Complete"));
Serial.println(F("\n\n"));
  while (Serial.available() && Serial.read());                                    // empty buffer
  while (!Serial.available());                                                    // wait for data
  while (Serial.available() && Serial.read());                                    // empty buffer again
//  delay(1000);
mpu.resetFIFO();

}


void setup() {

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Serial
  Serial.begin(serial_Frequency);
  //package.reserve(500);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Pin Modes
  pinMode(pin_IMU_Interrupt, INPUT);
  pinMode(pin_left_PMW, OUTPUT);
  pinMode(pin_right_PMW, OUTPUT);
  pinMode(pin_IN1, OUTPUT);
  pinMode(pin_IN2, OUTPUT);
  pinMode(pin_IN3, OUTPUT);
  pinMode(pin_IN4, OUTPUT);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Pull Ups
  digitalWrite(pin_IMU_Interrupt, HIGH);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Interrupts
  PCintPort::attachInterrupt(pin_IMU_Interrupt, dmpDataReady, RISING);
  attachInterrupt(interruptPin_left_Encoder, ISR_left_Encoder, RISING);
  attachInterrupt(interruptPin_right_Encoder, ISR_right_Encoder, RISING);

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Setting PMW Wavelength
  byte mode;
  switch (pmw_Length) {
    case 1: mode = 0x01; break;
    case 8: mode = 0x02; break;
    case 64: mode = 0x03; break;
    case 256: mode = 0x04; break;
    case 1024: mode = 0x05; break;
    default:  ;                        // Code that is executed when pmw_Length does not match a case
  }
  TCCR1B = TCCR1B & 0b11111000 | mode;

 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // Initializing Other Calculations
  encoder_Resolution_Size = (encoder_Tick_Resolution * 1000000. / encoder_PPR);
  voltage_Max = 255 - voltage_Offset;

  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  // IMU Setup
  Wire.begin();       // Join I2C bus
  TWBR = twbr_Value;  // 12 = 400kHz I2C clock (200kHz if CPU is 8MHz)
  mpu.initialize();   // Initialize IMU

  Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); // verify connection
//  Serial.println(F("\nSend any character to begin: "));                           // wait for ready
//  while (Serial.available() && Serial.read());                                    // empty buffer
//  while (!Serial.available());                                                    // wait for data
//  while (Serial.available() && Serial.read());                                    // empty buffer again
  devStatus = mpu.dmpInitialize();                                                // load and configure the DMP

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788);                  // 1688 factory default for my test chip

  if (devStatus == 0) {                       // make sure it worked (devStatus returns 0 if so)
    mpu.setDMPEnabled(true);                  // turn on the DMP, now that it's ready
    mpuIntStatus = mpu.getIntStatus();        // enable Arduino interrupt detection (Remove attachInterrupt function here)
    dmpReady = true;                          // set our DMP Ready flag so the main loop() function knows it's okay to use it
    packetSize = mpu.dmpGetFIFOPacketSize();  // get expected DMP packet size for later comparison
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }


  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  Serial.println(F("Setup Complete"));
Serial.println(F("\n\n"));
  while (Serial.available() && Serial.read());                                    // empty buffer
  while (!Serial.available());                                                    // wait for data
  while (Serial.available() && Serial.read());                                    // empty buffer again
//  delay(1000);
mpu.resetFIFO();

}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////// IMU ////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



void get_IMU_Values() {

///////////////////////////////////////////////////// IMU Comms ////////////////////////////////////////////////////////////////////////
  if (!dmpReady) {
    Serial.println("Shit failed yo");
    return;                                               // if programming failed, don't try to do anything
  }

  time_IMU_Prev = micros();

  while (!mpuInterrupt) { // && fifoCount < packetSize) {       // wait for MPU interrupt or extra packet(s) available
    // other program behavior stuff here
    // if you are really paranoid you can frequently test in between other
    // stuff to see if mpuInterrupt is true, and if so, "break;" from the
    // while() loop to immediately process the MPU data
    //Serial.println("Waiting in IMU function");

  }

  time_IMU_Dif = micros() - time_IMU_Prev;


  mpuInterrupt = false;                                             // reset interrupt flag
  mpuIntStatus = mpu.getIntStatus();                                // get INT_STATUS byte
  fifoCount = mpu.getFIFOCount();                                   // get current FIFO count

  if ((mpuIntStatus & 0x10) || fifoCount >= 1024) {                 // check for overflow (this should never happen unless our code is too inefficient)
    mpu.resetFIFO();                                                // reset so we can continue cleanly
    Serial.println(F("FIFO overflow!"));
  }


  else if (mpuIntStatus & 0x02) {                                   // otherwise, check for DMP data ready interrupt (this should happen frequently)
    while (fifoCount < packetSize) {
      delay(1);  // wait for correct available data length, should be a VERY short wait
      fifoCount = mpu.getFIFOCount();
      mpuIntStatus = mpu.getIntStatus();
    }
    mpu.getFIFOBytes(fifoBuffer, packetSize);                       // read a packet from FIFO
    fifoCount -= packetSize;                                        // track FIFO count here in case there is > 1 packet available (this lets us immediately read more without waiting for an interrupt)
    mpu.dmpGetQuaternion(&q, fifoBuffer);                           // display YPR angles in degrees
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  }

  mpu.resetFIFO();      // ADDED BY ME!!!!!!!!!!!





  if (p == 1 && p_Prev == 0) {        // Resets integral when balancing mode is started to avoid wide up while holding
    theta_Integral = 0;
    omega_Integral = 0;
    theta_Error = 0;
    omega_Error = 0;
    theta_Smoothed = theta_Now;
    omega_Smoothed = omega_Now;
    theta_Smoothed_Speed = 0;
    omega_Smoothed_Speed - 0;
  }

  p_Prev = p;
  imu_Time_Prev = imu_Time_Now;
  imu_Time_Now = micros();



  ////////////////////////////////////////////////////////// Theta Calcs//////////////////////////////////////////////////////////////////////////////////////////

  theta_Prev = theta_Now;
  theta_Now_Unfiltered = round((ypr[2] * 180 / M_PI) * angle_Rounding_Value) / angle_Rounding_Value;    //undo
  theta_Now = (1 - theta_Filter) * (theta_Now_Unfiltered) + (theta_Filter * theta_Prev); //undo


  theta_Error = theta_Now - theta_Zero;

  theta_Integral += theta_Error * (imu_Time_Now - imu_Time_Prev) / 1000000.0;
  if (theta_Integral > theta_Integral_Max) {
    theta_Integral = theta_Integral_Max;
  } else if (theta_Integral < -theta_Integral_Max) {
    theta_Integral = - theta_Integral_Max;
  }


  theta_Speed_Prev = theta_Speed_Now;
  theta_Speed_Now = ((1 - theta_Speed_Filter) * (theta_Now - theta_Prev) / (imu_Time_Now - imu_Time_Prev) * 1000000.) + theta_Speed_Filter * theta_Speed_Prev; // Relative to absolute zero
  theta_Average_Prev = theta_Average;
  theta_Average = (1 - angle_Average_Filter) * (theta_Now) + (angle_Average_Filter * theta_Average_Prev);

  theta_Smoothed_Prev = theta_Smoothed;
  theta_Smoothed = (1 - angle_Smoothed_Filter) * theta_Now + angle_Smoothed_Filter * theta_Smoothed_Prev;
  theta_Smoothed_Speed = (theta_Smoothed - theta_Smoothed_Prev) / (imu_Time_Now - imu_Time_Prev) * 1000000.;

  theta_Zero_Prev = theta_Zero;
  if ( p == 1) {                                                                                                        // Automatic setpoint adjustment
    theta_Zero_Unfiltered = theta_Zero - theta_Kt * theta_Error - theta_Ktd * theta_Smoothed_Speed;
    theta_Zero = (1 - theta_Zero_Filter) * theta_Zero_Unfiltered + theta_Zero_Filter * theta_Zero_Prev;
  } else {
    theta_Zero = theta_Average;
  }




  //////////////////////////////////////////////////////////////// Omega Calcs//////////////////////////////////////////////////////////////////////////////////////
  omega_Prev = omega_Now;
  omega_Now_Unfiltered = round((-ypr[1] * 180 / M_PI) * angle_Rounding_Value) / angle_Rounding_Value;    //undo
  omega_Now = (1 - omega_Filter) * (omega_Now_Unfiltered) + omega_Filter * (omega_Prev); //undo
  omega_Error = omega_Now - omega_Zero;


  omega_Error = omega_Now - omega_Zero;

  omega_Integral += omega_Error * (imu_Time_Now - imu_Time_Prev) / 1000000.0;
  if (omega_Integral > omega_Integral_Max) {
    omega_Integral = omega_Integral_Max;
  } else if (omega_Integral < -omega_Integral_Max) {
    omega_Integral = - omega_Integral_Max;
  }


  omega_Speed_Prev = omega_Speed_Now;
  omega_Speed_Now = ((1 - omega_Speed_Filter) * (omega_Now - omega_Prev) / (imu_Time_Now - imu_Time_Prev) * 1000000.) + omega_Speed_Filter * omega_Speed_Prev; // Relative to absolute zero
  omega_Average_Prev = omega_Average;
  omega_Average = (1 - angle_Average_Filter) * (omega_Now) + (angle_Average_Filter * omega_Average_Prev);

  omega_Smoothed_Prev = omega_Smoothed;
  omega_Smoothed = (1 - angle_Smoothed_Filter) * omega_Now + angle_Smoothed_Filter * omega_Smoothed_Prev;
  omega_Smoothed_Speed = (omega_Smoothed - omega_Smoothed_Prev) / (imu_Time_Now - imu_Time_Prev) * 1000000.;

  omega_Zero_Prev = omega_Zero;
  if ( p == 1) {                                                                                                     // Automatic setpoint adjustment
    omega_Zero_Unfiltered = omega_Zero - omega_Kt * omega_Error - omega_Ktd * omega_Smoothed_Speed;
    omega_Zero = (1 - omega_Zero_Filter) * omega_Zero_Unfiltered + omega_Zero_Filter * omega_Zero_Prev;
  } else {
    omega_Zero = omega_Average;
  }

}




///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////  SPEED  ////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



void get_left_Encoder_Speeds() {
  left_Speed_RPM_Prev = left_Speed_RPM;
  //left_Speed_RPM_Alt_Prev = left_Speed_RPM_Alt;

  left_Time_Saved = left_Time_Now;
  left_Time_Age = micros();

  left_Encoder_Direction_Debug = left_Encoder_Direction;

  left_Time_Acc_Prev = left_Time_Acc_Now;
  left_Time_Acc_Now = micros();

  if ( (left_Time_Age - left_Time_Saved) > (encoder_Speed_Time_Limit * 1000L)) {                                                 // If last time measurment was taken too long ago, it means wheel has stopped
    left_Speed_RPM = 0;
    left_Speed_RPM_Unfiltered = 0;
   } else {
    left_Speed_RPM_Unfiltered = encoder_Resolution_Size / left_Time_Dif * 60 * left_Encoder_Direction_Debug;    // Speed measurement
      }

  left_Abs_Tick_Count_Prev = left_Abs_Tick_Count;
  left_Speed_RPM = ((1 - left_Speed_Filter) * left_Speed_RPM_Unfiltered) + (left_Speed_Filter * left_Speed_RPM_Prev);
  left_Acceleration = (left_Speed_RPM - left_Speed_RPM_Prev) / (left_Time_Acc_Now - left_Time_Acc_Prev) * 1000000;
}


void get_right_Encoder_Speeds() {
  right_Speed_RPM_Prev = right_Speed_RPM;
  right_Time_Acc_Prev = right_Time_Acc_Now;
  right_Time_Acc_Now = micros();

  right_Time_Saved = right_Time_Now;
  right_Time_Age = micros();


  if ( (right_Time_Age - right_Time_Saved) > (encoder_Speed_Time_Limit * 1000L)) {                                                 // If last time measurment was taken too long ago, it means wheel has stopped
    right_Speed_RPM = 0;
      } else {
    right_Speed_RPM = encoder_Resolution_Size / right_Time_Dif * 60 * right_Encoder_Direction;    // Speed measurement

  }

  right_Speed_RPM = ((1 - right_Speed_Filter) * right_Speed_RPM) + (right_Speed_Filter * right_Speed_RPM_Prev);
  right_Acceleration = (right_Speed_RPM - right_Speed_RPM_Prev) / (right_Time_Acc_Now - right_Time_Acc_Prev) * 1000000;
}




////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////SET VOLTAGE /////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void set_left_Motor_Voltage() {         // Switched directions (0's and 1's) from originial
  if (left_Voltage == 0) {
    analogWrite(pin_left_PMW, 0);
  }
  else {
    if (left_Voltage < 0) {
      digitalWrite(pin_IN1, 1);
      digitalWrite(pin_IN2, 0);
    } else {
      digitalWrite(pin_IN1, 0);
      digitalWrite(pin_IN2, 1);
    }
    analogWrite(pin_left_PMW, abs(left_Voltage) + voltage_Offset);
  }
}


void set_right_Motor_Voltage() {         // Switched directions (0's and 1's) from originial
  if (right_Voltage == 0) {
    analogWrite(pin_right_PMW, 0);
  }
  else {
    if (right_Voltage < 0) {
      digitalWrite(pin_IN3, 1);
      digitalWrite(pin_IN4, 0);
    } else {
      digitalWrite(pin_IN3, 0);
      digitalWrite(pin_IN4, 1);
    }
    analogWrite(pin_right_PMW, abs(right_Voltage) + voltage_Offset);
  }
}



////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////// UPDATE VALUE USING SERIAL CONSOLE //////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

float update_Value(float x) {
  analogWrite(pin_left_PMW, 0);
  analogWrite(pin_right_PMW, 0);
  Serial.print(F("\n\n"));
  Serial.print(F("Current Value:\t"));
  Serial.println(x, 4);
  Serial.println(F("Input New Value:"));
  x = get_Serial();
  Serial.print(F("Value changed to:\t"));
  Serial.println(x, 4);
  delay(100);
  mpu.resetFIFO();
  delay(5);
  stat = 0;
  return x;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////GET SERIAL INPUT FROM CONSOLE//////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


float get_Serial() {

  String inString = "";
  //  Serial.println("Input New Value:");
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available());                 // wait for data


  delay(1);

  while (Serial.available() > 0 ) {
    inString += (char)Serial.read();
  }

  return inString.toFloat();
}

// Interrupts Service Routines for Encoders
// Only trigger when signal A rises.

void ISR_left_Encoder() {

  left_Rel_Tick_Count++;
  if (left_Rel_Tick_Count >= encoder_Tick_Resolution) {

    left_Encoder_Direction_Now = digitalReadFast (pin_left_EncoderB) ? -1 : +1;

    if (left_Encoder_Direction_Now == left_Encoder_Direction_Prev) {                     // Needs to register to ticks in the same directions. Sometimes the encoder will incorretly read wrong direction for one tick.
      left_Encoder_Direction = left_Encoder_Direction_Now;
    }

    left_Encoder_Direction_Prev = left_Encoder_Direction_Now;
    left_Abs_Tick_Count += left_Encoder_Direction;

    left_Time_Prev = left_Time_Now;
    left_Time_Now = micros();
    left_Time_Dif = left_Time_Now - left_Time_Prev;

    left_Rel_Tick_Count = 0;
  }
}


void ISR_right_Encoder() {

  right_Rel_Tick_Count++;
  if (right_Rel_Tick_Count >= encoder_Tick_Resolution) {

    right_Encoder_Direction_Now = digitalReadFast (pin_right_EncoderB) ? -1 : +1;

    if (right_Encoder_Direction_Now == right_Encoder_Direction_Prev) {                // Needs to register to ticks in the same directions. Sometimes the encoder will incorretly read wrong direction for one tick.
      right_Encoder_Direction = right_Encoder_Direction_Now;
    }

    right_Encoder_Direction_Prev = right_Encoder_Direction_Now;
    right_Abs_Tick_Count += right_Encoder_Direction;

    right_Time_Prev = right_Time_Now;
    right_Time_Now = micros();
    right_Time_Dif = right_Time_Now - right_Time_Prev;

    right_Rel_Tick_Count = 0;
  }
}



///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


void dmpDataReady() {
  //l++;
  if (PCintPort::arduinoPin == pin_IMU_Interrupt) {
    mpuInterrupt = true;

  }
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////





void get_left_PID_Voltage_Value() {

  left_P_Accel = theta_Kp * (theta_Now - theta_Zero);
  left_I_Accel = theta_Ki * theta_Integral;
  left_D_Accel = theta_Kd * theta_Speed_Now;
  left_S_Accel = theta_Ks * left_Speed_RPM / 1000.;
  left_PID_Accel = left_P_Accel + left_I_Accel + left_D_Accel + left_S_Accel;

  float friction = 0;
  if (left_Speed_RPM > 0.5) {
    friction = friction_Value;
  } else if (left_Speed_RPM < -0.5) {
    friction = -friction_Value;
  }

  float voltage = (left_PID_Accel + 0.303 * left_Speed_RPM + friction) / 9.4;     // Equation measured from Acceleration Motor Tests

  left_PID_Voltage = round(constrain(voltage, -voltage_Max, voltage_Max));
}




void get_right_PID_Voltage_Value() {

  right_P_Accel = omega_Kp * (omega_Now - omega_Zero);
  right_I_Accel = omega_Ki * omega_Integral;
  right_D_Accel = omega_Kd * omega_Speed_Now;
  right_S_Accel = omega_Ks * right_Speed_RPM / 1000.;
  right_PID_Accel = right_P_Accel + right_I_Accel + right_D_Accel + right_S_Accel;

  float friction = 0;
  if (right_Speed_RPM > 0.5) {
    friction = friction_Value;
  } else if (right_Speed_RPM < -0.5) {
    friction = -friction_Value;
  }

  float voltage = (right_PID_Accel + 0.295 * right_Speed_RPM + friction) / 9.4;      // Equation measured from Acceleration Motor Tests


  right_PID_Voltage = round(constrain(voltage, -voltage_Max, voltage_Max));
}