// ################################################################################
//
//  Balance Bot v0.18 Beta
//
//  Released:  25/08/2018
//
//  Author: TechKnowTone
//
// ################################################################################
/*
    TERMS OF USE: This software is furnished "as is", without technical support, and
    with no warranty, expressed or implied, as to its usefulness for any purpose. In
    no event shall the author or copyright holder be liable for any claim, damages,
    or other liability, whether in an action of contract, tort or otherwise, arising
    from, out of or in connection with the software or the use or other dealings in
    the software.

    Elements of this software were taken from the public domain. It uses
    libraries which must be installed on your system.

    This code is an extended version of that written by Joop Brokking for his
    self-balancing robot. The main differences are as follows:
    
    - The robot will re-enter safemode if received data exceeds 64 to prevent it from 
      responding to wireless data intended for a Critter RC robot
    - The gyros are allowed twice as long to settle on power-up, now 1000 readings
    - Four LEDs are used to display the mode/status of the robot
    - The motor drivers are disabled so save power and make it safer to handle
    - solid green LEDs indicates robot is in 'Safe' mode for handling
    - flashing LEDs indicates a transition from 'Safe' to 'Active' mode
    - solid red LEDs indicates robot is in 'Active' powered balancing mode
    - the red LEDs flash when the battery is < 10.5v prior to sleepinmg
    - The robot must be layed still on its back to take it into active mode
    - Raising the robot to upright will transition it to 'Active' balance mode
      - during initial balance there is no dead band to settle the PID
    - When active the green LEDs blink when a byte is received over Wi-Fi
    - data can be sent over the Wi-Fi for display on the serial monitor if a USB
      lead is connected to the Nunchuk remote control unit.
    - the angle accelerometer is rate limited to reduce step transients effects
    - holding 'C' on remote Nunchuk switches off 'I' and 'D' in PID when in active
      mode. Press again to get 'I' back and again for 'D'.
    - a 'handbrake' feature has been added to prevent movement on slopes
*/
// Declare libraries
#include <Wire.h>                   //Include the Wire.h library so we can communicate with the gyro

// Define constants
#define acc_calibration_value -2414;   // Enter the accelerometer calibration value, default 1000
#define DBLim 5.0                   // deadband limit (5.0)
#define DirLftPin 3                 // left driver Direction pin
#define DirRgtPin 5                 // right driver Direction pin
#define DrvLftEn 6                  // left driver enable signal, active HIGH
#define DrvRgtEn 7                  // right driver enable signal, active HIGH
#define gyro_address 0x68           // MPU-6050 I2C address (0x68 or 0x69)
#define LEDPinL0 12                 // LH Red LED pin
#define LEDPinL1 11                 // LH Green LED pin
#define LEDPinR0 9                  // RH Red LED pin
#define LEDPinR1 10                 // RH Green LED pin
#define mainLoopTime 4000           // main loop timer in microseconds
#define StepLftPin 2                // left driver Step pulse pin
#define StepRgtPin 4                // left driver Step pulse pin

// Declare PID controller coeficients
// As wheels are bigger diameter 69/51 values are reduced proportionately by 0.74
float max_target_speed = 20.0;      //Max target speed (20 - 160)
float pid_p_gain = 8.0;             //Gain setting for the P-controller (15)
float pid_i_gain = 0.5;             //Gain setting for the I-controller (1.18 - 1.5)
float pid_d_gain = 5.0;             //Gain setting for the D-controller (10 - 30)
float pid_out_max = 300.0;          // limit PID output to prevent stepper motors losing synch
float pid_sb_db = 0.0;              //Self-balancing dead band limit
float turning_speed = 16.0;         //Turning speed (16 - 32)

// Declare and initialise global variables
int acc_cal_value = acc_calibration_value;   // Enter the accelerometer calibration value, default 1000
int accel_delta = 0;                // raw accelerometer instantanious increase
int accel_ramp_rate = 30;           // raw accelerometer ramp rate, used to limit stepper impulses at small angles
int accelerometer_data_raw;         // raw accelerometer data for the vertical axis
int accelerometer_data_raw_i;       // instantaneous record for averaging
long acc_data_raw_av;               // cumulative sum for averaging
long acc_data_raw_Cnt;              // counter for averaging
float angle_acc;                    // accelerometer angle average value
float angle_acc_Av;                 // accelerometer angle averaged
float angle_acc_i;                  // accelerometer angle instantaneous value
float angle_acc_last;               // previous averaged accelerometer angle
float angle_acc_sum;                // accelerometer angles summed for averaging
float angle_gyro;                   // compound gyro angle
int AnyInt;                         // any value used for monitoring and debug
unsigned long AnyUS;                // any value used for monitoring and debug
float AnyVal;                       // any value used for monitoring and debug
int battery_voltage;                // a number which represent the average voltage on A0
int battery_voltage_i;              // instantaneous voltage on A0
int battery_voltage_sum = 0;        // sum for average voltage on A0
int C_Button = 0;                   // button pressed state; 0 = UP, 1 = Down
int Gear = 1;                       // Determines drive and turn speed settings 1 - 4
long gyro_pitch_calibration_value;  // gyro drift compensation value
int gyro_pitch_data_raw;            // gyro value read from MPU6050
long gyro_yaw_calibration_value;    // gyro drift compensation value
int gyro_yaw_data_raw;              // gyro value read from MPU6050
long Joy_Y = 0;                     // > 0 when joystick Y is pressed
int LED_case = 0;                   // rnadom LED pointer
int LED_Cnt = 0;                    // LED counter controls light sequence
int LEDFlashCnt = 0;                // LED flash counter
int LEDFlashRate = 20;              // LED flash rate period setting
int LED_Modes[] = {0,0,0,0};        // LED modes from RH green to LH green
int LED_Phase = 0;                  // LED light function phase
long LED_Rnd_Cnt = 0;               // delay between initiating random flashes in safeMode = 0
int LED_Rnd_Del = 0;                // delay between LED random flashes in safeMode = 0
int left_motor;                     // calculated motor period for throttle
unsigned long loop_timer;           // used to control main loop period
byte low_bat = 0;                   // low battery voltage detected if > 0
int moveDir = 0;                    // a flag, stationary = 0; forward = 1; reverse = -1
float mtsInc = 0.005;               // speed adjust increment
int once = 0;                       // print data once flag
int PID_d_En = 1;                   // PID d-gain enable 1=ON, 0-OFF
int PID_En_Cnt = 0;                 // counter used to enter gain disable modes
float pid_error_temp;               // current PID error
int PID_i_En = 1;                   // PID i-gain enable 1=ON, 0-OFF
float pid_i_mem = 0.0;              // used in PID integral calcs
float pid_last_d_error;             // used in PID derivative calcs
float pid_output = 0.0;             // current PDI output
float pid_output_left;              // steering modified PDI output
float pid_output_right;             // steering modified PDI output
int pid_output_trend = 0;           // tracker used in self-balancing
float pid_setpoint;                 // setpoint angle when moving
int PID_Tune = 0;                   // > 0 when in tuning mode
int receive_counter;                // used to extend received data for 100 ms
byte received_byte;                 // received byte from WiFi reciever
int reportStats = 0;                // counter controls status reporting
int reportLine = 0;                 // status reporting print line
int right_motor;                    // calculated motor period for throttle
int safeCnt = 0;                    // time-out counter used in safe mode function
int safeMode = 0;                   // start mode of safety state machine
int safeModeLast = -1;              // previous safe mode
int safePntCnt = 0;                 // print timer counter
float self_balance_pid_inc = 0.0005;    // values used to inc/dec balance setpoint (0.0015)
float self_balance_pid_setpoint = 0.0;  // offset used to set natural balance point
int sleepCnt = 0;                   // sleep trigger counter
byte startEn = 0;                   // > 0 to perform motor calcs and enable output
int Step_Cnt_Left = 0;              // Movement correction counter
int Step_Inc_Left = 0;              // Step counter increment for movement correction
int Step_Cnt_Right = 0;             // Movement correction counter
int Step_Inc_Right = 0;             // Step counter increment for movement correction
int Test = 0;                       // test flag used for any test and debug
int turning = 0;                    // >0 if turning demand received
int throttle_counter_left_motor = 0;   // ISR pulse period counter
int throttle_counter_right_motor = 0;  // ISR pulse period counter
int throttle_left_motor = 0;           // pulse period for left motor
int throttle_left_motor_memory = 0;    // ISR direction flag
int throttle_right_motor = 0;          // pulse period for left motor
int throttle_right_motor_memory = 0;   // ISR direction flag
int Z_Button = 0;                   // button pressed state; 0 = UP, 1 = Down

