// PID controller functions

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

void AccRampInc(int16_t zA) {
  // increment/decrement the associated AccRamp value
  int16_t zAny = 0;
  switch(zA) {
    case 0: zAny = AccRampX; break;
    case 1: zAny = AccRampY; break;
    case 2: zAny = AccRampZ; break;
  }
  if (DmB != 0) {if (zAny > 0) {zAny--;}}
  else {if (zAny < 99) {zAny ++;}}
  switch(zA) {
    case 0: AccRampX = zAny; break;
    case 1: AccRampY = zAny; break;
    case 2: AccRampZ = zAny; break;
  }
}

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

void AvFtrInc(int16_t zV) {
  // increment/decrement the Averaging filter value
  if (DmB == 0) {AvCnt += zV;} else {AvCnt -= zV;}
       if (AvCnt > 99) {AvCnt = 99;}
  else if (AvCnt < 1) {AvCnt = 1;}
}

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

void BrakeFrcInc(float zV) {
  // increment/decrement the brake velocity trigger value
  if (DmB == 0) {Brake_Force += zV;} else {Brake_Force -= zV;}
       if (Brake_Force > 9.999) {Brake_Force = 9.999;}
  else if (Brake_Force < 0.000) {Brake_Force = 0.000;}
}

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

void BrakeVelInc(float zV) {
  // increment/decrement the brake velocity trigger value
  if (DmB == 0) {Brake_Vel += zV;} else {Brake_Vel -= zV;}
       if (Brake_Vel > 9.99) {Brake_Vel = 9.99;}
  else if (Brake_Vel < 0.00) {Brake_Vel = 0.00;}
}

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

void changeGear(int16_t zG) {
  // set the speed flag depending on zG selection
  Gear = zG;
  switch (Gear) {
    case 1: maxTgtSpd = maxTgtSpd1; TgtAng = TgtAng1; break;
    case 2: maxTgtSpd = maxTgtSpd2; TgtAng = TgtAng2; break;
    case 3: maxTgtSpd = maxTgtSpd3; TgtAng = TgtAng3; break;
    default:
      Gear = 1; maxTgtSpd = maxTgtSpd1; TgtAng = TgtAng1;break;
  }
}

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

void commInc(float zV) {
  // increment the motor commutation acceleration by zV
  if (motorPnt < 0) {return;}

  switch (motorPnt) {
    case 0:
      if (DmB == 0) {motorA_Vd += zV;} else {motorA_Vd -= zV;}
      if (motorA_Vd > 9.99) {motorA_Vd = 9.99;}
      else if (motorA_Vd < 0.0) {motorA_Vd = 0.0;}
      break;
    case 1:
      if (DmB == 0) {motorB_Vd += zV;} else {motorB_Vd -= zV;}
      if (motorB_Vd > 9.99) {motorB_Vd = 9.99;}
      else if (motorB_Vd < 0.0) {motorB_Vd = 0.0;}
      break;
    case 2:
      if (DmB == 0) {motorC_Vd += zV;} else {motorC_Vd -= zV;}
      if (motorC_Vd > 9.99) {motorC_Vd = 9.99;}
      else if (motorC_Vd < 0.0) {motorC_Vd = 0.0;}
      break;
  }
}

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

void commType() {
  // user has clicked on Mode in the Commutate display
  if (ComType == 0) {ComType = 1;} else {ComType = 0;}
  TestDel = 0; TestMode = 11; // restart comm Test mode
}

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

void Control() {
  // This function responds to Nunchuk demands when in self-balancing demo mode.
  // When on USB power, it does not respond to demands.
  if (fabs(YawGyrAng - YawGyrAngLast) < 0.01) {
    // run a timer on the angle being stationary
    YawTimeout++; if (YawTimeout > 200) {YawGyrAng = 0.000; Turning = false;}
  } else {YawTimeout = 0; Turning = true;}
  YawGyrAngLast = YawGyrAng;
  if (USB) {return;}  // drive signals are not applied

  if (PtchGo && RollGo && WiFiEn) {
    // Uni-Bot has entered Safe Mode 4 self-balancing state, and is connected to Wii over Wi-Fi
    //############################################################################
    // Steering - Demo mode
    //############################################################################
    // Since acceleration can not be sustained, this has to be applied using a
    // state machine sawtooth process.
    switch (TurnTask) {
      case 0: // get motor B ready, on low power
        motorB_Vel = 0.0; motorBVol = 1.0; motorB.voltage_limit = getVolNow(motorBVol);
        OpB_En = true; digitalWrite(MotorBEn,HIGH); TurnTask = 1; Turn = 0;
        break;
      case 1: // responding to JoyX demand
        if (JoyX >= 192) {
          // turn to the right, so motor spins anti-clockwise
          if (Turn < 1) {YawGyrAng = 0.000; Turn = 1; PwrPlus = true;}                    // increase drive wheel power momentarily for stability
          else {Turn++;}  // when turning right, Turn >= 1
          // reduce turn force when moving forward/backwards
          if (moveDir == 0) {motorB_Vd = TurnMaxVd; motorB_Vel -= motorB_Vd;}             // if not driving, use max turn force
          else {motorB_Vd = TurnMaxVd/4.5; motorB_Vel -= motorB_Vd;}                      // if driving, use reduced turn force
          if (motorBVol != 4.0) {motorBVol = 4.0; motorB.voltage_limit = getVolNow(motorBVol);}      // put PWM voltage on 4.0v
          if (!OpB_En) {OpB_En = true; digitalWrite(MotorBEn,HIGH);}                      // ensure SimpleFOC mini is enabled
          if (motorB_Vel < -motorB_VelMax) {motorB_Vel = -motorB_VelMax; TurnTask = 2;}   // exit on max speed
          else if (YawGyrAng >= 85.0) {LED_Flash = 20; TurnTask = 2;}                                     // exit on max angle
        } else if (JoyX <= 64) {
          // turn to the left, so motor spins clockwise
          if (Turn > -1) {YawGyrAng = 0.000; Turn = -1; PwrPlus = true;}                  // increase drive wheel power momentarily for stability
          else {Turn--;}  // when turning left, Turn <= -1
          // reduce turn force when moving forward/backwards
          if (moveDir == 0) {motorB_Vd = TurnMaxVd; motorB_Vel += motorB_Vd;}             // if not driving, use max turn force
          else {motorB_Vd = TurnMaxVd/4.5; motorB_Vel += motorB_Vd;}                      // if driving, use reduced turn force
          if (motorBVol != 4.0) {motorBVol = 4.0; motorB.voltage_limit = getVolNow(motorBVol);}      // put PWM voltage on 4.0v
          if (!OpB_En) {OpB_En = true; digitalWrite(MotorBEn,HIGH);}                      // ensure SimpleFOC mini is enabled
          if (motorB_Vel > motorB_VelMax) {motorB_Vel = motorB_VelMax; TurnTask = 2;}     // exit on max speed
          else if (YawGyrAng <= -85.0) {LED_Flash = 20; TurnTask = 2;}                                    // exit on max angle
        } else {
          // no JoyX demand, so slow down if it was turning
          if (Turn != 0) {TurnTask = 2;}
        } break;
      case 2: // slowing down to a stop
             if (motorB_Vel >=  TurnMinVd) {
               // motor was spinning clockwise, so Uni-Bot was turning left
               if (moveDir == 0) {motorB_Vel -= TurnMinVd;} else {motorB_Vel -= TurnMinVd/2.0;}
               if (motorB_Vel <=  TurnMinVd) {TurnTask = 0;}
               if (JoyX >= 192) {TurnTask = 1; Turn = 0;}  // user is reversing the steering during slow down
             }
        else if (motorB_Vel <= -TurnMinVd) {
          // motor was spinning anti-clockwise, so Uni-Bot was turning right
          if (moveDir == 0) {motorB_Vel += TurnMinVd;} else {motorB_Vel += TurnMinVd/2.0;}
          if (motorB_Vel >= -TurnMinVd) {TurnTask = 0;}
          if (JoyX <= 64) {TurnTask = 1; Turn = 0;}  // user is reversing the steering during slow down
        } else {TurnTask = 0;}
        if (motorBVol != 3.0) {motorBVol = 3.0; motorB.voltage_limit = getVolNow(motorBVol);}
        // if demand was released then re-applied
          // user has sustained or re-applied demand during slow down, re-trigger below half speed
        if ((JoyX <= 64) || (JoyX >= 192)) {
          if (fabs(motorB_Vel) <= (motorB_VelMax/2.0)) {TurnTask = 1; Turn = 0;}
        } break;
      default:
        TurnTask = 0; break;
    } motorB.move(motorB_Vel);
    // if (Report20ms >= 5) {Report20ms = 0; PrintTx+= String(motorB_Vel,2) + "\n";} // report this every 100ms
  } else if ((DispMode == DM_TurnCtrl) && (MainMode == 1)) {
    //############################################################################
    // Steering - Test mode
    //############################################################################
    // we are looking at the Turn Control display in Test mode, so we simply turn the
    // B-wheel, depending on the JoyX value, to indicate its direction of travel
    if (RxClass || RxNunch) {
      if (JoyX >= 132) {
        motorB_Vel = mapF((float)JoyX,132.0,255.0,0.0,-9.0);
        motorB.move(motorB_Vel); motorB.voltage_limit = getVolNow(3.0);
        OpB_En = true; digitalWrite(MotorBEn,HIGH);
      } else if (JoyX <= 124) {
        motorB_Vel = mapF((float)JoyX,0.0,124.0,9.0,0.0);
        motorB.move(motorB_Vel); motorB.voltage_limit = getVolNow(3.0);
        OpB_En = true; digitalWrite(MotorBEn,HIGH);
      } else {
        motorB_Vel = 0.0; motorB.move(motorB_Vel); motorB.voltage_limit = 0.0;
        OpB_En = false; digitalWrite(MotorBEn,LOW);
      }
    }
  } else if (OpB_En) {
    // ensure turn motor is off if not required
    motorB_Vel = 0.0; motorB.move(motorB_Vel);
    motorBVol = 0.0; motorB.voltage_limit = motorBVol;
    OpB_En = false; digitalWrite(MotorBEn,LOW); TurnTask = 0; Turn = 0;
  }

  if (PtchGo && RollGo && WiFiEn) {
    // Uni-Bot has entered Safe Mode 4 self-balancing state, so apply
    //############################################################################
    // Forward/reverse drive - Demo mode
    //############################################################################
    // The demand from the Nunchuk Y joystick controls the robots behavior.
    // The changes to the setpoint are ramped up and down to avoid sudden changes.
    // The Gear selection using the 'C' and 'Z' will have determined maxTgtSpd
    if (JoyY >= 192) {
      // drive forward
      if (moveDir != 1) {moveDir = 1; PwrPlus = true;}                  // set direction and increase motor PWM voltage
      if (!TgtSet) {
        TgtSet = true;                                                  // Preset the PtchSp target achieved flag
        if (PtchSp > -TgtAng) {PtchSp -= 0.01; TgtSet = false;}         // Slowly change the setpoint angle so the robot starts leaning forewards
        if (motorC_Vel <  maxTgtSpd) {PtchSp -= 0.005; TgtSet = false;} // Slowly change the setpoint angle to increase forward speed
      } else {
             if (motorC_Vel <  maxTgtSpd) {PtchSp -= 0.0005;}           // Slowly change the setpoint angle to increase forward speed
        else if (motorC_Vel >  maxTgtSpd) {PtchSp += 0.0005;}           // Slowly change the setpoint angle to decrease forward speed
      }
    } else if (JoyY <= 64) {
      // drive in reverse
      if (moveDir != -1) {moveDir = -1; PwrPlus = true;}                // set direction and increase motor PWM voltage
      if (!TgtSet) {
        TgtSet = true;                                                  // Preset the PtchSp target achieved flag
        if (PtchSp < TgtAng) {PtchSp += 0.01; TgtSet = false;}          // Slowly change the setpoint angle so the robot starts leaning backwards
        if (motorC_Vel > -maxTgtSpd) {PtchSp += 0.005; TgtSet = false;} // Slowly change the setpoint angle to increase reverse speed
      } else {
            if (motorC_Vel > -maxTgtSpd) {PtchSp += 0.0005;}            // Slowly change the setpoint angle to increase reverse speed
        else if (motorC_Vel < -maxTgtSpd) {PtchSp -= 0.0005;}           // Slowly change the setpoint angle to decrease reverse speed
      }
    } else {
      // no demand, so slow down if moving
      if (moveDir != 0) {
             if (PtchSp > 0.05) {PtchSp -= 0.05;}             // If the PID setpoint is larger than 0.5 reduce the setpoint with 0.05 every loop
        else if (PtchSp < -0.05) {PtchSp += 0.05;}            // If the PID setpoint is smaller than -0.5 increase the setpoint with 0.05 every loop
        else {
          moveDir = 0;                                        // reset the direction
          SbPtchSpInc = SbPtchSpIncI/2.0;                     // reset auto self-balance function to 1/2 initial value
        }
      } else {
        PtchSp = 0.00;                                        // reset the  setpoint
        TgtSet = false;                                       // reset the PtchSp achieved flag
      }
    }
  }
}

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