// --------------------------------------------------------------------------------

void setup() {
  // Setup basic functions
  Serial.begin(9600);       //Start the serial port at 9600 kbps
  Serial.println("\n\n\nBalanceBot v0.16\n");
  Wire.begin();             //Start the I2C bus as master
  TWBR = 12;                //Set the I2C clock speed to 400kHz
  
  // To create a variable pulse for controlling the stepper motors a timer
  // is created that will execute a piece of code (subroutine) every 20us
  // This subroutine is called TIMER2_COMPA_vect
  TCCR2A = 0;               //Make sure that the TCCR2A register is set to zero
  TCCR2B = 0;               //Make sure that the TCCR2A register is set to zero
  TIMSK2 |= (1 << OCIE2A);  //Set the interupt enable bit OCIE2A in the TIMSK2 register
  TCCR2B |= (1 << CS21);    //Set the CS21 bit in the TCCRB register to set the prescaler to 8
  OCR2A = 39;               //The compare register is set to 39 => 20us / (1s / (16.000.000MHz / 8)) - 1
  TCCR2A |= (1 << WGM21);   //Set counter 2 to CTC (clear timer on compare) mode
  
  // enable outputs for the stepper driver ENABLE, STEP and DIRECTION pins
  pinMode(DirLftPin, OUTPUT);        //Configure digital pin for left Direction output
  pinMode(DirRgtPin, OUTPUT);        //Configure digital pin for right Direction output
  pinMode(DrvLftEn, OUTPUT); digitalWrite(DrvLftEn, HIGH);  //Configure digital pin for left Enable output, start disabled
  pinMode(DrvRgtEn, OUTPUT); digitalWrite(DrvRgtEn, HIGH);  //Configure digital pin for right Enable output, start disabled
  pinMode(LEDPinL0, OUTPUT); digitalWrite(LEDPinL0, LOW);   // LH red LED pin OFF
  pinMode(LEDPinL1, OUTPUT); digitalWrite(LEDPinL1, HIGH);  // LH green LED pin ON
  pinMode(LEDPinR0, OUTPUT); digitalWrite(LEDPinR0, LOW);   // RH red LED pin OFF
  pinMode(LEDPinR1, OUTPUT); digitalWrite(LEDPinR1, HIGH);  // RH green LED pin ON
  pinMode(StepLftPin, OUTPUT);       //Configure digital pin for left Step pulse output
  pinMode(StepRgtPin, OUTPUT);       //Configure digital pin for right Step pulse output
  //By default the MPU-6050 sleeps. So we have to wake it up.
  Wire.beginTransmission(gyro_address);   //Start communication with the address found during search.
  Wire.write(0x6B);         //We want to write to the PWR_MGMT_1 register (6B hex)
  Wire.write(0x00);         //Set the register bits as 00000000 to activate the gyro
  Wire.endTransmission();   //End the transmission with the gyro.
  //Set the full scale of the gyro to +/- 250 degrees per second
  Wire.beginTransmission(gyro_address);   //Start communication with the address found during search.
  Wire.write(0x1B);         //We want to write to the GYRO_CONFIG register (1B hex)
  Wire.write(0x00);         //Set the register bits as 00000000 (250dps full scale)
  Wire.endTransmission();   //End the transmission with the gyro
  //Set the full scale of the accelerometer to +/- 4g.
  Wire.beginTransmission(gyro_address);   //Start communication with the address found during search.
  Wire.write(0x1C);         //We want to write to the ACCEL_CONFIG register (1A hex)
  Wire.write(0x08);         //Set the register bits as 00001000 (+/- 4g full scale range)
  Wire.endTransmission();   //End the transmission with the gyro
  //Set some filtering to improve the raw data.
  Wire.beginTransmission(gyro_address);   //Start communication with the address found during search
  Wire.write(0x1A);         //We want to write to the CONFIG register (1A hex)
  Wire.write(0x03);         //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
  Wire.endTransmission();   //End the transmission with the gyro 

  // Take 500 reading from the gyro to allow it to settle.
  // This will taker 1.85 seconds to complete
  LED_Phase = 0; LED_Cnt = 0;
  loop_timer = micros() + 4000;             //set up a 4ms timer
  for(receive_counter = 0; receive_counter < 1000; receive_counter++){ //Create 1000 loops
    Wire.beginTransmission(gyro_address);   //Start communication with the gyro
    Wire.write(0x43);                       //Start reading the Who_am_I register 75h
    Wire.endTransmission();                 //End the transmission
    Wire.requestFrom(gyro_address, 4);      //Request 2 bytes from the gyro
    gyro_yaw_calibration_value += Wire.read()<<8|Wire.read();         //Combine the two bytes to make one integer
    gyro_pitch_calibration_value += Wire.read()<<8|Wire.read();       //Combine the two bytes to make one integer
    // scroll LEDs left to right while doing this
    LED_Cnt++; if (LED_Cnt > 20) {
      LED_Cnt = 0; LED_Phase++; if (LED_Phase > 3) {LED_Phase = 0;}
      switch(LED_Phase) {
        case 0: digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, HIGH);   // LH green LED ON
          digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, LOW); break;   // RH LEDs OFF
        case 1: digitalWrite(LEDPinL0, HIGH); digitalWrite(LEDPinL1, LOW);   // LH red LED ON
          break;  // RH already LEDs OFF
        case 2: digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, LOW);   // LH LEDs OFF
          digitalWrite(LEDPinR0, HIGH); digitalWrite(LEDPinR1, LOW); break;  // RH red LED ON
        case 3: digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, HIGH); break;  // RH green LED ON
      }
    }
    while(loop_timer > micros()) {}
    loop_timer += mainLoopTime;
  }
  gyro_pitch_calibration_value /= 1000;      //Divide the total value by 1000 to get the avarage gyro offset
  gyro_yaw_calibration_value /= 1000;        //Divide the total value by 1000 to get the avarage gyro offset

  // get the raw start angle of the robot
  Wire.beginTransmission(gyro_address);                                     //Start communication with the gyro
  Wire.write(0x3F);                                                         //Start reading at register 3F
  Wire.endTransmission();                                                   //End the transmission
  Wire.requestFrom(gyro_address, 2);                                        //Request 2 bytes from the gyro
  accelerometer_data_raw = Wire.read()<<8|Wire.read();                    //Combine the two bytes to make one integer
  accelerometer_data_raw += acc_cal_value;                                //Add the accelerometer calibration value
  
  // now complete setup process
  Gear = 1; Load_Speeds();
  digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, HIGH);  // LH green LED ON
  digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, HIGH);  // RH green LED ON
  LED_Modes[0] = 1; LED_Modes[1] = 0; LED_Modes[2] = 0; LED_Modes[3] = 1;
  loop_timer = micros() + mainLoopTime;             //Set the loop_timer variable at the next end loop time
}