void FilterAInc(float zV) {
  // increment/decrement motorA lowpass filter coefficient
  if (DmB == 0) {LpfA += zV;}
  else {LpfA -= zV;}
       if (LpfA > 1.000) {LpfA = 1.000;}
  else if (LpfA < 0.01) {LpfA = 0.01;}
}

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

void FilterBInc(float zV) {
  // increment/decrement motorB lowpass filter coefficient
  if (DmB == 0) {LpfB += zV;}
  else {LpfB -= zV;}
       if (LpfB > 1.000) {LpfB = 1.000;}
  else if (LpfB < 0.01) {LpfB = 0.01;}
}

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

void FilterCInc(float zV) {
  // increment/decrement motorC lowpass filter coefficient
  if (DmB == 0) {LpfC += zV;}
  else {LpfC -= zV;}
       if (LpfC > 1.000) {LpfC = 1.000;}
  else if (LpfC < 0.01) {LpfC = 0.01;}
}

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

void GrsAngInc(int zY,float zV) {
  // increment/decrement the gears pitch angle
  if (DmB == 0) {
    switch (zY) {
      case 1: TgtAng1 += zV; if (TgtAng1 > 9.99) {TgtAng1 = 9.99;} break;
      case 2: TgtAng2 += zV; if (TgtAng2 > 9.99) {TgtAng2 = 9.99;} break;
      case 3: TgtAng3 += zV; if (TgtAng3 > 9.99) {TgtAng3 = 9.99;} break;
    }
  } else {
    switch (zY) {
      case 1: TgtAng1 -= zV; if (TgtAng1 < 0.00) {TgtAng1 = 0.00;} break;
      case 2: TgtAng2 -= zV; if (TgtAng2 < 0.00) {TgtAng2 = 0.00;} break;
      case 3: TgtAng3 -= zV; if (TgtAng3 < 0.00) {TgtAng3 = 0.00;} break;
    }
  } changeGear(Gear);
}

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

void GrsSpdInc(int zY,float zV) {
  // increment/decrement the gears max speed
  if (DmB == 0) {
    switch (zY) {
      case 1: maxTgtSpd1 += zV; if (maxTgtSpd1 > 9.99) {maxTgtSpd1 = 9.99;} break;
      case 2: maxTgtSpd2 += zV; if (maxTgtSpd2 > 9.99) {maxTgtSpd2 = 9.99;} break;
      case 3: maxTgtSpd3 += zV; if (maxTgtSpd3 > 9.99) {maxTgtSpd3 = 9.99;} break;
    }
  } else {
    switch (zY) {
      case 1: maxTgtSpd1 -= zV; if (maxTgtSpd1 < 0.00) {maxTgtSpd1 = 0.00;} break;
      case 2: maxTgtSpd2 -= zV; if (maxTgtSpd2 < 0.00) {maxTgtSpd2 = 0.00;} break;
      case 3: maxTgtSpd3 -= zV; if (maxTgtSpd3 < 0.00) {maxTgtSpd3 = 0.00;} break;
    }
  } changeGear(Gear);
}

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

void motorChDir() {
  // change the direction of the motor using a SafeMode task
  switch (motorPnt) {
    case 0: motorATgt = -motorA_Vel; break;
    case 1: motorBTgt = -motorB_Vel; break;
    case 2: motorCTgt = -motorC_Vel; break;
  }
  TestMode = 3;
}

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

void motorChPnt() {
  // called when user clicks on velocity letter
  if (DmB) {
    motorPnt--; if (motorPnt < 0) {motorPnt = 2;}
  } else {
    motorPnt++; if (motorPnt > 2) {motorPnt = 0;}
  }
}

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

void motorFreq() {
  // called in velocity mode to toggle the motor.move() frequency
  if (motorPnt < 0) {return;}

  if (DmB) {
    switch(Motor_Freq) {
      case 200: Motor_Freq = 100; break;
      case 333: Motor_Freq = 200; break;
      case 400: Motor_Freq = 333; break;
      case 500: Motor_Freq = 400; break;
      case 999: Motor_Freq = 500; break;
    }
  } else {
    switch(Motor_Freq) {
      case 100: Motor_Freq = 200; break;
      case 200: Motor_Freq = 333; break;
      case 333: Motor_Freq = 400; break;
      case 400: Motor_Freq = 500; break;
      case 500: Motor_Freq = 999; break;  // set max update rate
    }
  }
}

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

void motorInc(float zV) {
  // increment the motor velocity by zV
  if (motorPnt < 0) {return;}

  switch (motorPnt) {
    case 0:
      if (DmB == 0) {if (motorA_Vel >= 0.0) {motorA_Vel += zV;} else {motorA_Vel -= zV;}}
      else {if (motorA_Vel >= 0.0) {motorA_Vel -= zV;} else {motorA_Vel += zV;}}
      if (motorA_Vel > 60.0) {motorA_Vel = 60.0;}
      else if (motorA_Vel < -60.0) {motorA_Vel = -60.0;}
      motorA.move(motorA_Vel); break;
    case 1:
      if (DmB == 0) {if (motorB_Vel >= 0.0) {motorB_Vel += zV;} else {motorB_Vel -= zV;}}
      else {if (motorB_Vel >= 0.0) {motorB_Vel -= zV;} else {motorB_Vel += zV;}}
      if (motorB_Vel > 60.0) {motorB_Vel = 60.0;}
      else if (motorB_Vel < -60.0) {motorB_Vel = -60.0;}
      motorB.move(motorB_Vel); break;
    case 2:
      if (DmB == 0) {if (motorC_Vel >= 0.0) {motorC_Vel += zV;} else {motorC_Vel -= zV;}}
      else {if (motorC_Vel >= 0.0) {motorC_Vel -= zV;} else {motorC_Vel += zV;}}
      if (motorC_Vel > 60.0) {motorC_Vel = 60.0;}
      else if (motorC_Vel < -60.0) {motorC_Vel = -60.0;}
      motorC.move(motorC_Vel); break;
  }
}

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

void motorInitA(float zVol) {
  // initialise motor A controller
  // called for the first time
  Serial.println(F("Initialising SimpleFOC driver A..."));
  if (BatVfpIni == 0.0) {BatVfpIni = BatVfp;} // set value when first used
  driverA.voltage_power_supply = BatVfpIni;
  driverA.init();
  motorA.linkDriver(&driverA);
  motorA.controller = MotionControlType::velocity_openloop;  // t1-t0 = 36us
  motorAVol = zVol;
  motorA.voltage_limit = motorAVol;   // maximum power_supply_voltage/2
  motorA.pole_pairs = 7;              // t1-t0 = 36us
  motorA.motion_downsample = 0;       // turn off downsampling
  motorA.init();                      // t1-t0 = 99622us
  digitalWrite(MotorAEn,HIGH);        // motorA PWM is now ON
  // set the output for zero velocity initially
  motorA_Vel = 0.0; motorATgt = 0.0;
  motorA.move(0.0); OpA_En = true;    // apply FOC PWM
}

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