// --------------------------------------------------------------------------------

void loop() {
  // this main loop runs every 4ms
  if (safeMode < 4) {
     received_byte = 0x00;                    // must be in safeMode >= 4 to received data
  } else {  
    if(Serial.available()){                   //If there is serial data available
      received_byte = Serial.read();          //Load the received serial data in the received_byte variable
      // check for Critter RC data frame
      if (received_byte >= 64) {
        // Critter RC mode data frame detected
        received_byte = 0x00;                 // reject received data
        reset_SafeMode();                     // force safeMode = 0
      } else {
        // Critter RC mode data frame not detected
        receive_counter = 0;                  //Reset the receive_counter variable
        safePntCnt = 30;                      // block data reporting
      }
    }
    if(receive_counter <= 25)receive_counter ++;  //The received byte will be valid for 25 program loops (100 milliseconds)
    else {received_byte = 0x00; PID_En_Cnt = 0;}  //After 100 milliseconds the received byte is deleted
  }
  
  // Load the battery voltage to the battery_voltage variable.
  // There is no reverse protection diode in this robot, so no offset 85 to add in
  // Resistor divider => 2.2k/(2.2k + 3.3k) = 0.4
  // Hence 12.5v equals 5v @ Analog input A0
  // Hence 12.5v equals 1023 analogRead(0)
  // 1250 / 1023 = 1.222
  // The variable battery_voltage holds 1050 if the battery voltage is 10.5V.
  battery_voltage_i = (analogRead(0) * 1.222);
//  if (Test > 0) {battery_voltage_i = 1000;}   // Force low battery condition for testing
  if (battery_voltage_sum < 1) {battery_voltage_sum = 8 * battery_voltage_i;} // force average at start-up
  battery_voltage = battery_voltage_sum / 8;  // average 8 readings
  battery_voltage_sum = battery_voltage_sum + battery_voltage_i - battery_voltage;
  if(battery_voltage < 1050){               // If batteryvoltage is below 10.5V
    if (low_bat < 1) {
      low_bat = 1;                          // Set the low_bat variable to 1
      safeMode = -1;                        // Go to non-responsive state
      Serial.println("Battery LOW - self power-down iminent");
    }
    if(battery_voltage < 950){Sleep();}     // Enter low power lock-out state immediately
  }

  ///////////////////////////////////////////////////////////////////////////////////
  //Angle calculations
  ///////////////////////////////////////////////////////////////////////////////////
  Wire.beginTransmission(gyro_address);                                     //Start communication with the gyro
  Wire.write(0x3F);                                                         //Start reading at register 3F
  Wire.endTransmission();                                                   //End the transmission
  Wire.requestFrom(gyro_address, 2);                                        //Request 2 bytes from the gyro
  accelerometer_data_raw_i = Wire.read()<<8|Wire.read();                    //Combine the two bytes to make one integer
  accelerometer_data_raw_i += acc_cal_value;                                //Add the accelerometer calibration value
  accel_delta = abs(accelerometer_data_raw - accelerometer_data_raw_i);
  // test for rapid overshoot due to stepping around zero angle point
  if (accelerometer_data_raw < accelerometer_data_raw_i) {
    if (accel_delta > accel_ramp_rate) {accelerometer_data_raw += accel_ramp_rate;}
    else {accelerometer_data_raw += accel_delta;}
  }
  else if (accelerometer_data_raw > accelerometer_data_raw_i) {
    if (accel_delta > accel_ramp_rate) {accelerometer_data_raw -= accel_ramp_rate;}
    else {accelerometer_data_raw -= accel_delta;}
  }
  if(accelerometer_data_raw > 8200)accelerometer_data_raw = 8200;           //Prevent division by zero by limiting the acc data to +/-8200;
  if(accelerometer_data_raw < -8200)accelerometer_data_raw = -8200;         //Prevent division by zero by limiting the acc data to +/-8200;

  angle_acc = asin((float)accelerometer_data_raw/8200.0)* 57.296;           //Calculate the current angle according to the accelerometer

  // Go-ACTIVE test
  if (safeMode == 1) {
    // test for being raised to upright start point
    if((startEn == 0) && (angle_acc > -0.5) && (angle_acc < 0.5)){          //If the accelerometer angle is almost 0
      angle_gyro = angle_acc; pid_setpoint = 0.0;                           //Load the accelerometer angle in the angle_gyro variable
      self_balance_pid_setpoint = 0.0;                                      // reset any previous set points
      Step_Cnt_Left = 0; Step_Cnt_Right = 0;                                // count steps from current position
      startEn = 1;                                                          //Set the start variable to start the PID controller
      digitalWrite(DrvLftEn, LOW); digitalWrite(DrvRgtEn, LOW);             // enable motor drivers
      safeMode = 2;                                                         // set safe mode flag to RUNNING
    }
  }
  
  Wire.beginTransmission(gyro_address);                                     //Start communication with the gyro
  Wire.write(0x43);                                                         //Start reading at register 43
  Wire.endTransmission();                                                   //End the transmission
  Wire.requestFrom(gyro_address, 4);                                        //Request 4 bytes from the gyro
  gyro_yaw_data_raw = Wire.read()<<8|Wire.read();                           //Combine the two bytes to make one integer
  gyro_pitch_data_raw = Wire.read()<<8|Wire.read();                         //Combine the two bytes to make one integer
  
  gyro_pitch_data_raw -= gyro_pitch_calibration_value;                      //Add the gyro calibration value
  angle_gyro += gyro_pitch_data_raw * 0.000031;                             //Calculate the traveled during this loop angle and add this to the angle_gyro variable
  
  ///////////////////////////////////////////////////////////////////////////////////
  //MPU-6050 offset compensation
  ///////////////////////////////////////////////////////////////////////////////////
  //Not every gyro is mounted 100% level with the axis of the robot. This can be cause by misalignments during manufacturing of the breakout board. 
  //As a result the robot will not rotate at the exact same spot and start to make larger and larger circles.
  //To compensate for this behavior a VERY SMALL angle compensation is needed when the robot is rotating.
  //Try 0.0000003 or -0.0000003 first to see if there is any improvement.

  gyro_yaw_data_raw -= gyro_yaw_calibration_value;                          //Add the gyro calibration value
  //Uncomment the following line to make the compensation active
  //angle_gyro -= gyro_yaw_data_raw * 0.0000003;                            //Compensate the gyro offset when the robot is rotating

  angle_gyro = (angle_gyro * 0.9996) + (angle_acc * 0.0004);                //Correct the drift of the gyro angle with the accelerometer angle

  ///////////////////////////////////////////////////////////////////////////////////
  //PID controller calculations
  ///////////////////////////////////////////////////////////////////////////////////
  //The balancing robot is angle driven. First the difference between the desired angel (setpoint) and actual angle (process value)
  //is calculated. The self_balance_pid_setpoint variable is automatically changed to make sure that the robot stays balanced all the time.
  //The (pid_setpoint - pid_output * 0.015) part functions as a speed limiting function.
  pid_error_temp = angle_gyro - self_balance_pid_setpoint - pid_setpoint;
  if(pid_output > 10.0 || pid_output < -10.0)pid_error_temp += pid_output * 0.015 ; // brake (0.015)

  if (PID_i_En > 0) {
    pid_i_mem += pid_i_gain * pid_error_temp;                               //Calculate the I-controller value and add it to the pid_i_mem variable
    if(pid_i_mem > pid_out_max) pid_i_mem = pid_out_max;                                //Limit the I-controller to the maximum controller output
    else if(pid_i_mem < -pid_out_max) pid_i_mem = -pid_out_max;
  } else {pid_i_mem = 0;}
  
  //Calculate the PID output value
  pid_output = pid_p_gain * pid_error_temp;                                 // always include p - proportional elemnet
  if (PID_i_En > 0) {pid_output += pid_i_mem;}                              // optional i - integrator element
  if (PID_d_En > 0) {pid_output += pid_d_gain * (pid_error_temp - pid_last_d_error);} // optional d - differentiator element
  if(pid_output > pid_out_max) pid_output = pid_out_max;                                //Limit the PI-controller to the maximum controller output
  else if(pid_output < -pid_out_max) pid_output = -pid_out_max;

  pid_last_d_error = pid_error_temp;                                        //Store the error for the next loop

  // Test for robot falling over  
  if(angle_gyro > 30.0 || angle_gyro < -30.0 || startEn == 0){              // If the robot tips over or the start variable is zero
    digitalWrite(DrvLftEn, HIGH); digitalWrite(DrvRgtEn, HIGH);             // disable motor drivers
    throttle_left_motor = 0; throttle_right_motor = 0;                      // remove throttle drive from interrup routine
    pid_output = 0;                                                         // Set the PID controller output to 0 so the motors stop moving
    pid_i_mem = 0;                                                          // Reset the I-controller memory
    startEn = 0;                                                            // Set the start variable to 0
    self_balance_pid_setpoint = 0;                                          // Reset the self_balance_pid_setpoint variable
    if (safeMode > 1) {reset_SafeMode();}
  }

  ///////////////////////////////////////////////////////////////////////////////////
  // Nunchuk Control calculations
  ///////////////////////////////////////////////////////////////////////////////////
  turning = 0;                                                              // Clear turning demand flag
  pid_output_left = pid_output;                                             //Copy the controller output to the pid_output_left variable for the left motor
  pid_output_right = pid_output;                                            //Copy the controller output to the pid_output_right variable for the right motor

  if(received_byte & B00000001){                                            //If the first bit of the receive byte is set change the left and right variable to turn the robot to the left
    // Joystick pressed left -X
    if (PID_Tune == 0) {
      // in normal control mode
      turning = 1;
      pid_output_left += turning_speed;                                       //Increase the left motor speed
      pid_output_right -= turning_speed;                                      //Decrease the right motor speed
    } else {
      // forced into PID tuning mode with 'C' button
      // JOy-X is used to adjust PID variables p, i & d
      received_byte  = received_byte & B11111110;
      switch(PID_Tune) {
        case 1:
          // PID p tuning mode 1.5 - 15.0
          if (pid_p_gain > 1.5) {pid_p_gain -= 0.1;}
          else {pid_p_gain = 1.5;}
          Serial.println(""); Serial.println(pid_p_gain,1);
          break;
        case 2:
          // PID i tuning mode 0.15 - 1.5
          if (pid_i_gain > 0.15) {pid_i_gain -= 0.01;}
          else {pid_i_gain = 0.15;}
          Serial.println(""); Serial.println(pid_i_gain,2);
          break;
        case 3:
          // PID d tuning mode 3.0 - 30.0
          if (pid_d_gain > 3.0) {pid_d_gain -= 0.1;}
          else {pid_d_gain = 3.0;}
          Serial.println(""); Serial.println(pid_d_gain,2);
          break;
      } safePntCnt = 30;  // temporarily block reporting
    }
  }
  if(received_byte & B00000010){                                            //If the second bit of the receive byte is set change the left and right variable to turn the robot to the right
    // Joystick pressed right +X
    if (PID_Tune == 0) {
      // in normal control mode
      turning = 1;
      pid_output_left -= turning_speed;                                       //Decrease the left motor speed
      pid_output_right += turning_speed;                                      //Increase the right motor speed
    } else {
      // forced into PID tuning mode with 'C' button
      // JOy-X is used to adjust PID variables p, i & d
      received_byte  = received_byte & B11111101;
      switch(PID_Tune) {
        case 1:
          // PID p tuning mode 1.5 - 15.0
          if (pid_p_gain < 15.0) {pid_p_gain += 0.1;}
          else {pid_p_gain = 15.0;}
          Serial.println(""); Serial.println(pid_p_gain,1);
          break;
        case 2:
          // PID i tuning mode 0.15 - 1.5
          if (pid_i_gain < 1.5) {pid_i_gain += 0.01;}
          else {pid_i_gain = 1.5;}
          Serial.println(""); Serial.println(pid_i_gain,2);
          break;
        case 3:
          // PID d tuning mode 3.0 - 30.0
          if (pid_d_gain < 30.0) {pid_d_gain += 0.1;}
          else {pid_d_gain = 30.0;}
          Serial.println(""); Serial.println(pid_d_gain,2);
          break;
      } safePntCnt = 30;  // temporarily block reporting
    }
  }

  if(received_byte & B00000100){                                            //If the third bit of the receive byte is set change the left and right variable to turn the robot to the right
    // Joystick pressed forward +Y
    if (PID_Tune == 0) {
      // in normal control mode
      moveDir = 1;
      if(pid_setpoint > -2.5)pid_setpoint -= 0.05;                            //Slowly change the setpoint angle so the robot starts leaning forewards
      if(pid_output > -max_target_speed)pid_setpoint -= 0.005;            //Slowly change the setpoint angle so the robot starts leaning forewards
    } else {
      // in PID tuning mode
      if (Joy_Y < 50) {
        pid_setpoint -= 0.05;
      } else {if (pid_setpoint < 0.0) {pid_setpoint += 0.05;}}
      Joy_Y++;
    }
  }
  
  if(received_byte & B00001000){                                            //If the forth bit of the receive byte is set change the left and right variable to turn the robot to the right
    // Joystick pressed backward -Y
    if (PID_Tune == 0) {
      // in normal control mode
      moveDir = -1;
      if(pid_setpoint < 2.5)pid_setpoint += 0.05;                             //Slowly change the setpoint angle so the robot starts leaning backwards
      if(pid_output < max_target_speed)pid_setpoint += 0.005;                 //Slowly change the setpoint angle so the robot starts leaning backwards
    } else {
      // in PID tuning mode
      if (Joy_Y < 50) {
        pid_setpoint += 0.05;
      } else {if (pid_setpoint > 0.0) {pid_setpoint -= 0.05;}}
      Joy_Y++;
    }
  }

  // No Y demand
  if(!(received_byte & B00001100)){
    // braking function, if no demand
    // slowly move the PID setpoint angle back to the zero position
    if(pid_setpoint > 0.5)pid_setpoint -=0.05;              //If the PID setpoint is larger than 0.5 reduce the setpoint with 0.05 every loop
    else if(pid_setpoint < -0.5)pid_setpoint +=0.05;        //If the PID setpoint is smaller than -0.5 increase the setpoint with 0.05 every loop
    else {
      pid_setpoint = 0.0;                                   //If the PID setpoint is smaller than 0.5 or larger than -0.5 set the setpoint to 0
      if (moveDir != 0) {
        // previously moving so reset movement counters
        moveDir = 0; Step_Cnt_Left = 0; Step_Cnt_Right = 0;
      }
    }
    Joy_Y = 0;                                              // Restore single-shot function in tuning mode
  }

  // Special 'C' button functions
  // used for PID tuning or special features
  if(received_byte & B00100000){
    // 'C' button pressed
    // check if user wants to inhibit PID gains?
    PID_En_Cnt++;
    if (PID_En_Cnt == 250) {
      // after 1 seconds if PID gains are disabled then re-enable them
      switch (PID_Tune) {
        case 1:
          // was in PID p tune mode 1
          // change to PID i tune mode 2
          PID_i_En = 1; PID_Tune = 2; Set_PID_LEDs ();
          break;
        case 2:
          // was in PID i tune mode 2
          // change to PID d tune mode 3
          PID_d_En = 1; PID_Tune = 3; Set_PID_LEDs ();
          break;
        case 3:
          // was in PID d tune mode 3
          // change to no tune mode 0
          PID_Tune = 0; Set_PID_LEDs ();
          break;
      }
    }
    if (PID_En_Cnt == 750) {
      // 'C' button held for more than 3 seconds so switch OFF PID i and d gains
      // Assume robot is on stand vertical so zero setpoints
      PID_i_En = 0; PID_d_En = 0; PID_Tune = 1; Set_PID_LEDs ();
      pid_setpoint = angle_gyro;  // use current angle as reference setpoint
      self_balance_pid_setpoint = 0.0;  // zero the self balancing offset
      Gear = 1; Load_Speeds();  // Select lowest speeds for tuning
    }
    // set higher speed mode?
    if ((Gear < 4) && (C_Button == 0) && (PID_Tune == 0)) {Gear++; Load_Speeds();}
    // tune values, uncomment to make one active
    C_Button = 1;
  } else {C_Button = 0;}

  // Special 'Z' button functions
  // used for PID tuning or special features
  if(received_byte & B00010000){
    // 'Z' button pressed
    // set low speed mode
    if ((Gear > 1) && (Z_Button == 0)) {Gear--; Load_Speeds();}
    Z_Button = 1;
  } else {Z_Button = 0;}
  
  // The self balancing point is adjusted when there is no forward or backwards movement from the transmitter.
  // This way the robot will always find it's balancing point
  // The trend function prevents the setpoint from being wound up via pushing or slopes
  if((pid_setpoint == 0.0) && (PID_Tune == 0)){                                                    //If the setpoint is zero degrees
    if(pid_output < -pid_sb_db){
      pid_output_trend--; if (pid_output_trend > 0) {pid_output_trend = 0;}
      if (pid_output_trend < -100) {pid_output_trend = -100;}
      else {self_balance_pid_setpoint += self_balance_pid_inc;}  //Increase the self_balance_pid_setpoint if the robot is still moving forewards
    }
    else if(pid_output > pid_sb_db){
      pid_output_trend++; if (pid_output_trend < 0) {pid_output_trend = 0;}
      if (pid_output_trend > 100) {pid_output_trend = 100;}
      else {self_balance_pid_setpoint -= self_balance_pid_inc;}   //Decrease the self_balance_pid_setpoint if the robot is still moving backwards
    } else {pid_output_trend = 0;}
  }

  ///////////////////////////////////////////////////////////////////////////////////
  // New motor pulse calculations
  ///////////////////////////////////////////////////////////////////////////////////
  //To compensate for the non-linear behaviour of the stepper motors the following calculations are needed to get a linear speed behaviour.
  if (startEn > 0) {
    if(pid_output_left > 0.0) {
      if(pid_output_left >= 5.0) {
        pid_output_left = 405.0 - ((1.0/(pid_output_left + 9.0)) * 5500.0);
      } else {
        pid_output_left = pid_output_left * 2.428;
      }
    }
    else if(pid_output_left < 0.0) {
      if(pid_output_left <= -5.0) {
        pid_output_left = -405.0 - ((1.0/(pid_output_left - 9.0)) * 5500.0);
      } else {
        pid_output_left = pid_output_left * 2.428;
      }
    }
  
    if(pid_output_right > 0.0) {
      if(pid_output_right >= 5.0) {
        pid_output_right = 405.0 - ((1.0/(pid_output_right + 9.0)) * 5500.0);
      } else {
        pid_output_right = pid_output_right * 2.428;
      }
    }
    else if(pid_output_right < 0.0) {
      if(pid_output_right <= -5.0) {
        pid_output_right = -405.0 - (1.0/(pid_output_right - 9.0)) * 5500.0;
      } else {
        pid_output_right = pid_output_right * 2.428;
      }
    }
  
    //Calculate the needed pulse time for the left and right stepper motor controllers
    if(pid_output_left > 0.0) {
      if(pid_output_left >= 12.14) {
        left_motor = (int)(400.0 - pid_output_left);
      } else {
        left_motor = (int)(2400.0 - (pid_output_left * 165.8));
      }
    }
    else if(pid_output_left < 0.0) {
      if(pid_output_left <= -12.14) {
        left_motor = (int)(-400.0 - pid_output_left);
      } else {
        left_motor = (int)(-2400.0 - (pid_output_left * 165.8));
      }
    }
    else left_motor = 2400.0; // set long period at stationary point
  
    if(pid_output_right > 0.0) {
      if(pid_output_right >= 12.14) {
        right_motor = (int)(400.0 - pid_output_right);
      } else {
        right_motor = (int)(2400.0 - (pid_output_right * 165.8));
      }
    }
    else if(pid_output_right < 0.0) {
      if(pid_output_right <= -12.14) {
        right_motor = (int)(-400.0 - pid_output_right);
      } else {
        right_motor = (int)(-2400.0 - (pid_output_right * 165.8));
      }
    }
    else right_motor = 2400.0; // set long period at stationary point
  } else {left_motor = 2400.0; right_motor = 2400.0;}

  // Copy the pulse time to the throttle variables so the interrupt subroutine can use them
  throttle_left_motor = left_motor;
  throttle_right_motor = right_motor;


  // additional tasks
  safety_Modes();
  lightLEDs();
//  reportStats--; if (reportStats < 1) Report_Status();  // use for debugging modes

  ///////////////////////////////////////////////////////////////////////////////////
  //Loop time timer
  ///////////////////////////////////////////////////////////////////////////////////
  //The angle calculations are tuned for a loop time of 4 milliseconds. To make sure every loop is exactly 4 milliseconds a wait loop
  //is created by setting the loop_timer variable to +4000 microseconds every loop.
  // Given overhead of floating point maths we check for loop over-run
  AnyUS = 0;
  while(loop_timer > micros()) {AnyUS++;}
  loop_timer += mainLoopTime;
  if (AnyUS < 1) {Serial.print("!");}    // oh dear loop over-run has occured
}