void motorInitB(float zVol) {
  // initialise motor B controller
  // called for the first time
  Serial.println(F("Initialising SimpleFOC driver B..."));
  if (BatVfpIni == 0.0) {BatVfpIni = BatVfp;} // set value when first used
  driverB.voltage_power_supply = BatVfpIni;
  driverB.init();
  motorB.linkDriver(&driverB);
  motorB.controller = MotionControlType::velocity_openloop;  // t1-t0 = 36us
  motorBVol = zVol;
  motorB.voltage_limit = motorBVol;   // maximum power_supply_voltage/2
  motorB.pole_pairs = 7;              // t1-t0 = 36us
  motorB.motion_downsample = 0;       // turn off downsampling
  motorB.init();  // t1-t0 = 99622us
  digitalWrite(MotorBEn,HIGH);        // motorB PWM is now ON
  // set the output for zero velocity initially
  motorB_Vel = 0.0; motorBTgt = 0.0;
  motorB.move(0.0); OpB_En = true;    // apply FOC PWM
}

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

void motorInitC(float zVol) {
  // initialise motor C controller
  // called for the first time
  Serial.println(F("Initialising SimpleFOC driver C..."));
  if (BatVfpIni == 0.0) {BatVfpIni = BatVfp;} // set value when first used
  driverC.voltage_power_supply = BatVfpIni;
  driverC.init();
  motorC.linkDriver(&driverC);
  motorC.controller = MotionControlType::velocity_openloop;  // t1-t0 = 36us
  motorCVol = zVol;
  motorC.voltage_limit = motorCVol;   // maximum power_supply_voltage/2
  motorC.pole_pairs = 7;              // t1-t0 = 36us
  motorC.motion_downsample = 0;       // turn off downsampling
  motorC.init();                      // t1-t0 = 99622us
  digitalWrite(MotorCEn,HIGH);        // motorA PWM is now ON
  // set the output for zero velocity initially
  motorC_Vel = 0.0; motorCTgt = 0.0;
  motorC.move(0.0); OpC_En = true;    // enable loopFOC
}

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

void PID_Ptch() {
  // calculate Pitch PID gains
  // the controller is angle driver, relative to a setpoint
  // the self-balance setpoint is determined through subsequence calculations
  // ############################################################################
  //
  // Pitch PID gains
  //
  // ############################################################################
  // Pitch PID gains are only calculated if Ptchen == true
  if (Ptchen) {
    PtchErr = PtchGyrAng - SbPtchSp - PtchSp;

    // apply temporary step function, if user has clicked on the PID display
    if (PtchStepT > 0) {
      PtchStepT--; PtchErr += PtchStepV;
    } else if (PtchStepT < 0) {
      PtchStepT++; PtchErr -= PtchStepV;
    }

    // apply a brake function to resist pushing and rolling forward/backwards
    if (moveDir == 0) {
      if (fabs(Ptch_Op) > Brake_Vel) {Brake = Ptch_Op * Brake_Force; PtchErr += Brake;}
      else {Brake = 0.0;}
    }

    Ptch_P = PtchPwr * Ptch_p_gain * PtchErr;          // calculate the P component
    Ptch_Op = Ptch_P;

    if (Ptch_i_En > 0) {
      // since the PID loop is running at 200Hz the i-gain errors build up
      // very quickly, so we pre-scale the i-gain velues
      Ptch_i_mem += PtchPwr * Ptch_i_gain * PtchErr * 0.1;
          if (Ptch_i_mem > Ptch_i_max) {Ptch_i_mem = Ptch_i_max;}
      else if (Ptch_i_mem < -Ptch_i_max) {Ptch_i_mem = -Ptch_i_max;}
      Ptch_Op += Ptch_i_mem;
    } else {Ptch_i_mem = 0.0;}

    if (Ptch_d_En > 0) {
      Ptch_D = 2.0 * PtchPwr * Ptch_d_gain * (PtchErr - PtchLastDErr);
      Ptch_Op += Ptch_D;
    }
    PtchLastDErr = PtchErr;

    // pre-scale the output
    Ptch_Op *= 0.1;

    // limit PID output
        if (Ptch_Op > Ptch_out_max) {Ptch_Op = Ptch_out_max;}
    else if (Ptch_Op < -Ptch_out_max) {Ptch_Op = -Ptch_out_max;}

    // ############################################################################
    // Set Pitch motor demand
    // ############################################################################
    // the change in motor velocity (force) is proportional to pid_Op
    // the sign of moto_Vd needs to be opposite to the pid_Op
    motorC_Vd = motorC_Vel;             // note previous pass through value
    motorC_Vel = -Ptch_Op;              // set new motorC demand

    // ############################################################################
    // Lowpass filter
    // ############################################################################
    // apply lowpass FIR filter to output
    // when LpfC == 1.0 zero filtering, as LpfC >> 0.0 then max filtering
    // motorC_Vel = (LpfC * motorC_Vel) + ((1.0 - LpfC) * motorC_Vd);

    // Auto power control, when less velocity change is required we reduce the applied voltage
    // this only applies once the system is fully up and running

    // ############################################################################
    // Pitch automatic self-balance adjustment
    // ############################################################################
    // When enabled, at start-up or when there is not Nunchuk Y-demand, this function
    // adjusts the self-balance setpoint, to achieve balance.
    // The SbPtchTrend variable ensures that this is now wound up by pushing
    if ((PtchSb) && (moveDir == 0)) {
      // self-balance enabled and Uni-Bot is not being driven by Nunchuk demands
      if (motorC_Vel > 0.00) {
        // if SbPtchCnt is -ve when switching direction, reduce the setpoint increment
        if (SbPtchCnt < 0) {SbPtchCnt = 0; if (SbPtchSpInc > 0.000) {SbPtchSpInc -= 0.001; SbPtchSp += SbPtchSpInc;} else {SbPtchSpInc = 0.000;}}
        SbPtchCnt++; if (SbPtchCnt >= 30) {SbPtchCnt = 0; SbPtchSp += SbPtchSpInc;}
      } else if (motorC_Vel < 0.00) {
        // if SbPtchCnt is +ve when switching direction, reduce the setpoint increment
        if (SbPtchCnt > 0) {SbPtchCnt = 0; if (SbPtchSpInc > 0.000) {SbPtchSpInc -= 0.001; SbPtchSp -= SbPtchSpInc;} else {SbPtchSpInc = 0.000;}}
        SbPtchCnt--; if (SbPtchCnt <= -30) {SbPtchCnt = 0; SbPtchSp -= SbPtchSpInc;}
      }
    }

    // ############################################################################
    // Speed factor
    // ############################################################################
    // demand from Nunchuk may have created a speed factor, so add this in
    // motorC_Vel += motorC_Spd;
    motorC_Vd = motorC_Vel - motorC_Vd; // change in velocity in this pass through
    // if (Report20ms >= 5) {Report20ms = 0; PrintTx+= String(motorC_Spd,2) + "\t" + String(motorC_Vel,2) + "\n";} // report this every 100ms

    // limit motor to max forward/reverse speeds
        if (motorC_Vel > motorC_VelMax) {motorC_Vel = motorC_VelMax;}
    else if (motorC_Vel < -motorC_VelMax) {motorC_Vel = -motorC_VelMax;}

    // ############################################################################
    // Pitch automatic PWM power adjustment
    // ############################################################################
    // apply demand driven PWM voltage limits
    // demands are applied immediately, then decay, like a peak detector
    float zVel = fabs(motorC_Vel);
    if (zVel > 1.0) {
      // pitch wheel motor is turning, so adjust power for speed
      zVel = mapF(zVel,1.0,5.0,2.0,4.0); if (zVel  > 4.0) {zVel = 4.0;}
    } else zVel = 2.0;
    // check for power plus when joystick demands movement
    if (PwrPlus) {PwrPlus = false;if (zVel < 4.00) {zVel = 4.00;}}
    // check for motor speed change
    float zVd = fabs(motorC_Vd);
    zVd = mapF(zVd,0.20,0.80,2.0,4.0); if (zVd > 4.0) {zVd = 4.0;}
    // use the larger of the two values
    if (fabs(zVd) > fabs(zVel)) {zVel = zVd;}
    motorCVol = zVel; motorC.voltage_limit = getVolNow(motorCVol);
    motorC.move(motorC_Vel);

    // all done, the motor_Vel values are applied during the main loop
  } else if (OpC_En) {
    // motor should not be on
    motorCVol = 0.0; motorC.voltage_limit = motorCVol;
    motorC_Vel = 0.0; motorC.move(motorC_Vel);
    OpC_En = false; digitalWrite(MotorCEn,LOW);
  }
}

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