// --------------------------------------------------------------------------------

void lightLEDs() {
  // LEDs are connected back-to-back so need to be driven as byphase devices
  // the main loop running at 250Hz is subdivided to achieve this
  // to control an LED you need to set an array value as follows:
  // 0 = OFF
  // 1 = ON
  // 2 = Flash, at rate = 1/LEDFlashRate
  if (LED_Phase < 1) {
    // control the green LEDs in phase 0
    if (LEDFlashCnt < LEDFlashRate) {LEDFlashCnt++;} else {LEDFlashCnt = 0;}
    switch (LED_Modes[0]) {
      // RH green LED control
      case 0: digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, LOW); break;
      case 1: digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, HIGH); break;
      case 2: if (LEDFlashCnt < 3) {
        digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, HIGH); // turn RH green LED ON briefly
        } else {
          digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, LOW); // turn RH green LED OFF
        } break;
    }
    switch (LED_Modes[3]) {
      // LH green LED control
      case 0: digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, LOW); break;
      case 1: digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, HIGH); break;
      case 2: if (LEDFlashCnt < 3) {
        digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, HIGH); // turn LH green LED ON briefly
        } else {
          digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, LOW); // turn LH green LED OFF
        } break;
    } LED_Phase = 1;
  } else {
    // control the red LEDs in phase 1
    switch (LED_Modes[1]) {
      // RH red LED control
      case 0: digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, LOW); break;
      case 1: digitalWrite(LEDPinL0, HIGH); digitalWrite(LEDPinL1, LOW); break;
      case 2: if (LEDFlashCnt < 3) {
        digitalWrite(LEDPinL0, HIGH); digitalWrite(LEDPinL1, LOW); // turn RH red LED ON briefly
        } else {
          digitalWrite(LEDPinL0, LOW); digitalWrite(LEDPinL1, LOW); // turn RH red LED OFF
        } break;
    }
    switch (LED_Modes[2]) {
      // LH red LED control
      case 0: digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, LOW); break;
      case 1: digitalWrite(LEDPinR0, HIGH); digitalWrite(LEDPinR1, LOW); break;
      case 2: if (LEDFlashCnt < 3) {
        digitalWrite(LEDPinR0, HIGH); digitalWrite(LEDPinR1, LOW); // turn LH red LED ON briefly
        } else {
          digitalWrite(LEDPinR0, LOW); digitalWrite(LEDPinR1, LOW); // turn LH red LED OFF
        } break;
    } LED_Phase = 0;
  }
}