void PID_readAcc() {
  // PID accelerometer readings from MPU6050
  // we are only interested in the Y and Z accelerometers
   t2 = micros();                       // we are timing this I2C function
  AcXL = AcX; AcYL = AcY; AcZL = AcZ;   // track previous values
  Wire.beginTransmission(MPU_address);
  Wire.write(0x3D);                     // starting with register 0x3D (ACCEL_YOUT_H)
  I2CErr = Wire.endTransmission();      // End the transmission with the MPU.
  if (I2CErr != 0) {return;}            // abort on errors
  delayMicroseconds(I2Cdel);            // Allow MPU time to respond

  Wire.requestFrom(MPU_address,4);      // request 4 register read
  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)
  AcY -= AccOffY;  // note AcYOff should be zero initially in order to determine its value
  AcZ -= AccOffZ;  // note AcZOff should be zero initially in order to determine its value

  // //################################
  // // Averaging Noise Filters
  // //################################
  // AcX = (AcX + AcXL)/2; AcY = (AcY + AcYL)/2; AcZ = (AcZ + AcZL)/2;
  
  // //################################
  // // rapid Roll overshoot
  // //################################
  // if rate of change of AccDif is too great, so limit to change to AccRamp
  if (AccRampX > 0) {
    AccDif = AcX - AcXL; if (fabs(AccDif) > AccRampX) {if (AcX > AcXL) {AcX = AcXL + AccRampX;} else {AcX = AcXL - AccRampX;}}
  }
  if (AccRampY > 0) {
    AccDif = AcY - AcYL; if (fabs(AccDif) > AccRampY) {if (AcY > AcYL) {AcY = AcYL + AccRampY;} else {AcY = AcYL - AccRampY;}}
  }
  if (AccRampZ > 0) {
    AccDif = AcZ - AcZL; if (fabs(AccDif) > AccRampZ) {if (AcZ > AcZL) {AcZ = AcZL + AccRampZ;} else {AcZ = AcZL - AccRampZ;}}
  }

  //##############################################
  // Averaging filters to reduce MPU random noise
  //##############################################
  AcXSum = AcXSum - AcXAvg + AcX; AcXAvg = AcXSum /AvCnt; AcX = AcXAvg;
  AcYSum = AcYSum - AcYAvg + AcY; AcYAvg = AcYSum /AvCnt; AcY = AcYAvg;
  AcZSum = AcZSum - AcZAvg + AcZ; AcZAvg = AcZSum /AvCnt; AcZ = AcZAvg;

  // limit values to avoid trig function problems
  if (AcX > 8192) {AcX = 8192;} else if (AcX < -8192) {AcX = -8192;}
  if (AcY > 8192) {AcY = 8192;} else if (AcY < -8192) {AcY = -8192;}
  if (AcZ > 8192) {AcZ = 8192;} else if (AcZ < -8192) {AcZ = -8192;}

  t3 = micros() - t2;                   // capture time spent in t3, 980us
}

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

void PID_readGyr() {
  // now read all of the gyroscopes
  GyXL = GyX; GyYL = GyY; GyZL = GyZ;   // track previous values
  Wire.beginTransmission(MPU_address);
  Wire.write(0x43);                     // starting with register 0x43 (GYRO_XOUT_H)
  I2CErr = Wire.endTransmission();      // End the transmission with the MPU.
  if (I2CErr != 0) {return;}            // abort on errors
  delayMicroseconds(I2Cdel);                // Allow MPU time to respond

  Wire.requestFrom(MPU_address,6);      // request 6 register reads
  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();     // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  // get measurement timing
  GyTD = micros() - GyT;                // get the differential time between readings
  GyT = GyTD + GyT;                     // set the timer = micros(), for the next reading
  // apply calibration offsets to these raw values
  GyX -= GyrOffX;  // note GyrOffX should be zero initially in order to determin its value
  GyY -= GyrOffY;  // note GyrOffY should be zero initially in order to determin its value
  GyZ -= GyrOffZ;  // note GyrOffZ should be zero initially in order to determin its value

  // //################################
  // // Averaging Noise Filters
  // //################################
  // GyX = (GyX + GyXL)/2; GyY = (GyY + GyYL)/2; GyZ = (GyZ + GyZL)/2;
  //##############################################
  // Averaging filters to reduce MPU random noise
  //##############################################
  GyXSum = GyXSum - GyXAvg + GyX; GyXAvg = GyXSum /AvCnt; GyX = GyXAvg;
  GyYSum = GyYSum - GyYAvg + GyY; GyYAvg = GyYSum /AvCnt; GyY = GyYAvg;
  GyZSum = GyZSum - GyZAvg + GyZ; GyZAvg = GyZSum /AvCnt; GyZ = GyZAvg;
}

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

void PID_Roll() {
  // calculate Roll PID gains
  // the Roll reaction wheel turning force is based on wheel acceleration
  // ############################################################################
  //
  // Roll PID gains
  //
  // ############################################################################
  // Roll PID gains are only calculated if Rollen == true
  if (Rollen) {
    RollErr = RollGyrAng - SbRollSp - RollSp;
    // float zRoll_P = Roll_P;
    Roll_P = RollPwr * Roll_p_gain * RollErr;          // calculate the P component
    Roll_Op = Roll_P;

    if ((Roll_i_En > 0) && (Roll_i_gain != 0.00)) {
      // The i-gain is reduced by a factor of x0.1 to improve the tuning range, and
      // make the integrator less aggressive 
      Roll_i_mem += RollPwr * Roll_i_gain * RollErr * 0.1;
          if (Roll_i_mem > Roll_i_max) {Roll_i_mem = Roll_i_max;}
      else if (Roll_i_mem < -Roll_i_max) {Roll_i_mem = -Roll_i_max;}
      Roll_Op += Roll_i_mem;
    } else {Roll_i_mem = 0.0;}

    if (Roll_d_En > 0) {
      Roll_D = RollPwr * Roll_d_gain * (RollErr - RollLastDErr);
      Roll_Op += Roll_D;
    }
    RollLastDErr = RollErr;

    // limit PID output
        if (Roll_Op > Roll_out_max) {Roll_Op = Roll_out_max;}
    else if (Roll_Op < -Roll_out_max) {Roll_Op = -Roll_out_max;}

    motorA_Vd = Roll_Op * 0.1;


    //###########################################################
    // constrain acceleration Vd
    //###########################################################
         if (motorA_Vd > motorA_VdMax) {motorA_Vd = motorA_VdMax;}
    else if (motorA_Vd < -motorA_VdMax) {motorA_Vd = -motorA_VdMax;}
    motorA_Vel += motorA_Vd;


    //#############################################
    // automatic self-balance adjustment
    //#############################################
    // the Uni-Bot will have a vertical angle at which it is in balance. This
    // code aims to find that point, by adjusting the self-balance setpoint,
    // based on wheel rotation, resulting in a hunting side to side sway. The
    // amount of adjustment is reduced over time, in a homing action.
    if (RollSb) {
      if (motorA_Vel > 0.05) {
        if (SbRollCnt < 0) {SbRollCnt = 0; if (SbRollSpInc > 0.004) {SbRollSpInc -= 0.001;}}
        SbRollCnt++; if (SbRollCnt >= 10) {SbRollCnt = 0; SbRollSp -= SbRollSpInc;}
      } else if (motorA_Vel < -0.05) {
        if (SbRollCnt > 0) {SbRollCnt = 0; if (SbRollSpInc > 0.004) {SbRollSpInc -= 0.001;}}
        SbRollCnt--; if (SbRollCnt <= -10) {SbRollCnt = 0; SbRollSp += SbRollSpInc;}
      }
    } else {SbRollSp = RollStartAng;}

    // ensure motor velocity does not exceed max rating
         if (motorA_Vel >  motorA_VelMax) {motorA_Vel =  motorA_VelMax; motorA_Vd= 0.0;}
    else if (motorA_Vel < -motorA_VelMax) {motorA_Vel = -motorA_VelMax; motorA_Vd= 0.0;}

    //#############################################
    // power control
    //#############################################
    // apply demand driven PWM voltage limits
    // demands are determined for speed and rate of change, and we take the larger value
    // first look at speed
    float zVs = fabs(motorA_Vel);
         if (zVs <= 5.0) {zVs = 2.5;}
    else {zVs = mapF(zVs,5.0,60.0,2.5,4.5);}
    // now look at rate change
    float zVd = fabs(motorA_Vd);
    if (zVd <= 0.1) {zVd = 2.5;} else {zVd = mapF(zVd,0.1,0.8,2.5,4.5);}
    // use max value or slowly reduce applied voltage
    if (zVs < zVd) {zVs = zVd;}
    if (motorAVol < zVs) {motorAVol = zVs;} else {motorAVol -= 0.001;}
    motorA.voltage_limit = getVolNow(motorAVol);
    motorA.move(motorA_Vel);

    //#############################################
    // Overspeed warning
    //#############################################
    // a wardning is issued if the speed exceeds 66% for 200ms
    if (fabs(motorA_Vel) >= 40.0) {
      OvrSpd++; if (OvrSpd == 40) {Display_Warning();}
    } else if (OvrSpd > 0) {OvrSpd--;}

    // all done, the motor_Vel values are applied during the main loop
  } else if (OpA_En) {
    // motor should not be on
    motorA_Vel = 0.0; motorA.move(motorA_Vel);
    motorAVol = 0.0; motorA.voltage_limit = motorAVol;
    OpA_En = false; digitalWrite(MotorAEn,LOW);
  }
}

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

void Pitch_OFF() {
  // called from Pitch Safe Mode or when toggling Ptchen
  motorCVol = 0.0;                    // zero PWM voltage
  motorC.voltage_limit = motorCVol;   // set PWM voltage limit
  motorC_Vel = 0.0;                   // zero motor velocity
  motorC.move(motorC_Vel);            // stop the motor function
  PIDtask = 0;                        // disable PID tasks
  OpC_En = false;                     // disable motor o/p and FOC
  digitalWrite(MotorCEn,LOW);         // motor PWM is OFF
  PtchErr = 0.0;                      // clear angle error
  Ptch_Op = 0.0;                      // force PID o/p to zero
  SbPtchSp = 0.0;                     // reset self-balancing setpoint
}

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

void PtchDinc(float zV) {
  // increment the Pitch PID D-gain by zV
  if (DmB != 0) {
    // mouse right-click
    if (Ptch_d_gain > 0.00) {Ptch_d_gain -= zV;}
    else {Ptch_d_En = false; Ptch_d_gain= 0.00;}
  } else {
         if (!Ptch_d_En) {Ptch_d_En = true; Ptch_d_gain= 0.00;}
    else if (Ptch_d_gain <= 99.99) {Ptch_d_gain += zV;}
  }
  if (Ptch_d_gain < 0.00) {Ptch_d_gain = 0.00;}     // value must be >= 0.0
  if (Ptch_d_gain > 99.99) {Ptch_d_gain = 99.99;} // value must be <= 99.99
}

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

void PtchIinc(float zV) {
  // increment the Pitch PID I-gain by zV
  if (DmB != 0) {
    // mouse right-click
    if (Ptch_i_gain > 0.00) {Ptch_i_gain -= zV;}
    else {Ptch_i_En = false; Ptch_i_gain= 0.00;}
  } else {
         if (!Ptch_i_En) {Ptch_i_En = true; Ptch_i_gain= 0.00;}
    else if (Ptch_i_gain <= 99.99) {Ptch_i_gain += zV;}
  }
  if (Ptch_i_gain < 0.00) {Ptch_i_gain = 0.00;}     // value must be >= 0.0
  if (Ptch_i_gain > 99.99) {Ptch_i_gain = 99.99;} // value must be <= 99.99
}

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