// --------------------------------------------------------------------------------

void Load_Speeds () {
  // loads speed settings depending on 'Gear' value, 1 - 4
  switch (Gear) {
    case 1: max_target_speed = 20.0; mtsInc = 0.002; turning_speed = 16.0; break;
    case 2: max_target_speed = 40.0; mtsInc = 0.003; turning_speed = 22.0; break;
    case 3: max_target_speed = 80.0; mtsInc = 0.004; turning_speed = 28.0; break;
    case 4: max_target_speed = 160.0; mtsInc = 0.005; turning_speed = 32.0; break;
  } Serial.print("\nGear = "); Serial.println(Gear);  // report gear change
  safePntCnt = 25;  // delay data reporting for the above message
}

// --------------------------------------------------------------------------------

void Report_Data() {
  // called from safety modes to report data during PID tuning, etc
  // use sparingly as WiFi link can't support much traffic
  // remove line commment // to enable a specific print function
  safePntCnt--;
  if (safePntCnt < 1) {
    safePntCnt = 25;  // reset print line timer
//    Serial.println(AnyVal);
//    Serial.print("Acc A "); Serial.println(angle_acc,1);
//    Serial.println(angle_acc,1);
//    Serial.print("Batt.Vt = "); Serial.println(battery_voltage);
//    Serial.print(left_motor); Serial.print("  "); Serial.println(right_motor);
//    Serial.print("Gyro A "); Serial.println(angle_gyro);
//    Serial.print("PID.Bal.Off = "); Serial.println(self_balance_pid_inc,4);
//    Serial.print("PID o/p = "); Serial.println(pid_output,2);
//    Serial.print(pid_output,2); Serial.print("   "); Serial.println(pid_output_trend);
//    Serial.print(pid_output_left,2); Serial.print("  "); Serial.println(left_motor);
//    Serial.println(pid_output_left,2);
//    Serial.print("PID.SP = "); Serial.println(pid_setpoint);
    Serial.print("SBSP "); Serial.println(self_balance_pid_setpoint,3);
//    Serial.print(self_balance_pid_setpoint,4); Serial.print("   "); Serial.println(pid_output_trend);
//    Serial.print("Steps = "); Serial.print(Step_Cnt_Left);
//    Serial.print("   "); Serial.println(Step_Cnt_Right);
//    Serial.print("TL = "); Serial.print(left_motor);
//    Serial.print("    TR = "); Serial.println(right_motor);
  }
}

// --------------------------------------------------------------------------------

void Report_Status() {
  // Report values over the serial link
  // Performed as individual print lines to avoid over-running the serial buffer
  // Note all this serial activity may cause main loop over-run, hence use sparingly
  reportStats++;
  if (reportStats > 10) {
    reportStats = 0;
    switch(reportLine) {
      case 0: Serial.print("\n"); break;
      case 1: Serial.print("Batt.Vt = "); Serial.println(battery_voltage); break;
      case 2: Serial.print("Acc.Ang = "); Serial.println(angle_acc); break;
      case 3: Serial.print("PID p = "); Serial.println(pid_p_gain,2); break;
      case 4: Serial.print("PID i = "); Serial.println(pid_i_gain,2); break;
      case 5: Serial.print("PID d = "); Serial.println(pid_d_gain,2); break;
      case 6: Serial.print("PID SB lim = "); Serial.println(pid_sb_db,2); break;
      case 8: Serial.print("SbPIDinc = "); Serial.println(self_balance_pid_inc,4); break;
      default: Report_Data(); // optional data reporting depending on code flags
    } reportLine++;
  }
}

// --------------------------------------------------------------------------------

void reset_SafeMode() {
  //Reset safe mode values
  safeMode = 0; safeCnt = 0; safePntCnt = 0; once = 0;
  Gear = 1; Load_Speeds();                                    // Return to slow speed modes
  reportStats = 0; reportLine = 0;
  acc_cal_value = acc_calibration_value;
  max_target_speed = 20.0; turning_speed = 16.0;              // Return to slow speed modes
}