void PtchIMinc(float zV) {
  // increment the Pitch PID I-max by zV
  if (DmB != 0) {if (Ptch_i_max >= 0.00) {Ptch_i_max -= zV;}}
  else {if (Ptch_i_max <= 99.99) {Ptch_i_max += zV;}}
  if (Ptch_i_max < 0.00) {Ptch_i_max = 0.00;}     // value must be >= 0.0
  if (Ptch_i_max > 99.99) {Ptch_i_max = 99.99;} // value must be <= 99.99
}

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

void PtchPinc(float zV) {
  // increment the Pitch PID P-gain by zV
  if (DmB != 0) {if (Ptch_p_gain >= 0.00) {Ptch_p_gain -= zV;}}
  else {if (Ptch_p_gain <= 99.99) {Ptch_p_gain += zV;}}
  if (Ptch_p_gain < 0.00) {Ptch_p_gain = 0.00;}     // value must be >= 0.0
  if (Ptch_p_gain > 99.99) {Ptch_p_gain = 99.99;} // value must be <= 99.99
}

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

void Pitch_Safe_Mode() {
  // controls the use of the Pitch PID controller by monitoring conditions, as well
  // as a task pointer for other functions
  // States:
  // -2 - Lock Pitch motor for 1 sec
  // -1 - turn Pitch motor off
  //  0 - default state
  //  1 - monitors angle for ready state
  //  2 - ready state, looks for near set-point start
  //  3 - setpoint triggered, so ramp up power
  //  4 - balancing state, check for topple
  if (PtchSafeDel > 0) {PtchSafeDel--; return;}
  if (!Ptchen) {return;}  // Pitch PID mode is disabled

  switch (PtchSafeMode) {
    case -2:  // lock motor state
      // called when UNi-Bot topples over
      // we set the motor speed to zero for 1 sec, and reduce power o/p
      motorCVol = 2.5; motorC.voltage_limit = getVolNow(motorAVol);
      motorC_Vel = 0.0; motorC.move(motorC_Vel);
      PtchSafeDel = 100; PtchSafeMode++; break;
    case -1: // ensure PID controller and motor drive are off
      if (OpC_En) {Pitch_OFF();}
      PtchSafeMode++; PtchSafeCnt = 0;break; // null state
    case 0: // default state, waiting for action
      // remain in this state until the robot is placed face down for 1 sec
      // then goto state 1
      if (AcY < -7000) {PtchSafeCnt++; if (PtchSafeCnt > 100) {PtchSafeMode++; set_LED_Task(1);}}
      else {PtchSafeCnt = 0;}
      break;
    case 1: // check monitor angle for ready state
      // the robot has to be raised from the face down state
      if ((fabs(PtchGyrAng) <= ReadyAng) && (fabs(RollGyrAng) <= ReadyAng)) {
        // turn pitch PWM o/p back on
        // set the output for zero velocity initially
        motorCVol = 2.5;                  // initial PWM voltage
        motorC.voltage_limit = getVolNow(motorAVol); // set motor PWM voltage
        motorC_Vel = 0.0; OpC_En = true;  // enable loopFOC
        digitalWrite(MotorCEn,HIGH);      // motor PWM is now ON
        motorC.move(motorC_Vel);          // motor speed is 0.0
        PtchSafeMode++;                   // goto next state
        DispMode = 1;                     // Display_MPU_Levels()
      } else {
        // not 'ready' so motor remains off
        if (OpC_En) {
          motorCVol = 0.0; motorC.voltage_limit = motorAVol;
          motorC_Vel = 0.0; motorC.move(motorC_Vel);
          OpC_En = false; digitalWrite(MotorCEn,LOW);
        }
      }
      break;
    case 2: // ready state, looks for near set-point start
      // check that we remain within the 'ready' region, but if we aren't,
      // then drop back into state 1. Allow a margin to create a hysteresis effect
      PtchGo = false;
      if ((fabs(PtchGyrAng) > (ReadyAng + 5.0)) || (fabs(RollGyrAng) > (ReadyAng + 5.0))) {PtchSafeMode = 1;}
      // now look for centre point
      else if ((PtchGyrAng <= (PtchStartAng + 0.5)) && (PtchGyrAng >= (PtchStartAng - 0.5))) {
        PtchGo = true;
        // synchronise startup, roll and pitch must start together, if they are both enabled
        if (RollGo || !Rollen) {
          // switch to power-on mode
          PtchGyrAng = PtchAccAng;    // set gyro angle to match AcY angle
          motorC_Spd = 0.0;           // motorC speed factor, based on Nunchuk demand
          motorC_Vel = 0.0;           // zero motor velocity
          moveDir = 0;                // reset move direction flag. 0 == stationary
          PtchSp = 0.0;               // reset any demands on the primary setpoint
          Ptch_i_mem = 0;             // zero the i-gain memory
          PtchLastDErr = 0.0;         // zero the last d-gain
          SbPtchSp = PtchStartAng;    // set Pitch self-balancing set point to an initial angle
          PtchSafeMode++;             // switch to power-up safemode
          SbPtchCnt = 0;              // counter used in auto self-balance point adjustment
          SbPtchSpInc = SbPtchSpIncI; // set self-balance auto-increment
          PIDtask = 10;               // enable PID tasks with a value > 7
          motorCVol = motorCVolMax;   // set motor PWM voltage to max
          motorC.voltage_limit = getVolNow(motorCVol);
          PtchPwr = 0.5;              // limit PID out to 50% initially
        }
      }
      // in the ready region we slowly drive the wheel towards the balance point
            if (PtchGyrAng > (PtchStartAng + 0.3)) {motorC_Vel = -1.0; motorCVol = 2.5;}   // driver slowly forwards towards balance point
      else if (PtchGyrAng < (PtchStartAng - 0.3)) {motorC_Vel =  1.0; motorCVol = 2.5;}   // driver slowly backwards towards balance point
      else {motorC_Vel = 0.0; motorCVol = 2.0;}                         // STOP motorC in centre deadband
      motorC.voltage_limit = getVolNow(motorCVol);
      break;
    case 3: // setpoint triggered, so ramp up power and transition to RollSafeMode = 4
      // at 10ms cycles, this takes 1 second from 50%
      if (PtchPwr < 1.0) {PtchPwr+= 0.005;} else {PtchSafeMode++;}
      break;
    case 4: // balancing state
      // test for robot falling over
      if (fabs(PtchAccAng) >= toppleAng) {
        // toppling over, so abort and reset both safe modes
        PIDtask = 0;                // turn OFF the PID loop sequence
        PtchGo = false;             // reset Pitch PID active flag
        Task10ms = 0;               // reset the Demo mode task pointer
        if (Rollen) {
          // both Pitch and Roll are enabled, so force both into initialisation states
          PtchSafeMode = -2; RollSafeMode = -2; set_LED_Task(1);
        } else {
          // since Roll PID is disabled, it is assumed that we are trialing Pitch balancing
          // so here we stop the PID and Pitch motor, and goto SafeMode 1
          motorC_Vel = 0.0;         // stop the drive wheel motor
          motorC.move(motorC_Vel);  //
          Reset_Ptch_PID();         // clear the PID variables for plotting
          PtchSafeMode = 1;         // return to looking for 'ready' state
          set_LED_Task(1);          // set LEDs to Demo mode
        }
        Turn_OFF();                 // stop turn motor if it was on
        moveDir = 0;                // reset direction flag
        changeGear(1);              // drop into bottom speed settings
        Display_Topple();           // display event
      }
      break;
  }
}

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

void PtchSinc(float zV) {
  // increment the Pitch PID StartAng by zV
  if (DmB != 0) {if (PtchStartAng >= -9.99) {PtchStartAng -= zV;}}
  else {if (PtchStartAng <= 9.99) {PtchStartAng += zV;}}
  if (PtchStartAng < -9.99) {PtchStartAng = -9.99;}   // value must be >= -9.99
  if (PtchStartAng > 9.99) {PtchStartAng = 9.99;}     // value must be <= 9.99
}

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

void PtchSbInc(float zV) {
  // increment the Pitch SbPtchSpInc by zV
  // if the function is OFF then turn it on with minimum value
  // if the function is reduced beyond 0.000 then tuirn it OFF
  if (DmB != 0) {
    if (PtchSb) {
            if (SbPtchSpInc == 0.000) {PtchSb = false;}
      else if (SbPtchSpInc >= 0.000) {SbPtchSpInc -= zV;}
    }
  } else {
         if (!PtchSb) {PtchSb = true; SbPtchSpInc= 0.000;}
    else if (SbPtchSpInc <= 9.999) {SbPtchSpInc += zV;}
  }
  if (SbPtchSpInc <= 0.000) {SbPtchSpInc = 0.000;}      // value must be >= 0.000
  if (SbPtchSpInc >= 9.999) {SbPtchSpInc = 9.999;}      // value must be <= 9.999
  if (PtchSafeMode != 4) {SbPtchSpIncI = SbPtchSpInc;}  // keep initial increment in track
}

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

void PtchStep() {
  // Introduce a temporary step angle into the Pitch PID controller, to see how
  // it responds and to tune how stable the controller is.
  // PtchStepT sets the pulse duration (x5ms). PtchStepV sets the size if the pulse.
  // **WARNING** larget values of PtchStepT will cause the Uni-Bot to drive away
  if (DmB == 0) {
    // left mouse click
    PtchStepT = 100; PtchStepV = 1.0;  // 1.0 degree step for 500ms
  } else {
    // right mouse click
    PtchStepT = -100; PtchStepV = 1.0; // 1.0 degree step for 500ms
  }
}

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

void Reset_Ptch_PID() {
  // zero Pitch coefficients
  PtchGo = false;
  Ptch_P = 0.0;
  Ptch_i_mem = 0.0;
  Ptch_D = 0.0;
  Ptch_Op = 0;
}

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