// --------------------------------------------------------------------------------

void safety_Modes() {
  // this function handles safety from the outset, beginning in mode 0.
  //
  // -9 - short random LED burst whilst in safeMode 0
  // -1 - non-responsive state, battery nearly flat
  //  0 - waiting for BalanceBot to be placed on its back
  //  1 - waiting for BalanceBot to be raised to the upright position
  //  2 - going active, set red LEDs ON
  //  3 - dead band adjustment
  //  4 - fully active, BalanceBot can receive Nunchuk data
  //
  if (safeMode != safeModeLast) {Serial.print("\nSafeMode = "); Serial.println(safeMode);}
  safeModeLast = safeMode;
  switch(safeMode) {
    case 4:
      // robot is active, so flash green LEDs if data received
      if (receive_counter <= 4) {
        LED_Modes[0] = 1; LED_Modes[3] = 1; // green LEDs flash if serial data received
      } else {LED_Modes[0] = 0; LED_Modes[3] = 0;}
      // send various values over Wi-Fi for debugging
      Report_Data();
      break;
    case 3:
      // deaband adjustment for 4 seconds whilst getting average acceleration
      if (safeCnt > 250) {
        // ignore the first 250 readings
        acc_data_raw_av += accelerometer_data_raw_i; acc_data_raw_Cnt++;
        safeMode = 4;
      }
      safeCnt++;
      if (safeCnt > 1000) {
        safeMode = 4; safePntCnt = 0;
      }
      Report_Data();
      break;
    case 2:
      // robot is going active
      Set_PID_LEDs (); // red LEDs are on permanently dependant on PID enables
      safeMode = 3; safeCnt = 0;
      acc_data_raw_av = 0; acc_data_raw_Cnt = 0; break;
    case 1:
      // robot is responding to tilting towards balance point with LEDs
      LEDFlashRate = map(abs(angle_acc), 90.0, 0.0,50,8);
      if (LEDFlashRate > 30) {
        LED_Modes[0] = 2; LED_Modes[1] = 0; LED_Modes[2] = 0; LED_Modes[3] = 2;
      } else {LED_Modes[0] = 0; LED_Modes[1] = 2; LED_Modes[2] = 2; LED_Modes[3] = 0;}
      // send angle over Wi-Fi for debugging
      Report_Data();
      break;
    case 0:
      // Safe mode - drive systems disabled, LEDs on green
      // robot must be horizontal and steady for 1 seconds
      LED_Modes[0] = 1; LED_Modes[1] = 0; LED_Modes[2] = 0; LED_Modes[3] = 1;
      // send messages over WiFi or via USB port
      Report_Status();
      // check for on its stand, if so flash LEDs occasionally
      if (abs(angle_acc) < 10.0) {
        LED_Rnd_Cnt++;
        if (LED_Rnd_Cnt > 2500) {LED_Rnd_Cnt = 20; LED_Rnd_Del = 0; safeMode = -9;}
      } else {LED_Rnd_Cnt = 0;}
      // check for robot lying on its back
      if (angle_acc >= 75.0) {
        if (abs(angle_acc - angle_acc_last) < 3.0) safeCnt++;
        angle_acc_last = angle_acc;
      } else {safeCnt = 0;}
      if (safeCnt >= 750) {
        // horizontal for 3 seconds
        LEDFlashRate = 50; safeMode = 1;
        LED_Modes[0] = 2; LED_Modes[1] = 0; LED_Modes[2] = 0; LED_Modes[3] = 2;
      } break;
    case -1:
      // non-responsive state, but drive motors may still be active
      // after 10 seconds the robot will sleep, falling backwards if upright
      LEDFlashRate = 100;     // set low battery slow flash rate
      LED_Modes[0] = 0; LED_Modes[1] = 2; LED_Modes[2] = 2; LED_Modes[3] = 0;
      safePntCnt--; if (safePntCnt < 1) {
        // send messages over WiFi or via USB port
        safePntCnt = 20;  // reset print line timer
      } sleepCnt++;
      if (sleepCnt > 2500) {
        if (angle_acc > (angle_acc_last + 2.0)) Sleep();   // force sleep after 10 seconds, falling backwards
      }
      angle_acc_last = angle_acc; break;
    case -9:
      // random LED lights
      LED_Rnd_Del--;
      if (LED_Rnd_Del < 1) {
        LED_Rnd_Del = 20; LED_case = random(8);
        switch (LED_case) {
          case 0: LED_Modes[0] = 1; LED_Modes[1] = 0; LED_Modes[2] = 0; LED_Modes[3] = 0; break;
          case 1: LED_Modes[0] = 0; LED_Modes[1] = 1; LED_Modes[2] = 0; LED_Modes[3] = 0; break;
          case 2: LED_Modes[0] = 0; LED_Modes[1] = 0; LED_Modes[2] = 1; LED_Modes[3] = 0; break;
          case 3: LED_Modes[0] = 0; LED_Modes[1] = 0; LED_Modes[2] = 0; LED_Modes[3] = 1; break;
          case 4: LED_Modes[0] = 1; LED_Modes[1] = 0; LED_Modes[2] = 1; LED_Modes[3] = 0; break;
          case 5: LED_Modes[0] = 0; LED_Modes[1] = 1; LED_Modes[2] = 0; LED_Modes[3] = 1; break;
          case 6: LED_Modes[0] = 1; LED_Modes[1] = 0; LED_Modes[2] = 0; LED_Modes[3] = 1; break;
          case 7: LED_Modes[0] = 0; LED_Modes[1] = 1; LED_Modes[2] = 1; LED_Modes[3] = 0; break;
        }
          LED_Rnd_Cnt--; if (LED_Rnd_Cnt < 1) {
          safeMode = 0; LED_Rnd_Cnt = 0;
          LED_Modes[0] = 1; LED_Modes[1] = 0; LED_Modes[2] = 0; LED_Modes[3] = 1;
          Serial.print("Acc.Cal.= "); Serial.println(accelerometer_data_raw_i - acc_cal_value);
          Serial.print("Batt.Vt = "); Serial.println(battery_voltage);
        }
      } break;
  }
}