void Reset_Roll_PID() {
  // zero Roll coefficients
  RollGo = false;
  Roll_P = 0.0;
  Roll_i_mem = 0.0;
  Roll_D = 0.0;
  Roll_Op = 0;
}

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

void RollDinc(float zV) {
  // increment the Roll PID D-gain by zV
  if (DmB != 0) {
    // mouse right-click
    if (Roll_d_gain > 0.00) {Roll_d_gain -= zV;}
    else {Roll_d_En = false; Roll_d_gain= 0.00;}
  } else {
         if (!Roll_d_En) {Roll_d_En = true; Roll_d_gain= 0.00;}
    else if (Roll_d_gain <= 99.99) {Roll_d_gain += zV;}
  }
  if (Roll_d_gain < 0.00) {Roll_d_gain = 0.00;}     // value must be >= 0.0
  if (Roll_d_gain > 99.99) {Roll_d_gain = 99.99;}   // value must be <= 99.99
}

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

void RollIinc(float zV) {
  // increment the Roll PID I-gain by zV
  if (DmB != 0) {
    // mouse right-click
    if (Roll_i_gain > 0.00) {Roll_i_gain -= zV;}
    else {Roll_i_En = false; Roll_i_gain= 0.00;}
  } else {
         if (!Roll_i_En) {Roll_i_En = true; Roll_i_gain= 0.00;}
    else if (Roll_i_gain <= 99.99) {Roll_i_gain += zV;}
  }
  if (Roll_i_gain < 0.00) {Roll_i_gain = 0.00;}     // value must be >= 0.0
  if (Roll_i_gain > 99.99) {Roll_i_gain = 99.99;}   // value must be <= 99.99
}

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

void RollIMinc(float zV) {
  // increment the Roll PID I-max by zV
  if (DmB != 0) {if (Roll_i_max >= 0.00) {Roll_i_max -= zV;}}
  else {if (Roll_i_max <= 99.99) {Roll_i_max += zV;}}
  if (Roll_i_max < 0.00) {Roll_i_max = 0.00;}     // value must be >= 0.0
  if (Roll_i_max > 99.99) {Roll_i_max = 99.99;} // value must be <= 99.99
}

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

void RollPinc(float zV) {
  // increment the Roll PID P-gain by zV
  if (DmB != 0) {if (Roll_p_gain >= 0.00) {Roll_p_gain -= zV;}}
  else {if (Roll_p_gain <= 99.99) {Roll_p_gain += zV;}}
  if (Roll_p_gain < 0.00) {Roll_p_gain = 0.00;}     // value must be >= 0.0
  if (Roll_p_gain > 99.99) {Roll_p_gain = 99.99;} // value must be <= 99.99
}

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

void Roll_OFF() {
  // called from Roll Safe Mode or when toggling Rollen
  motorAVol = 0.0;                    // zero PWM voltage
  motorA.voltage_limit = motorAVol;   // set PWM voltage limit
  motorA_Vel = 0.0;                   // zero motor velocity
  motorA.move(motorA_Vel);            // stop the motor function
  PIDtask = 0;                        // disable PID tasks
  OpA_En = false;                     // disable motor o/p and FOC
  digitalWrite(MotorAEn,LOW);         // motor PWM is OFF
  RollErr = 0.0;                      // clear angle error
  Roll_Op = 0.0;                      // force PID o/p to zero
  SbRollSp = 0.0;                     // reset self-balancing setpoint
}

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

void Roll_Safe_Mode() {
  // controls the use of the Roll PID controller by monitoring conditions, as well
  // as a task pointer for other functions
  // States:
  // -1 - null state
  //  0 - default state
  //  1 - monitors angle for ready state
  //  2 - ready state, looks for near set-point start
  //  3 - setpoint triggered, so ramp up power
  //  4 - balancing state, check for topple
  if (RollSafeDel > 0) {RollSafeDel--; return;}
  if (!Rollen) {return;}  // Roll PID mode is disabled

  switch (RollSafeMode) {
    case -2:  // lock motor state
      // called when Uni-Bot topples over
      // we set the motor speed to zero for 1 sec and reduce power o/p
      motorAVol = 2.5; motorA.voltage_limit = getVolNow(motorAVol);
      motorA_Vel = 0.0; motorA.move(motorA_Vel);
      RollSafeDel = 100; RollSafeMode++; break;
    case -1:  // reset state
      // ensure PID controller and motorA drive are off
      if (OpA_En) {Roll_OFF();}
      RollSafeMode++; RollSafeCnt = 0; break;
    case 0: // default state, waiting for action
      // remain in this state until the robot is placed face down for 1 sec
      // then goto state 1
      if (AcY < -7000) {
        // AcY is vertical looking down, so the Uni-Bot is resting
        // on its front balance wheel, so increment the face down counter
        if (RollSafeCnt < 100) {RollSafeCnt++;}
      } else {
        if (RollSafeCnt < 100) {RollSafeCnt = 0;} // picked up too early
        else {
          // the Uni-=Bot was face down for at least 1 sec, and now being raised
          // reset PID variables for viewing in 'Scope' mode
          Reset_Roll_PID();
          RollSafeMode++; set_LED_Task(1);
        }
      }
      break;
    case 1: // monitor angle for ready state
      // the robot has to be raised from the face down state
      // if not within the ReadyAng region then limit motor voltage to reduce heat
      if (motorAVol != 2.00) {motorAVol = 2.0; motorA.voltage_limit = getVolNow(motorAVol);}
      if (((fabs(PtchGyrAng) <= ReadyAng) || !Ptchen) && (fabs(RollGyrAng) <= ReadyAng)) {
        // turn PWM o/p back on
        digitalWrite(MotorAEn,HIGH);        // motor PWM is now ON
        // set the output for zero velocity initially
        motorA_Vel = 0.0; OpA_En = true;    // enable loopFOC
        RollSafeMode++;
      } break;
    case 2: // ready state, looks for near set-point start
      // check that we remain within the 'ready' region
      // if we aren't, then drop back into state 1, and reduce PWM voltage
      RollGo = false;
      if (((fabs(PtchGyrAng) > ReadyAng) && Ptchen) || (fabs(RollGyrAng) > ReadyAng)) {RollSafeMode = 1;}
      // now look for centre point
      else if ((RollGyrAng <= (RollStartAng + 0.3)) && (RollGyrAng >= (RollStartAng - 0.3))) {
        RollGo = true;
        // synchronise startup, roll and pitch must start together, if they are both enabled
        if (PtchGo || !Ptchen) {
          // switch to power-on mode
          RollGyrAng = RollAccAng;    // set gyro angle to match AcY angle
          motorA_Vel = 0.0;           // zero motor velocity
          RollSp = 0.0;               // reset any demands on the primary setpoint
          Roll_i_mem = 0;             // zero the i-gain memory
          RollLastDErr = 0.0;         // zero the last d-gain
          SbRollSp = RollStartAng;    // set Roll self-balancing set point to an initial angle
          RollSafeMode++;             // switch to power-up safemode
          SbRollCnt = 0;              // counter used in auto self-balance point adjustment
          SbRollSpInc = SbRollSpIncI; // set self-balance auto-increment
          PIDtask = 10;               // enable PID tasks with a value > 7
          motorAVol = motorAVolMax;   // set motor PWM voltage to max
          motorA.voltage_limit = getVolNow(motorAVol);
          OvrSpd = 0;                 // counter to generate ov erspeed warning
          RollPwr = 0.5;              // limit PID out to 50% initially
        }
      } break;
    case 3: // setpoint triggered, so ramp up PID output and transition to RollSafeMode = 4
      // at 10ms cycles, this takes 1 second from 50%
      if (RollPwr < 1.0) {RollPwr+= 0.005;} else {RollPwr = 1.0; RollSafeMode++;}
      break;
    case 4: // balancing state
      // test for toppling over
      if (fabs(RollAccAng) >= toppleAng) {
        // toppling so abort and reset both safe modes if in normal Demo mode
        PIDtask = 0;                // turn OFF the PID loop sequence
        RollGo = false;             // reset Roll PID active flag
        Task10ms = 0;               // reset the Demo mode task pointer
        if (Ptchen) {
          // both Pitch and Roll are enabled, so force both into initialisation states
          PtchSafeMode = -2; RollSafeMode = -2; set_LED_Task(1);
        } else {
          // since Pitch PID is disabled, it is assumed that we are trialing Roll balancing
          // so here we stop the PID and Roll motor, and goto SafeMode 1
          PIDtask = 0;              // turn OFF the PID loop sequence
          motorA_Vel = 0.0;         // stop the reaction wheel motor
          motorA.move(motorA_Vel);  // call SimpleFOC to stop the motor
          Reset_Roll_PID();         // clear the PID variables for plotting
          RollSafeMode = 1;         // return to looking for 'ready' state
          set_LED_Task(1);          // set LEDs to Demo mode
        }
        Turn_OFF();                 // stop turn motor if it was on
        moveDir = 0;                // reset direction flag
        changeGear(1);              // drop into bottom speed settings
        Display_Topple();           // display event
      }
      break;
  }
}

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

void RollSbInc(float zV) {
  // increment the Roll SbRollSpInc by zV
  // if the function is OFF then turn it on with minimum value
  // if the function is reduced beyond 0.000 then tuirn it OFF
  if (DmB != 0) {
    if (RollSb) {
            if (SbRollSpInc == 0.000) {RollSb = false;}
      else if (SbRollSpInc >= 0.000) {SbRollSpInc -= zV;}
    }
  } else {
         if (!RollSb) {RollSb = true; SbRollSpInc= 0.000;}
    else if (SbRollSpInc <= 9.999) {SbRollSpInc += zV;}
  }
  if (SbRollSpInc <= 0.000) {SbRollSpInc = 0.000;}   // value must be >= 0.000
  if (SbRollSpInc >= 9.999) {SbRollSpInc = 9.999;}   // value must be <= 9.999
  // if not balancing, we also set the initial value
  if (RollSafeMode != 4) {SbRollSpIncI = SbRollSpInc;} // keep initial increment in track}
}

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

void RollSinc(float zV) {
  // increment the Roll PID StartAng by zV
  if (DmB != 0) {if (RollStartAng >= -9.99) {RollStartAng -= zV;}}
  else {if (RollStartAng <= 9.99) {RollStartAng += zV;}}
  if (RollStartAng < -9.99) {RollStartAng = -9.99;}   // value must be >= -9.99
  if (RollStartAng > 9.99) {RollStartAng = 9.99;}     // value must be <= 9.99
}

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

void RollVdInc(float zV) {
  // increments the Roll VdMax by zV
  if (DmB != 0) {if (motorA_VdMax >= 0.01) {motorA_VdMax -= zV;}}
  else {if (motorA_VdMax <= 9.99) {motorA_VdMax += zV;}}
  if (motorA_VdMax < 0.00) {motorA_VdMax = 0.00;}   // value must be >= 0.00
  if (motorA_VdMax > 9.99) {motorA_VdMax = 9.99;}   // value must be <= 9.99
}

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

void Test_Mode() {
  // state functions used in Test mode
  if (TestDel > 0) {TestDel--; return;}

  switch (TestMode) {
    case -1: break; // null state
    case 1:
      //##########################################################################
      // initialise motor velocity test
      //##########################################################################
      ComTest = false; Cooler = false; VelTest = true;
      set_LED_Task(3); motorPnt = 0;
      motorA_Vel = 0.0; motorA.move(motorA_Vel);
      OpA_En = true; digitalWrite(MotorAEn,HIGH);
      motorAVol = 1.0; motorA.voltage_limit = getVolNow(motorAVol);  // maximum power_supply_voltage/2
      motorB_Vel = 0.0; motorB.move(motorB_Vel);
      OpB_En = true; digitalWrite(MotorBEn,HIGH);
      motorBVol = 1.0; motorB.voltage_limit = getVolNow(motorBVol);  // maximum power_supply_voltage/2
      motorC_Vel = 0.0; motorC.move(motorC_Vel);
      OpC_En = true; digitalWrite(MotorCEn,HIGH);
      motorCVol = 1.0; motorC.voltage_limit = getVolNow(motorCVol);  // maximum power_supply_voltage/2
      Motor_Freq = 333; TaskMs = 0;
      TestMode ++; break;
    case 2:
      // keep sending motor move commands
      if (Motor_Freq < 999) {
        t0 = micros(); motorA.move(motorA_Vel); t1 = micros() - t0;
        motorB.move(motorB_Vel);
        motorC.move(motorC_Vel);
      } break;
    case 3:
      // change the motor velocity to motorTgt
      switch (motorPnt) {
        case 0:
          if (motorATgt > motorA_Vel) {motorA_Vel += 0.5;}
          else if (motorATgt < motorA_Vel) {motorA_Vel -= 0.5;}
          if (fabs(motorATgt - motorA_Vel) < 0.5) {motorA_Vel = motorATgt; TestMode = 2;}
          break;
        case 1:
          if (motorBTgt > motorB_Vel) {motorB_Vel += 0.5;}
          else if (motorBTgt < motorB_Vel) {motorB_Vel -= 0.5;}
          if (fabs(motorBTgt - motorB_Vel) < 0.5) {motorB_Vel = motorBTgt; TestMode = 2;}
          break;
        case 2:
          if (motorCTgt > motorC_Vel) {motorC_Vel += 0.5;}
          else if (motorCTgt < motorC_Vel) {motorC_Vel -= 0.5;}
          if (fabs(motorCTgt - motorC_Vel) < 0.5) {motorC_Vel = motorCTgt; TestMode = 2;}
          break;
      } 
      // keep moving all of the motors
      motorA.move(motorA_Vel);
      motorB.move(motorB_Vel);
      motorC.move(motorC_Vel);
      break;
    case 4:
      // end motor velocity test, setting speed to zero and PWM voltage to 1.0v
      // this is a two step process to stop the wheels
      motorAVol = 1.0; motorA.voltage_limit = getVolNow(motorAVol);  // maximum power_supply_voltage/2
      motorBVol = 1.0; motorB.voltage_limit = getVolNow(motorBVol);  // maximum power_supply_voltage/2
      motorCVol = 1.0; motorC.voltage_limit = getVolNow(motorCVol);  // maximum power_supply_voltage/2
      motorA_Vel = 0.0; motorA.move(motorA_Vel);
      motorB_Vel = 0.0; motorB.move(motorB_Vel);
      motorC_Vel = 0.0; motorC.move(motorC_Vel);
      Motor_Freq = 100; TaskMs = 0;
      TestMode ++; TestDel = 5; break;
    case 5:
      // turn PWM o/p OFF
      OpA_En = false; digitalWrite(MotorAEn,LOW);         // motorA PWM is now OFF
      OpB_En = false; digitalWrite(MotorBEn,LOW);         // motorB PWM is now OFF
      OpC_En = false; digitalWrite(MotorCEn,LOW);         // motorC PWM is now OFF
      TestMode = -1; set_LED_Task(0);
      VelTest = false; motorPnt = -1;
      break;

    case 10:
      //##########################################################################
      // initialise motor commutate test
      //##########################################################################
      // initialise the commutation test
      ComTest = true; Cooler = false; VelTest = false;
      set_LED_Task(3); motorPnt = 0;
      motorA_Vd = 0.0; motorA_Vel = 0.0; motorA.move(motorA_Vel);
      motorAVol = 1.0; motorA.voltage_limit = getVolNow(motorAVol);  // maximum power_supply_voltage/2
      OpA_En = true; digitalWrite(MotorAEn,HIGH);
      motorB_Vd = 0.0; motorB_Vel = 0.0; motorB.move(motorB_Vel);
      motorBVol = 1.0; motorB.voltage_limit = getVolNow(motorBVol);  // maximum power_supply_voltage/2
      OpB_En = true; digitalWrite(MotorBEn,HIGH);
      motorC_Vd = 0.0; motorC_Vel = 0.0; motorC.move(motorC_Vel);
      motorCVol = 1.0; motorC.voltage_limit = getVolNow(motorCVol);  // maximum power_supply_voltage/2
      OpC_En = true; digitalWrite(MotorCEn,HIGH);
      Motor_Freq = 333; TaskMs = 0; TestMode++;
      break;
    case 11:
       motorA_Ph = 1;  motorB_Ph = 1; motorC_Ph = 1;
      if (ComType == 0) {TestMode = 12;} else {TestMode = 13;}
      break;
    case 12:
      // apply the acceleration term Vd to the motor Vel in a saw tooth fashion
      if (motorA_Vd >= 0.001) {
        switch (motorA_Ph) {
          case 1: motorA_Vel += motorA_Vd; if (motorA_Vel > motorA_VelMax) {motorA_Vel = motorA_VelMax; motorA_Ph = 0;} break;
          case 0: motorA_Vel -= motorA_Vd; if (motorA_Vel < -motorA_VelMax) {motorA_Vel = -motorA_VelMax; motorA_Ph = 1;} break;
        } motorA.move(motorA_Vel);
      } else {motorA_Vel = 0.00; motorA_Ph = 1; motorA.move(motorA_Vel);}
      if (motorB_Vd >= 0.001) {
        switch (motorB_Ph) {
          case 1: motorB_Vel += motorB_Vd; if (motorB_Vel > motorB_VelMax) {motorB_Vel = motorB_VelMax; motorB_Ph = 0;} break;
          case 0: motorB_Vel -= motorB_Vd; if (motorB_Vel < -motorB_VelMax) {motorB_Vel = -motorB_VelMax; motorB_Ph = 1;} break;
        } motorB.move(motorB_Vel);
      } else {motorB_Vel = 0.00; motorB_Ph = 1; motorB.move(motorB_Vel);}
      if (motorC_Vd >= 0.001) {
        switch (motorC_Ph) {
          case 1: motorC_Vel += motorC_Vd; if (motorC_Vel > motorC_VelMax) {motorC_Vel = motorC_VelMax; motorC_Ph = 1;} break;
          case 0: motorC_Vel -= motorC_Vd; if (motorC_Vel < -motorC_VelMax) {motorC_Vel = -motorC_VelMax; motorC_Ph = 0;} break;
        } motorC.move(motorC_Vel);
      } else {motorC_Vel = 0.00; motorC_Ph = 1; motorC.move(motorC_Vel);}
      break;
    case 13:
      // apply the acceleration term Vd in a start/stop fashion
      if (motorA_Vd >= 0.001) {
        switch (motorA_Ph) {
          case 1: motorA_Vel += motorA_Vd; if (motorA_Vel > motorA_VelMax) {motorA_Vel = motorA_VelMax; motorA_Ph = 2;} break;
          case 2: motorA_Vel -= motorA_Vd; if (motorA_Vel <= 0.00) {motorA_Vel = 0.00; motorA_Ph = 3; A_Cnt = 150;} break;
          case 3: A_Cnt--; if (A_Cnt < 1) {motorA_Ph = 4;} break;
          case 4: motorA_Vel -= motorA_Vd; if (motorA_Vel < -motorA_VelMax) {motorA_Vel = -motorA_VelMax; motorA_Ph = 5;} break;
          case 5: motorA_Vel += motorA_Vd; if (motorA_Vel >= 0.00) {motorA_Vel = 0.00; motorA_Ph = 6; A_Cnt = 150;} break;
          case 6: A_Cnt--; if (A_Cnt < 1) {motorA_Ph = 1;} break;
        } motorA.move(motorA_Vel);
      } else {motorA_Vel = 0.00; motorA_Ph = 1; motorA.move(motorA_Vel);}
      if (motorB_Vd >= 0.001) {
        switch (motorB_Ph) {
          case 1: motorB_Vel += motorB_Vd; if (motorB_Vel > motorB_VelMax) {motorB_Vel = motorB_VelMax; motorB_Ph = 2;} break;
          case 2: motorB_Vel -= motorB_Vd; if (motorB_Vel <= 0.00) {motorB_Vel = 0.00; motorB_Ph = 3; B_Cnt = 150;} break;
          case 3: B_Cnt--; if (B_Cnt < 1) {motorB_Ph = 4;} break;
          case 4: motorB_Vel -= motorB_Vd; if (motorB_Vel < -motorB_VelMax) {motorB_Vel = -motorB_VelMax; motorB_Ph = 5;} break;
          case 5: motorB_Vel += motorB_Vd; if (motorB_Vel >= 0.00) {motorB_Vel = 0.00; motorB_Ph = 6; B_Cnt = 150;} break;
          case 6: B_Cnt--; if (B_Cnt < 1) {motorB_Ph = 1;} break;
        } motorA.move(motorB_Vel);
      } else {motorB_Vel = 0.00; motorB_Ph = 1; motorB.move(motorB_Vel);}
      if (motorC_Vd >= 0.001) {
        switch (motorC_Ph) {
          case 1: motorC_Vel += motorC_Vd; if (motorC_Vel > motorC_VelMax) {motorC_Vel = motorC_VelMax; motorC_Ph = 2;} break;
          case 2: motorC_Vel -= motorC_Vd; if (motorC_Vel <= 0.00) {motorC_Vel = 0.00; motorC_Ph = 3; C_Cnt = 150;} break;
          case 3: C_Cnt--; if (C_Cnt < 1) {motorC_Ph = 4;} break;
          case 4: motorC_Vel -= motorC_Vd; if (motorC_Vel < -motorC_VelMax) {motorC_Vel = -motorC_VelMax; motorC_Ph = 5;} break;
          case 5: motorC_Vel += motorC_Vd; if (motorC_Vel >= 0.00) {motorC_Vel = 0.00; motorC_Ph = 6; C_Cnt = 150;} break;
          case 6: C_Cnt--; if (C_Cnt < 1) {motorC_Ph = 1;} break;
        } motorA.move(motorC_Vel);
      } else {motorC_Vel = 0.00; motorC_Ph = 1; motorA.move(motorC_Vel);}
      break;
    case 14:
      // end motor commutate test, setting speed to zero and PWM voltage to 1.0v
      // this is a two step process to stop the wheels
      motorAVol = 1.0;
      motorA.voltage_limit = getVolNow(motorAVol);  // maximum power_supply_voltage/2
      motorBVol = 1.0;
      motorB.voltage_limit = getVolNow(motorBVol);  // maximum power_supply_voltage/2
      motorCVol = 1.0;
      motorC.voltage_limit = getVolNow(motorCVol);  // maximum power_supply_voltage/2
      motorA_Vel = 0.0; motorA.move(motorA_Vel);
      motorB_Vel = 0.0; motorB.move(motorB_Vel);
      motorC_Vel = 0.0; motorC.move(motorC_Vel);
      Motor_Freq = 100;
      TestMode ++; TestDel = 5; break;
    case 15:
      // turn PWM o/p OFF
      OpA_En = false; digitalWrite(MotorAEn,LOW);         // motorA PWM is now OFF
      OpB_En = false; digitalWrite(MotorBEn,LOW);         // motorB PWM is now OFF
      OpC_En = false; digitalWrite(MotorCEn,LOW);         // motorC PWM is now OFF
      TestMode = -1; set_LED_Task(0);
      ComTest = false; motorPnt = -1; break;


    case 20:
      //##########################################################################
      // initialise motor cooler test
      //##########################################################################
      // initialise the motor cooler test
      ComTest = false; Cooler = true; VelTest = false;
      set_LED_Task(3); motorPnt = -1;
      motorA_Vel = 0.0; motorA.move(motorA_Vel);
      motorAVol = 2.0; motorA.voltage_limit = getVolNow(motorAVol);
      OpA_En = true; digitalWrite(MotorAEn,HIGH);
      motorB_Vel = 0.0; motorB.move(motorB_Vel);
      motorBVol = 2.0; motorB.voltage_limit = getVolNow(motorBVol);
      OpB_En = true; digitalWrite(MotorBEn,HIGH);
      motorC_Vel = 0.0; motorC.move(motorC_Vel);
      motorCVol = 2.0; motorC.voltage_limit = getVolNow(motorCVol);
      OpC_En = true; digitalWrite(MotorCEn,HIGH);
      Motor_Freq = 333; TaskMs = 0;
      TestMode ++; break;
    case 21:
      // ramp up to 3.0
      motorA_Vel+= 1.0; motorB_Vel+= 1.0; motorC_Vel+= 1.0;
      if (motorA_Vel >= 30.0) {TestMode ++;}
      TestDel = 8; break;
    case 22:
      // sustain for 3 seconds, then reverse
      TestMode ++; TestDel = 600; break;
    case 23:
      // reverse to -3.0
      motorA_Vel-= 1.0; motorB_Vel-= 1.0; motorC_Vel-= 1.0;
      if (motorA_Vel <= -30.0) {TestMode ++;}
      TestDel = 8; break;
    case 24:
      // sustain for 3 seconds then restart ramp up
      TestMode = 21; TestDel = 600; break;
    case 25:
      // remove drive and let the motors wind down
      OpA_En = false; digitalWrite(MotorAEn,LOW);         // motorA PWM is now OFF
      OpB_En = false; digitalWrite(MotorBEn,LOW);         // motorB PWM is now OFF
      OpC_En = false; digitalWrite(MotorCEn,LOW);         // motorC PWM is now OFF
      motorA_Vel = 0.0; motorA.move(motorA_Vel); motorAVol = 0.0; motorA.voltage_limit = motorAVol;
      motorB_Vel = 0.0; motorB.move(motorB_Vel); motorBVol = 0.0; motorB.voltage_limit = motorBVol;
      motorC_Vel = 0.0; motorC.move(motorC_Vel); motorCVol = 0.0; motorC.voltage_limit = motorCVol;
      TestMode = -1; set_LED_Task(0);
      Cooler = false; break;
  }
}

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