// --------------------------------------------------------------------------------

void Set_PID_LEDs () {
  // sets the red LEDs depending on PID enable flags
  switch (PID_Tune) {
    case 0:
      LED_Modes[1] = 1; LED_Modes[2] = 1;  // both red LEDs ON
      break;
    case 1:
      LED_Modes[1] = 0; LED_Modes[2] = 2;  // right LED OFF, left LED flash
      break;
    case 2:
      LED_Modes[1] = 2; LED_Modes[2] = 0;  // right LED flash, left LED OFF
      break;
    case 3:
      LED_Modes[1] = 2; LED_Modes[2] = 2;  // both red LED flash
      break;
  }
}

// --------------------------------------------------------------------------------

void Sleep() {
  // called if battery voltage is too low, so enter into a low power state
//  digitalWrite(13, LOW);        // Turn LED13 OFF
  digitalWrite(DrvLftEn, HIGH);  // disable left drive
  digitalWrite(DrvRgtEn, HIGH);  // disable right drive
  digitalWrite(LEDPinL0, LOW);   // LH red LED pin OFF
  digitalWrite(LEDPinL1, LOW);   // LH green LED pin OFF
  digitalWrite(LEDPinR0, LOW);   // RH red LED pin OFF
  digitalWrite(LEDPinR1, LOW);   // RH green LED pin OFF
  Serial.println("\n\n\nBye!\n\n"); delay(1000);
  while(true) {}  // code lock-out
}

// --------------------------------------------------------------------------------

///////////////////////////////////////////////////////////////////////////////////
//Interrupt routine  TIMER2_COMPA_vect
///////////////////////////////////////////////////////////////////////////////////
ISR(TIMER2_COMPA_vect){
  //Left motor pulse calculations
  throttle_counter_left_motor ++;                                //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
  if(throttle_counter_left_motor > throttle_left_motor_memory){  //If the number of loops is larger then the throttle_left_motor_memory variable
    throttle_counter_left_motor = 0;                             //Reset the throttle_counter_left_motor variable
    throttle_left_motor_memory = throttle_left_motor;            //Load the next throttle_left_motor variable
    if(throttle_left_motor_memory < 0){                          //If the throttle_left_motor_memory is negative
      PORTD &= 0b11110111;                                       //Set output 3 low to reverse the direction of the stepper controller
      throttle_left_motor_memory *= -1;                          //Invert the throttle_left_motor_memory variable
      Step_Inc_Left = -1;                                        //Set the step count increment
    }
    else {
      PORTD |= 0b00001000;                                       //Set output 3 high for a forward direction of the stepper motor
      Step_Inc_Left = 1;                                         //Set the step count increment
    }
  }
  else if(throttle_counter_left_motor == 1)PORTD |= 0b00000100;  //Set output 2 high to create a pulse for the stepper controller
  else if(throttle_counter_left_motor == 2){
    PORTD &= 0b11111011;                                         //Set output 2 low because the pulse only has to last for 20us 
    Step_Cnt_Left += Step_Inc_Left;                              //Count steps for movement correction
  }
  
  //right motor pulse calculations
  throttle_counter_right_motor ++;                               //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
  if(throttle_counter_right_motor > throttle_right_motor_memory){//If the number of loops is larger then the throttle_right_motor_memory variable
    throttle_counter_right_motor = 0;                            //Reset the throttle_counter_right_motor variable
    throttle_right_motor_memory = throttle_right_motor;          //Load the next throttle_right_motor variable
    if(throttle_right_motor_memory < 0){                         //If the throttle_right_motor_memory is negative
      PORTD |= 0b00100000;                                       //Set output 5 low to reverse the direction of the stepper controller
      throttle_right_motor_memory *= -1;                         //Invert the throttle_right_motor_memory variable
      Step_Inc_Right = -1;                                       //Set the step count increment
    }
    else {
      PORTD &= 0b11011111;                                       //Set output 5 high for a forward direction of the stepper motor
      Step_Inc_Right = 1;                                         //Set the step count increment
    }
  }
  else if(throttle_counter_right_motor == 1)PORTD |= 0b00010000; //Set output 4 high to create a pulse for the stepper controller
  else if(throttle_counter_right_motor == 2){
    PORTD &= 0b11101111;                                         //Set output 4 low because the pulse only has to last for 20us
    Step_Cnt_Right += Step_Inc_Right;                            //Count steps for movement correction
  }
}