void toggleMotorCool() {
  // in Test mode toggle motor cooler test ON/OFF
  if (USB) {return;}

  if (Cooler) {
    // cooler test is already running, so stop it
    TestDel = 0; TestMode = 25;
  } else {
    // start cooler test
    TestDel = 0; TestMode = 20;
  }
}

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

void togglePtchSM() {
  // called when user clicks on Pitch SafeMode value in that display
  if (USB) {Ptchen = false; return;}

  if (!Ptchen) {
    // Pitch PID was disabled, so enable it
    set_LED_Task(3); PtchSafeMode = 0; Ptchen= true;
  } else {
    // if in SafeMode == 0 then got to SafeMOde = 1; else so turn off motor drives and disable Demo Safe Mode
    if (PtchSafeMode == 0) {PtchSafeMode = 1;}
    else {Ptchen = false; Pitch_OFF();}
  }
}

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

void toggleRollSbSp() {
  // called when user clicks on Roll SbRollSp ON/OFF flag in that display
    if (RollSb) {RollSb = false;}
  else {RollSb = true;}
}

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

void toggleRollSM() {
  // called when user clicks on Roll SafeMode value in that display
  if (USB) {Rollen = false; return;}

  if (!Rollen) {
    // Roll PID was disabled, so enable it
    set_LED_Task(3); RollSafeMode = 0; Rollen = true;
  } else {
    // if in SafeMode == 0 then got to SafeMOde = 1; else so turn off motor drives and disable Demo Safe Mode
    if (RollSafeMode == 0) {RollSafeMode = 1;}
    else {Rollen = false; Roll_OFF();}
  }
}

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

void TurnMaxInc(float zV) {
  // increment the Turn Max Acc by zV
  if (DmB != 0) {if (TurnMaxVd > TurnMinVd) {TurnMaxVd -= zV;}}
  else {if (TurnMaxVd <= 9.99) {TurnMaxVd += zV;}}
  if (TurnMaxVd < TurnMinVd) {TurnMaxVd = TurnMinVd;} // value must be >= TurnMinVd
  if (TurnMaxVd > 9.999) {TurnMaxVd = 9.999;}         // value must be <= 9.999
}

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

void TurnMinInc(float zV) {
  // increment the Turn Min Acc by zV
  if (DmB != 0) {if (TurnMinVd >= 0.000) {TurnMinVd -= zV;}}
  else {if (TurnMinVd <= TurnMaxVd) {TurnMinVd += zV;}}
  if (TurnMinVd < 0.000) {TurnMinVd = 0.000;}         // value must be >= 0.000
  if (TurnMinVd > TurnMaxVd) {TurnMinVd = TurnMaxVd;} // value must be <= 9.999
}

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

void Turn_OFF() {
  // called to completely turn off the B motor
  motorBVol = 0.0;                    // zero PWM voltage
  motorB.voltage_limit = motorBVol;   // set PWM voltage limit
  motorB_Vel = 0.0;                   // zero motor velocity
  motorB.move(motorB_Vel);            // stop the motor function
  OpB_En = false;                     // disable motor o/p and FOC
  digitalWrite(MotorBEn,LOW);         // motor PWM is OFF
  Turn = 0;                           // reset turning direction flag
}

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

void voltageInc(float zV) {
  // increment the motor voltage by zV
  if (motorPnt < 0) {return;}

  switch (motorPnt) {
    case 0:
      if (DmB != 0) {if (motorAVol >= 0.0) {motorAVol -= zV;}}
      else {if (motorAVol <= 6.0) {motorAVol += zV;}}
      if (motorAVol < 0.00) {motorAVol = 0.00;}   // value must be >= 0.0
      motorA.voltage_limit = getVolNow(motorAVol); break;
    case 1:
      if (DmB != 0) {if (motorBVol >= 0.0) {motorBVol -= zV;}}
      else {if (motorBVol <= 6.0) {motorBVol += zV;}}
      if (motorBVol < 0.00) {motorBVol = 0.00;}   // value must be >= 0.0
      motorB.voltage_limit = getVolNow(motorBVol); break;
    case 2:
      if (DmB != 0) {if (motorCVol >= 0.0) {motorCVol -= zV;}}
      else {if (motorCVol <= 6.0) {motorCVol += zV;}}
      if (motorCVol < 0.00) {motorCVol = 0.00;}   // value must be >= 0.0
      motorC.voltage_limit = getVolNow(motorCVol); break;
  }
}

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

