OpenGradeSim.ino

/*
  Original Author: OpenGradeSimulator by Matt Ockendon 2019.11.14. See https://github.com/mockendon/opengradesim
  This branch by Brian Palmer 2020.4.6 at https://github.com/BrianFrankPalmer/opengradesim

    ____                  _____               __      ____ ____ __  ___
   / __ \ ___  ___  ___  / ___/____ ___ _ ___/ /___  / __//  _//  |/  /
  / /_/ // _ \/ -_)/ _ \/ (_ // __// _ `// _  // -_)_\ \ _/ / / /|_/ /
  \____// .__/\__//_//_/\___//_/   \___/ \___/ \__//___//___//_/  /_/
       /_/
  "This is the controller for a 3D printed elevation or 'grade' simulator to use with an indoor trainer.
  The project in inspired by the Wahoo Kickr Climb but shares none of its underpinnings.
  Elevation is simulated on an indoor trainer by increasing resistance over that generated by frictional
  losses." - Matt Odendon

  I was inspired to build Matt Odendon's cool opengradesim project and ended up coming up with this branch
  in the process. This version addresses a couple issues on his to-do list and adds a few other nicities.

  - A manual mode was added. Pressing the up or down button switches to manual mode. Pressing the stop button returns to smart trainer mode.
  - The blocking motor control routine was replaced with a PID controller. Now the loop() runs uninterrupted allowing the target grade
  to be constantly adjusted. This allows for accurate grade simulations (if your not peddling).
  - Peddle vibration injecting unwanted noise into the control loop was handled by locking the position once a target grade
  is achieved. It's only unlocked when the REQUESTED grade has changed by n% or more. The requested grade comes from either manual input,
  Matt's calculated grade routine,  or serial console input (+, -) during debugging/testing. But does not include the Nano's noisy sensor incline data.
  So you can now shift your weight, peddle, bang on your bike, etc. and the climber wont move unless the requested grade changes.
  - A bicolor LED was added. It turns red when the the actuator is moving and green when its locked on a target grade.
  - Now allows for negative grades.
  - Added a climber leveling mode. This mode both physically levels the bike to 0% incline and sets the controller grade
  display to 0%. While in this mode, the up and down buttons are used adjust the bikes incline to level. Once you have leveled the trainer, pressing
  the stop button stores the current incline to flash memory. This value is used to automatically re-level
  the bike the next time gradeSim is started. So now you dont have to physically move the control head to level the climber and the control
  head can be mounted at an angle.

  - Added UI Setting menu that allows setting:
      Units: (imperial/metric)
      Rider/Bike Weight: combined bike/rider weight in kg or lbs
      Wheel Size: defaults to 2070 mm
      Manual Step %: Controls sensivity of manual incline/decline buttons.
      Grade Accuracy: Controls precision of grade adjustments.
      Leveling Mode: Controls when the leveling mode is entered (each time GradeSim is started or first time only).
      PID parameters: All setting of motor control loop parameters.
      Debug Mode: When On, allows input through Serial console.
      Rebooting:
      Lowering Trainer:

  - Finished Matt's work on storage of user settings. This could easily be expanded store multiple rider/bike profiles.
  - Added a battery level display that displays the battery level of the <CABLE> device.
  - Added a IMU temperature sensing and display.  This was achieved by upgrading to the LSM6DS3 driver. See
  https://github.com/arduino-libraries/Arduino_LSM6DS3/issues/9. Uncomment the display lines to use.

  The circuit: This is basically like Matt's circuit except -
  - I use a bigger actuator that requires a bigger motor driver. After experementing with a few different boards I ended up using a
  Sabertooth 3x32 that I had from a previous project. They are pricey, but worth it. It can be controlled with 3V signal eliminating the
  need for a logic level shifter, can be controlled with just two wires, and COMPLETELY ELIMINATED THE MOTOR WHINE AT ALL SPEEDS.
  (Update - this was a bad idea. The sabertooth is really designed to be powered by a battery or a PSU. I couldnt get it to run for more than a
  few minutes off a wall wart.)
  - Uses a 3 button pad instead of 2.
  - Added a bi-color LED to indicate state.
  - Added 3 POTS for storing initial PID param values. I ended up not using these for anything because the PID values are stored in ROM and can
  be be adjusted in Settings -> PID values.
  - Uses a Serial cable connector between the control box and the motor driver. I had planned to swap this out for a coiled 6-pin RJ-12 cable.
  But after tripping over the serial cable a few times without causing any damage, I think I will stick with it.

  issues: PID err rate does not grow over time when not addressed


*/

#include <FlashStorage.h>
#include <MovingAverageFilter.h>
#include <ArduinoBLE.h> // bluetooth low energy
#include <Arduino_LSM6DS3.h> // wifi, accelerometer, and gyroscope sensor
#include <Sabertooth.h> // sabertooth motor driver
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <PID_v1.h>
#include "Defines.h"
#include "Types.h"
#include "PushButton.h"
#include "OLEDDisplay.h"
#include "MyMenu.h"

Sabertooth ST(128); // The Sabertooth is on address 128.

// Declare our filters
int accelMvgAvgLen = 11; // orig = 9
MovingAverageFilter movingAverageFilter_x(accelMvgAvgLen); //
MovingAverageFilter movingAverageFilter_y(accelMvgAvgLen); // Moving average filters for the accelerometers
MovingAverageFilter movingAverageFilter_z(accelMvgAvgLen); //
int powerMvgAvgLen = 6; // orig = 8;
// 8 = 2 second power average at 4 samples per sec
// 6 = 2 second power average at 3 samples per sec
// 4 = 2 second power average at 2 samples per sec
// 2 = 2 second power average at 1 samples per sec
MovingAverageFilter movingAverageFilter_power(powerMvgAvgLen); // 8 = 2 second power average at 4 samples per sec
int speedMvgAvgLen = 2; // orig = 2
MovingAverageFilter movingAverageFilter_speed(speedMvgAvgLen); // 2 = 0.5 second speed average at 4 samples per sec

long previousMillis = 0; // last time in ms
float smoothRadPitch = 0; // variable for the pitch
double trainerIncline = 0; // variable for the % trainerIncline (actual per accelerometers)

bool trainerLeveled = false;
bool onTargetGrade = false;
double inputGrade = 0;

// motor pid params
double PID_motor_kp = 2.0, PID_motor_ki = 0.07, PID_motor_kd = 0.3;  // PID parms
double PID_trainerInclineErr = 0; // input
double PID_motorPWM = 0; // output
double PID_targetGrade = 0; // setpoint

int PWMStallPoint = 10; // drill motor. Change as needed.

double prev_SaberSpeed = 0;
//PID (&Input,&Output, &Setpoint, Kp, Ki, Kd, Mode)
PID motorPID(&PID_trainerInclineErr, &PID_motorPWM, &PID_targetGrade, PID_motor_kp, PID_motor_ki, PID_motor_kd, DIRECT);

TrainerMode trainerMode = LevelTrainer; // start in leveling mode.

// user settings

double riderWeight;
int wheelCircMM;
double trainerInclineZeroOffset;
double trainerErrSensitivity;
double manAdjPcnt;
bool displayUnits;
bool dimmer;
LevelingMode levelingMode;

// TODO: convert to array of user profiles. Add bikeID and Desc to allow multiple user/bike profiles.
typedef struct {
  bool valid;
  int riderWeight; // combined rider and bike weight
  int wheelCircMM; // wheel circumference in milimeters
  double trainerInclineZeroOffset; // trainer incline zero offset adjusted by user when leveling trainer
  double trainerErrSensitivity; // how close to target angle is close enough
  double manAdjPcnt; // % grade to move in manual mode when a direction button is pressed
  bool displayUnits; // controls formatting of weight and speed (Imperial=0 or Metric=1)
  bool dimmer; // controls brightness of display (0=off, 1=on)
  LevelingMode levelingMode; // controls when should leveling routine run ( EveryTime=0, FirstTime=1 )
  double PID_motor_kp; // motor proportional
  double PID_motor_ki; // integral
  double PID_motor_kd; // derivitive
} UserSettings;

UserSettings userSettings;

// Reserve a portion of flash memory to store a "UserSetting" and
// call it "userSettings_FlashStore".
FlashStorage(userSettings_FlashStore, UserSettings);

int powerTrainer = 0;             // variable for the power (W) read from bluetooth
int speedTrainer = 0;             // variable for the speed (kph) read from bluetooth
float speedMpersec = 0;           // for calculation
float resistanceWatts = 0;        // for calculation
float powerMinusResistance = 0;   // for calculation

// For power and speed declare some variables and set some default values
long PrevWheelRevs;                  // For speed data set 1
long PrevWheelEvntTime;                      // For speed data set 1
long currWheelRevs;                  // For speed data set 2
long CurrWheelEvntTime;                      // For speed data set 2

int speedKMH;                     // Calculated speed in KM per Hr
int PrevSpeedKMH;

// BLE peripheral and characteristics
BLEDevice cablePeripheral;
BLECharacteristic speedCharacteristic;
BLECharacteristic powerCharacteristic;
BLECharacteristic batteryCharacteristic;
uint8_t batteryLevel[1];

///////////////////////////////// Setup ///////////////////////////////////////

void setup() {
  Serial.begin(9600);
  delay(2000);
  // Init LED
  pinMode(RED_LED_PIN, OUTPUT);             // sets the digital pin as output
  pinMode(COMMON_HIGH_LED_PIN, OUTPUT);     // sets the digital pin as output
  pinMode(GREEN_LED_PIN, OUTPUT);           // sets the digital pin as output
  digitalWrite(COMMON_HIGH_LED_PIN, HIGH);  // LED common is high.
  biColorLED(true, false);

  // init motor controller
  saberToothSetup();

  // setup a pin connected to RST (A5, pin 19) to pull reset low if reset is required
  pinMode (RESET_PIN, OUTPUT);
  digitalWrite (RESET_PIN, HIGH);

  // init OLED display
  initOLED();

  // Check that the accelerometer is up and running else reset
  if (!IMU.begin()) {
    resetSys(F("IMU Failure!"));
  }

  initUserSettings();

  //turn the motor PID controller on
  motorPID.SetMode(AUTOMATIC);
  //motorPID.SetOutputLimits(-127, 127); // set to 1/2 the saberTooth serial range. (forward range only)
  motorPID.SetOutputLimits(0, 127); // set to 1/2 the saberTooth serial range. (forward range only)

  motorPID.SetTunings(PID_motor_kp, PID_motor_ki, PID_motor_kd);
  //
  //  // begin BLE initialization reset if fails
  //  if (!BLE.begin()) {
  //    resetSys(F("BLE failed!"));
  //  }
  //
  //  // assign event handlers for connected, disconnected to peripheral
  //  BLE.setEventHandler(BLEConnected, getSubscribedtoSensor);
  //  BLE.setEventHandler(BLEDisconnected, blePeripheralDisconnectHandler);
  //

  biColorLED(true, true); // red to green to indicate init. is complete.
  myMenu.setCurrentMenu(&mainMenuList);
}
bool Debugging = false;

////////////////////////////////  loop  ///////////////////////////////////////

void loop() {

  long currentMillis = millis();

  if (currentMillis - previousMillis >= 100)
  {
    previousMillis = currentMillis;
    checkButtons();
    myMenu.doMenu();
    doDisplay();
  }
}

////////////////////////   method declarations  ///////////////////////////////

boolean gradeSim() {
  static bool firstLoop = true;
  static bool trainerLeveled = false;

  static long previousSpeedandPowerMillis = 0; // the last time we queried the SmartTrainer for Speed and Power
  static double prevTargetGrade = 0;

  trainerIncline = findTrainerIncline();

  // handle select button

  if (selectBtnPressed())
  {
    Serial.println("select Btn Pressed");
    switch (trainerMode)
    {
      case LevelTrainer:
        if (firstLoop)
        {
          Serial.println("firstLoop");
          firstLoop = false;
          // set target incline as current location if offset has never been set, otherwise use the stored offset
          //prevTargetGrade = inputGrade = trainerInclineZeroOffset == 0 ? trainerIncline : trainerInclineZeroOffset;
          if (trainerInclineZeroOffset == NULL)
          {
            prevTargetGrade = inputGrade = trainerIncline;
            Serial.print("using current trainer incline ("); Serial.print(trainerIncline); Serial.println(") as target");
          } else {
            inputGrade = trainerInclineZeroOffset;
            Serial.print("using stored trainer incline ("); Serial.print(trainerInclineZeroOffset); Serial.println(") as target");
          }
          return false; // skipping first cycle to ignore the selectBtnPressed that got us here.
        }
        trainerInclineZeroOffset = trainerIncline;
        updateUserSettings();
      case Manual:
        trainerMode = SmartTrainer; // switching to smartTrainer mode
        break;
      case SmartTrainer:
        if (levelingMode == FirstTime && firstLoop) // ignore the first selectBtnPressed() if LevelingMode = FirstTime
        {
          firstLoop = false;
          return false;
        }

        // select btn pressed while in smartTrainer mode. Set stage for reentry.
        trainerMode = levelingMode == FirstTime ? SmartTrainer : LevelTrainer; // optionally restart in leveling mode the next time gradesim is started.
        firstLoop = true;
        prev_SaberSpeed = 0; // reset for next time
        prevTargetGrade = 0; // reset for next time
        //lowerActuator(); //delay(5000); // allow time for actuator to lower.

        stopActuator(); // turn off motor
        return true; // exit gradeSim
    }
  }
  //
  //  if (!cablePeripheral.connected() && !Debugging) {
  //    getBLEServices();
  //  } else {
  //    if (cablePeripheral.connected() && Debugging) {
  //      cablePeripheral.disconnect();
  //    }
  //  }

  if (Debugging)
  {
    //    powerTrainer = movingAverageFilter_power.process(210);
    //    speedTrainer = movingAverageFilter_speed.process(15);
    serialReceive();
  }

  // get input
  switch (trainerMode)
  {
    case LevelTrainer:
      setDouble(inputGrade, -90, 90, .25); // check for manual changes in grade
      break;
    case Manual:
      setDouble(inputGrade, -90, 90, manAdjPcnt); // check for manual changes in grade
      break;
    case SmartTrainer:
      if (upDownBtnPressed())
      {
        trainerMode = Manual; // switching to manual mode
        return false;
      }
      calculateTargetGrade(); // Use power and speed to calculate the grade
      inputGrade = trainerInclineZeroOffset + inputGrade;
      break;
  } // end switch

  //PID_targetGrade = round(inputGrade); // round to nearest whole number to limit grade changes to 1% or more.
  //PID_targetGrade = inputGrade; This is a no no.

  // Only respond to PID_targetGrade changes if we are already locked on the previous position.
  if (onTargetGrade && (prevTargetGrade != inputGrade))
  {
    prevTargetGrade = inputGrade;
    onTargetGrade = false;
  }

  if (!onTargetGrade)
  {
    moveActuator(); // Compute PWM and apply to motor
  }

  gradeSimDisplay(); // Display the current data

  return false;
}

void moveActuator(void)
{
  double err = PID_targetGrade - trainerIncline;
  PID_trainerInclineErr = -abs(err); // make err negative if it isnt already.

  int SaberSpeed = 0;

  if (PID_trainerInclineErr < -abs(trainerErrSensitivity)) // Are we too far away from target?
  {
    if (PID_trainerInclineErr < -2.00) {
      motorPID.SetTunings(abs(err), PID_motor_ki, PID_motor_kd); // Self adjusting mode - The speed of correction is equal to the incline error.
    } else {
      if (PID_trainerInclineErr < -1.0) {
        motorPID.SetTunings(1.5, PID_motor_ki, PID_motor_kd); // getting close. take over control of speed and slow down so we dont miss it.
      } else {
        motorPID.SetTunings(1.0, PID_motor_ki, PID_motor_kd); // even slower...
      }
    }
    //Serial.println("computing PWM");
    biColorLED(true, false); // red
    motorPID.Compute(); // Use targetGrade and current trainer angle to calc the motor pwm value.
    SaberSpeed = PID_motorPWM;

  } else {
    Serial.println("target achieved. stopping.");
    biColorLED(true, true); // green
    SaberSpeed = 0;
    onTargetGrade = true;
    ST.motor(1, SaberSpeed);
    return;
  }

  if (SaberSpeed != prev_SaberSpeed)
  {
    prev_SaberSpeed = SaberSpeed;
    //    if (abs(SaberSpeed) < 23) { // stop the motor if SaberSpeed is too small to move the actuator.
    //      SaberSpeed = 0;
    //    }

    if (trainerIncline > PID_targetGrade) {
      SaberSpeed = -abs(SaberSpeed); // flip direction if trainer is below target
    }

    ST.motor(1, SaberSpeed);
  }

  Serial.print("PID_targetGrade (setpoint):");  Serial.print(PID_targetGrade);
  Serial.print(" PID_trainerInclineErr (input):");  Serial.print(PID_trainerInclineErr);
  Serial.print(" PID_motorPWM (output):");  Serial.print(PID_motorPWM);
  Serial.print(" SaberSpeed (output):");  Serial.print(SaberSpeed);
  Serial.print(" p:");  Serial.print(motorPID.GetKp());
  Serial.print(", i:"); Serial.print(motorPID.GetKi());
  Serial.print(", d:"); Serial.print(motorPID.GetKd());
  Serial.println();

}

void stopActuator(void) {
  ST.motor(1, 0);
}
void raiseActuator(void) {
  ST.motor(1, 127);
}
void lowerActuator(void) {
  ST.motor(1, -127);
}

bool lowerTrainer()
{
  // Purpose: This non-blocking routine to lower linear acuator and shut-off motor ASAP. Will return true
  // soon after the actuator is stopped by the limit switches. It does this by detecting
  // when the grade changes have stopped for WAIT_FOR_ACTUATOR_STOP_MIL.
  // Usage - Call from your control loop until it returns true.

  static bool firstLoop = true;
  static double prevIncline = 0;
  static long previousMillis = 0;
  long currentMillis = millis();
  char buf[70];

  if (firstLoop)
  {
    firstLoop = false;
    prevIncline = trainerIncline;
    Serial.println("lowering actuator...");
    lowerActuator(); // start lowering the trainer
    sprintf_P(buf, PSTR("lowering..."), trainerIncline);
    displayLineLeft(1, 12, 1, buf);
    displayLineLeft(2, 24, 1, " "); // erase the unused line
  } else {
    if (prevIncline != trainerIncline) { // still moving down so reset
      Serial.println("still lowering...");
      prevIncline = trainerIncline;
      previousMillis = currentMillis;
    } else {
      // stopped or possibly havent started moving yet
      if (currentMillis - previousMillis >= WAIT_FOR_ACTUATOR_STOP_MIL) // have been stopped for WAIT_ACTUATOR_STOP_MIL. fini!
      {
        Serial.print(currentMillis - previousMillis);
        Serial.print(" >= ");
        Serial.println(WAIT_FOR_ACTUATOR_STOP_MIL);
        Serial.println("done lowering");
        //trainerInclineZeroOffset = trainerIncline; // zero the display
        PID_targetGrade = trainerIncline; // initialize the starting point grade
        previousMillis = 0; // reset stuff for next time.
        firstLoop = true;
        return true;
      }
    }
  }
  return false;
}

double findTrainerIncline() {
  float rawx, rawy, rawz;
  float x, y, z;
  double trainerIncline = 0;

  if (IMU.accelerationAvailable()) {
    IMU.readAcceleration(rawx, rawy, rawz);

    x = movingAverageFilter_x.process(rawx);      //
    y = movingAverageFilter_y.process(rawy);      //   Apply moving average filters to reduce noise
    z = movingAverageFilter_z.process(rawz);      //

    //    char buf[80];
    //    sprintf_P(buf, PSTR("IMU x:%d y:%d z:%d"), x, y, z);

    // find pitch in radians
    float radpitch = atan2(( y) , sqrt(x * x + z * z));

    smoothRadPitch = radpitch;

    // find the % grade from the pitch
    trainerIncline = tan(smoothRadPitch) * 100;

    trainerIncline = trainerIncline * -1; // flip the sign since its mounted with the USB port on the left.
  }

  //  char buf[5];
  //  sprintf_P(buf, PSTR("findTrainerIncline: %d"), trainerIncline);
  //  Serial.println(buf);

  return trainerIncline;

}

void calculateTargetGrade(void) {
  float speed28 = pow(speedTrainer, 2.8);                                             // pow() needed to raise y^x where x is decimal
  resistanceWatts = (0.0102 * speed28) + 9.428;                                       // calculate power from rolling / wind resistance
  powerMinusResistance = powerTrainer - resistanceWatts;                              // find power from climbing
  //Serial.print("powerMinusResistance:");Serial.println(powerMinusResistance);

  speedMpersec = speedTrainer / 3.6;                                                  // find speed in SI units. 1 meter / second (m/s) is equal 3.6 kilometers / hour (km/h)
  if (speedMpersec == 0)
  {
    inputGrade = 0;
  }
  else
  {
    inputGrade = ((powerMinusResistance / (riderWeight * 9.8)) / speedMpersec) * 100; // calculate grade of climb in %
  }

  // Limit upper and lower grades
  if (inputGrade < -10) {
    inputGrade = -10;
  }
  if (inputGrade > 20) {
    inputGrade = 20;
  }
}

void gradeSimDisplay()
{
  displayLineLeft(0, 20, 1, " "); // erase line 0
  displayLineLeft(1, 20, 1, " "); // erase line 1
  displayLineLeft(2, 20, 1, " "); // erase line 2

  // --   row 1 --
  //displayTextLeft( row,  rowPos,  startcol,  colwidth,  textsize, message )
  char buf[7];

  // watts, kph/mph
  if (cablePeripheral.connected() || Debugging) {
    sprintf_P(buf, PSTR("%d Watts"), powerTrainer);
    displayTextLeft (0, 0, 0, 8, 1, buf);

    if (displayUnits == 0)
    {
      int mph = speedTrainer / 1.609344; // mph conversion
      sprintf_P(buf, PSTR("%d mph"), mph);
    }
    else
    {
      sprintf_P(buf, PSTR("%d kph"), speedTrainer);
    }

    displayTextRight(0, 0, 20, 7, 1, buf);
  } else {

    displayTextLeft (0, 0, 0, 8, 1, "-- Watts");
    if (displayUnits == 0)
    {
      displayTextRight(0, 0, 20, 7, 1, "-- mph");
    } else {
      displayTextRight(0, 0, 20, 7, 1, "-- kph");
    }

  }

  // --   row 2  --  Display current trainerIncline centered and 2X-scale text--
  //  int tGrade = (int) PID_targetGrade - trainerInclineZeroOffset;
  //  sprintf_P(buf, PSTR("%d%%"), tGrade); // Display target grade centered and 2X-scale text
  //  displayTextRight (1, 9, 6, 7, 2, buf);

  double adjActualIncline = trainerIncline - trainerInclineZeroOffset;
  int myint = adjActualIncline;
  int myfraction = abs((adjActualIncline -  myint) * 100);
  sprintf_P(buf, PSTR("%d.%02d"), myint, myfraction);
  //( row, rowPos, startcol, colwidth, textsize, message )
  displayTextRight (1, 9, 7, 6, 2, buf);

  // --   row 3 --
  //nn% Bat Level nn.nn%

  // -- display battery level
  //  if (cablePeripheral.connected() || Debugging) {
  //    sprintf_P(buf, PSTR("%d%% Bt"), batteryLevel[0]);
  //  } else {
  //    sprintf_P(buf, PSTR("---%% Bt"), 100);
  //  }

  // -- display IMU temp
  //sprintf_P(buf, PSTR("%d C"), IMU_Temperature);

  // display pwm with 2 decimal polaces
  myint = PID_motorPWM;
  myfraction = abs((PID_motorPWM -  myint) * 100);
  //sprintf_P(buf, "%d.%01d M", myint, myfraction);
  sprintf_P(buf, "%d M", myint);

  //displayTextLeft( row, rowPos, startcol, colwidth, textsize, message )
  displayTextLeft (2, 24, 0, 11, 1, buf);

  // Display smartTrainer target grade with 2 decimal places bottom right
  double adjTargetIncline = inputGrade - trainerInclineZeroOffset;
  myint = adjTargetIncline;
  myfraction = abs((adjTargetIncline -  myint) * 100);

  switch (trainerMode) {
    case 0: // trainerLevel Mode
      sprintf_P(buf, PSTR("Level %d.%02d"), myint, myfraction);
      break;
    case 1: // manual mode
      sprintf_P(buf, PSTR("Manual %d.%02d"), myint, myfraction);
      break;
    case 2:
      sprintf_P(buf, PSTR("GradeSim %d.%02d"), myint, myfraction);
      break;
  }
  //void displayTextRight( row, rowPos, startcol, colwidth, textsize, message)
  displayTextRight(2, 24, 20, 13, 1, buf);

}

void blePeripheralConnectHandler(BLEDevice central) {
  // central connected event handler
  Serial.print("Connected event, central: ");
  Serial.println(central.address());
}

void blePeripheralDisconnectHandler(BLEDevice central) {
  // central disconnected event handler
  Serial.print("Disconnected event, central: ");
  Serial.println(central.address());
}

void getBLEServices() {

  //displayLineLeft(1, 12, 0, F("Bluetooth scanning"));
  //displayLineLeft(2, 24, 1, F("for ((CABLE)) Device"));
  //displayLineLeft(2, 24, 2, F(" ")); // erase the unused line
  //doDisplay();
  static String PeriphUUID = "";

  if (PeriphUUID != "") {
    // start scanning for the peripheral we were already connected to.
    Serial.println("scanForUuid:" + PeriphUUID); //
    BLE.scanForUuid(PeriphUUID);
  } else {
    //    Serial.println("Scanning for ((CABLE)) device");
    BLE.scan(); // Scan or rescan for BLE services
  }

  // check if a peripheral has been discovered and allocate it
  cablePeripheral = BLE.available();

  if (cablePeripheral) {
    // discovered a peripheral, print out address, local name, and advertised service
    Serial.print("discovered ");
    Serial.print(cablePeripheral.address());
    Serial.print(" '");
    Serial.print(cablePeripheral.localName());
    Serial.print("' ");
    Serial.print(cablePeripheral.advertisedServiceUuid());
    Serial.println();

    if (cablePeripheral.localName() == ">CABLE") {
      //if (cablePeripheral.localName() == "KICKR CORE 5043") { // 0x2A63
      //if (cablePeripheral.localName() == "Cycling Speed and Cadence") {
      // stop scanning
      BLE.stopScan();
      Serial.println("found " + cablePeripheral.localName() + " (UUID:" +
                     cablePeripheral.advertisedServiceUuid() + ") device. scan stopped");
      PeriphUUID = cablePeripheral.advertisedServiceUuid();
      cablePeripheral.connect();
    }
  }
}

void getSubscribedtoSensor(BLEDevice central) {

  // discover Cycle Speed and Cadence attributes
  //Serial.println("Discovering Cycle Speed and Cadence service ...");
  if (cablePeripheral.discoverService("1816")) {
    Serial.println("Cycle Speed and Cadence Service discovered");
  } else {
    Serial.println("Cycle Speed and Cadence Attribute discovery failed.");
    cablePeripheral.disconnect();

    //resetSystem();
    return;
  }

  // discover Cycle Power attributes
  //Serial.println("Discovering Cycle Power service ...");
  if (cablePeripheral.discoverService("1818")) {
    Serial.println("Cycle Power Service discovered");
  } else {
    Serial.println("Cycle Power Attribute discovery failed.");
    cablePeripheral.disconnect();
    //resetSystem();
    return;
  }

  // discover Battery Level attributes
  if (cablePeripheral.discoverService("180f")) {
    Serial.println("Battery Service discovered");
  } else {
    Serial.println("Battery Service Attribute discovery failed.");
    //cablePeripheral.disconnect(); not critical
    //return;
  }

  // retrieve the characteristics

  speedCharacteristic = cablePeripheral.characteristic("2A5B");
  powerCharacteristic = cablePeripheral.characteristic("2A63");
  batteryCharacteristic = cablePeripheral.characteristic("2A19");

  // subscribe to the characteristics
  // Note authentication is not supported on ArduinoBLE library v1.1.2. This
  // is why this project uses the Cable device that does not require auth.

  //-- cycle speed --
  speedCharacteristic.setEventHandler(BLEWritten, refreshSpeed);  // assign event handler to handle updates
  if (!speedCharacteristic.subscribe()) {
    Serial.println("can not subscribe to speed");
    cablePeripheral.disconnect();
    return;
  } else {
    Serial.println("subscribed to speed");
  }

  // --- cycle power ---
  powerCharacteristic.setEventHandler(BLEWritten, refreshPower);  // assign event handler to handle updates
  if (!powerCharacteristic.subscribe()) {
    Serial.println("can not subscribe to power");
    cablePeripheral.disconnect();
    return;
  } else {
    Serial.println("subscribed to power");
  };

  // --- battery level ---
  batteryCharacteristic.setEventHandler(BLEWritten, refreshBatteryLevel);  // assign event handler to handle updates
  if (!batteryCharacteristic.subscribe()) {
    Serial.println("can not subscribe to battery level");
    // not critical cablePeripheral.disconnect();
  } else {
    Serial.println("subscribed to battery level");
    batteryCharacteristic.readValue(batteryLevel, 1); // Initialize battery level display value
  };

}

void refreshSpeed(BLEDevice central, BLECharacteristic theCharacteristic) {
  // Speed = (Difference in two successive Wheel Revolution values) / (Difference in two successive Last Wheel Event Time values)
  // CSCFeature Flags see https://www.bluetooth.com/specifications/specs/cycling-speed-and-cadence-service-1-0/
  // This allows the Collector to determine whether or not the following fields are present:
  // bit 0: Wheel Revolution Data Present bit
  // bit 1: Crank Revolution Data Present bit
  // bit 2-15: reserved
  // Verified with LightBlue that once started, these BLE notifications continue to transmit the last values even after the
  // trainer has stopped.

  if (theCharacteristic.valueUpdated()) {

    //  This value needs a 16 byte array
    uint8_t holdvalues[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} ;

    //  But I'm only going to read the first 7
    if (theCharacteristic.readValue(holdvalues, 7))
    {

      byte flagsFields = holdvalues[0];     // binary flags 8 bit int; 1 = wheel data present, 2 = crank only? I havent observed this state), 3 = wheel and crank data present
      byte rawValue1 = holdvalues[1];       // revolutions least significant byte in HEX
      byte rawValue2 = holdvalues[2];       // revolutions next most significant byte in HEX
      byte rawValue3 = holdvalues[3];       // revolutions next most significant byte in HEX
      byte rawValue4 = holdvalues[4];       // revolutions most significant byte in HEX
      byte rawValue5 = holdvalues[5];       // time since last wheel event least sig byte in HEX
      byte rawValue6 = holdvalues[6];       // time since last wheel event most sig byte in HEX

      if (flagsFields & 0x01) { // bitwise AND to check if wheel data is present

        long currWheelRevs = (rawValue1 + (rawValue2 * 256) + (rawValue3 * 65536) + (rawValue4 * 16777216));

        CurrWheelEvntTime = (rawValue5 + (rawValue6 * 256));

        if (PrevWheelEvntTime > 0) // not the first wheel event notification
        {

          // Find distance difference in mm and convert to km
          float distanceTravelled = ((currWheelRevs - PrevWheelRevs) * wheelCircMM);
          float kmTravelled = distanceTravelled / 1000000;

          // Find time in 1024ths of a second and convert to hours.
          // The specs say the Wheel Event Time value is in 1/1024 second units and rolls over at 65536 or every 42 seconds (65536/1024 = 42).
          // Using an Arduino NANO 33 IOT, I observe it roll over every 32 seconds at 32767 (2 bytes = 16 bits) so Im going with that.
          // I'm assuming this is truncation of bits across platforms, but becuase it binary truncation it still works!
          float timeDifference = CurrWheelEvntTime > PrevWheelEvntTime ? CurrWheelEvntTime - PrevWheelEvntTime : 32767 + CurrWheelEvntTime - PrevWheelEvntTime;
          float timeSecs = timeDifference / 1024;
          float timeHrs = timeSecs / 3600;
          speedKMH = (kmTravelled / timeHrs); // Find speed kph

          // Sometimes the same message is rebroadcast even when at speed. This results in false 0 kpm. Ignore these when PrevSpeed
          if (speedKMH > 0 || PrevSpeedKMH < 4)
          {
            Serial.print("<< Speed update: "); Serial.print(timeSecs, DEC); Serial.print(" sec / ");
            Serial.print(speedKMH, DEC); Serial.println(" kph >>");

            speedTrainer = movingAverageFilter_speed.process(speedKMH);  // average speed
          }
        } //else {
        //Serial.println("Priming the pump");
        //}

        Serial.print("PrevWheelRevs: "); Serial.print(PrevWheelRevs); Serial.print("; PrevWheelEvntTime: "); Serial.println(PrevWheelEvntTime);
        Serial.print("currWheelRevs: "); Serial.print(currWheelRevs); Serial.print("; CurrWheelEvntTime: "); Serial.println(CurrWheelEvntTime);

        PrevWheelRevs = currWheelRevs;
        PrevWheelEvntTime = CurrWheelEvntTime;
        PrevSpeedKMH = speedKMH;
      }  // end if wheeldata present
    } // end if read
  } // end if (speedCharacteristic.valueUpdated())
}

//void refreshPower(void) {
void refreshPower(BLEDevice central, BLECharacteristic powerCharacteristic) {
  // see specs at https://www.bluetooth.com/specifications/specs/cycling-power-service-1-1/
  // CSCMeasurement characteristic are notified of updates approximately once per second.

  // Get updated power value
  if (powerCharacteristic.valueUpdated()) {
    // Define an array for the value
    uint8_t holdpowervalues[6] = {0, 0, 0, 0, 0, 0} ;

    // Read value into array
    powerCharacteristic.readValue(holdpowervalues, 6);

    // Power is returned as watts in location 2 and 3 (loc 0 and 1 is 8 bit flags)
    byte rawpowerValue2 = holdpowervalues[2];       // power least sig byte in HEX
    byte rawpowerValue3 = holdpowervalues[3];       // power most sig byte in HEX

    long rawpowerTotal = (rawpowerValue2 + (rawpowerValue3 * 256));
    //Serial.print("Power updated: "); Serial.println(rawpowerTotal);

    // Use moving average filter to give '3s power'
    powerTrainer = movingAverageFilter_power.process(rawpowerTotal);
  }
}

//void refreshBatteryLevel(void) {
void refreshBatteryLevel(BLEDevice central, BLECharacteristic batteryCharacteristic) {

  // Get updated battery level value
  if (batteryCharacteristic.valueUpdated()) {
    batteryCharacteristic.readValue(batteryLevel, 1);
    Serial.print("battery Level:");
    Serial.println(batteryLevel[0]);
  }
}

void refreshTemp(void) {

  static int tempMvAvgLen = 8;
  static MovingAverageFilter movingAverageFilter_temp(tempMvAvgLen);
  static int IMU_Temperature = 0;

  Serial.print("IMU_Temperature:");

  float temp;
  if (IMU.temperatureAvailable()) {
    // after IMU.readTemperature() returns, IMU_Temp will contain the temperature reading
    IMU.readTemperature(temp);

    IMU_Temperature = movingAverageFilter_temp.process(temp);      //
    Serial.print(temp);
    Serial.print(" ");
    Serial.println(IMU_Temperature);
  }
}

void resetSys(const __FlashStringHelper * txt) {

  char buf[20];
  sprintf_P(buf, PSTR("%s"), F(txt));

  displayLineLeft(0, 20, 1, " "); // erase line 0
  displayLineLeft(1, 12, 2, buf); // display txt in double size
  displayLineLeft(2, 20, 1, " "); // erase line 2
  doDisplay();

  digitalWrite (RESET_PIN, LOW);
}

bool resetSystem(void) {
  resetSys(F(" Resetting"));
  return true;
}

bool gradeSimWithLeveling(void) {
  static int loopCnt = 0;

  switch (loopCnt) {
    case 0:
      loopCnt = 1;
      break;
    case 1:
      trainerMode = LevelTrainer;
      levelingMode = EveryTime; // require leveling this time
      loopCnt = 2;
      break;
    case 2:
      levelingMode = userSettings.levelingMode;
      loopCnt = 3; // out of the way
      break;
  }

  if (gradeSim()) {
    loopCnt = 0; // for reentry;
    levelingMode = userSettings.levelingMode; // reapply user leveling preferences
    return true;
  } else {
    return false;
  }
}

/*  PID settings  */
bool setP(void)
{
  //bool setDouble(double& val, double valMin, double valMax, double increment)
  if (setDouble(PID_motor_kp, 0.0, 10, .1))
  {
    motorPID.SetTunings(PID_motor_kp, PID_motor_ki, PID_motor_kd);
    updateUserSettings();
    return true;
  } else {
    displayPIDParmVals();
    return false;
  }
}

bool setI(void)
{

  if (setDouble(PID_motor_ki, 0.0, 10, 0.01))
  {
    motorPID.SetTunings(PID_motor_kp, PID_motor_ki, PID_motor_kd);
    updateUserSettings();
    return true;
  } else {
    displayPIDParmVals();
    return false;
  }
}
bool setD(void)
{

  if (setDouble(PID_motor_kd, 0.0, 10, 0.1))
  {
    motorPID.SetTunings(PID_motor_kp, PID_motor_ki, PID_motor_kd);
    updateUserSettings();
    return true;
  } else {
    displayPIDParmVals();
    return false;
  }
}

bool setWeight(void)
{
  float incrementor = 1;
  if (displayUnits == 0) // imperial
  {
    incrementor = 0.45359237;
  }
  if (setDouble(riderWeight, 1, 1000, incrementor))
  {
    updateUserSettings();
    return true;
  } else {
    if (displayUnits == 0) // imperial
      displayNumber(riderWeight / incrementor, F(" lbs")); // display as int
    else
      displayNumber(riderWeight, F(" kg")); // display as int

    return false;
  }
}

bool setWheelSize(void)
{
  if (setNumber(wheelCircMM, 1, 9999, 1))
  {
    updateUserSettings();
    return true;
  } else {
    //  always format as mm
    //     if (displayUnits==0) // imperial
    //      displayNumber(wheelCircMM/0.393701, F(" in"));
    //     else
    displayNumber(wheelCircMM, F(" mm"));
    return false;
  }
}

boolean settrainerErrSensitivity(void)
{
  if (setDouble(trainerErrSensitivity, 0.0, 2, 0.1))
  {
    updateUserSettings();
    return true;
  } else {
    displayDouble(trainerErrSensitivity, F("%"));
    return false;
  }
}

boolean setManualAdjPcnt(void)
{
  if (setDouble(manAdjPcnt, 0.1, 10, 0.1))
  {
    updateUserSettings();
    return true;
  } else {
    displayDouble(manAdjPcnt, F("%"));
    return false;
  }
}

/* dimmer */
bool setOLEDDimOn(void) {
  return setOLEDDimMode(true);
}

bool setOLEDDimOff(void) {
  return setOLEDDimMode(false);
}

bool setOLEDDimMode(bool selection)
{
  if (selection != dimmer)
  {
    dimmer = selection;
    setOLEDDimmer (dimmer);
    updateUserSettings();
  }

  myMenu.setCurrentMenu(&settingsMenuList);
  myMenu.currentItemIndex = menuDisplayOption; // return to Display option;
  return true;
}

bool getOLEDDimMode(void) {
  return dimmer;
}

/* Debugging Mode */
boolean startDebuggingMode()
{
  return setDebuggingMode(true);
}

boolean stopDebuggingMode()
{
  return setDebuggingMode(false);
}

bool setDebuggingMode(bool selection)
{
  Debugging = selection;
  myMenu.setCurrentMenu(&settingsMenuList);
  myMenu.currentItemIndex = menuDebugMenuOption; // return to Debug option;
  return true;
}

bool getDebuggingMode(void) {
  return Debugging;
}

/* Display Units */
boolean setUnitsImperial()
{
  return setUnits(0);
}

boolean setUnitsMetric()
{
  return setUnits(1);
}

bool setUnits(bool selection)
{
  if (selection != displayUnits)
  {
    displayUnits = selection;
    updateUserSettings();
  }

  myMenu.setCurrentMenu(&settingsMenuList);
  myMenu.currentItemIndex = menuUnitsOption; // return to Display option;
  return true;
}

int getDisplayUnits(void) {
  return displayUnits;
}

/* Leveling */
boolean setLevelingOff() {
  setLevelingMode(FirstTime);
}
boolean setLevelingOn() {
  setLevelingMode(EveryTime);
}

boolean setLevelingMode(LevelingMode selection)
{
  if (selection != levelingMode)
  {
    levelingMode = selection;
    updateUserSettings();
  }

  myMenu.setCurrentMenu(&settingsMenuList);
  myMenu.currentItemIndex = menuLevelingOption; // return to Leveling option;
  return true;
}

LevelingMode getLevelingMode() {
  return levelingMode;
}

/* PID Parameters */
void displayPIDParmVals(void) {
  // Display PID values
  char buf[20];
  sprintf_P(buf, PSTR("%.2g, %.2g, %.2g"), PID_motor_kp, PID_motor_ki , PID_motor_kd);
  displayLineLeft(1, 12, 1, buf);
  displayLineLeft(2, 24, 1, " "); // erase the unused lines
}

boolean undoPIDChanges()
{
  setPIDDefaults();
  updateUserSettings();
  return true; // be very very carfull to return true here. creating a loop will burn out the ROM.
}

/* User Settings */
void initUserSettings()
{
  // Read or initialize the content of "userSettings_FlashStore"
  userSettings = userSettings_FlashStore.read();

  // Intitialize flash store with default values the first time.
  if (userSettings.valid == false) {
    Serial.println("Initializing User Settings Flash Storage");
    setFactoryDefaults();
    updateUserSettings();
    return;
  }

  riderWeight = userSettings.riderWeight;
  wheelCircMM = userSettings.wheelCircMM;
  trainerInclineZeroOffset = userSettings.trainerInclineZeroOffset;
  PID_motor_kp = userSettings.PID_motor_kp;
  PID_motor_ki = userSettings.PID_motor_ki;
  PID_motor_kd = userSettings.PID_motor_kd;
  manAdjPcnt = userSettings.manAdjPcnt;
  trainerErrSensitivity = userSettings.trainerErrSensitivity;
  displayUnits = userSettings.displayUnits;
  dimmer = userSettings.dimmer;
  levelingMode = userSettings.levelingMode;
  Serial.print("UserSettings: trainerInclineZeroOffset:");
  Serial.print(trainerInclineZeroOffset);
}

void updateUserSettings()
{
  userSettings.riderWeight = riderWeight;
  userSettings.wheelCircMM = wheelCircMM;
  userSettings.trainerInclineZeroOffset = trainerInclineZeroOffset;
  userSettings.PID_motor_kp = PID_motor_kp;
  userSettings.PID_motor_ki = PID_motor_ki;
  userSettings.PID_motor_kd = PID_motor_kd;
  userSettings.manAdjPcnt = manAdjPcnt;
  userSettings.trainerErrSensitivity = trainerErrSensitivity;
  userSettings.displayUnits = displayUnits;
  userSettings.dimmer = dimmer;
  userSettings.levelingMode = levelingMode;
  userSettings.valid = true;
  userSettings_FlashStore.write(userSettings);
}

void setPIDDefaults()
{
  PID_motor_kp = 2.0, PID_motor_ki = 0.07, PID_motor_kd = 0.3;  // default values
}

void setFactoryDefaults()
{
  riderWeight = 99; // rider + bike = 99 kg/218 lbs (trek 820 15.28 kg (33.68 lbs) + me 83.91 kg (185 lbs)); default val combined rider and bike weight
  wheelCircMM = 2070; // Default val for wheel circumference in milimeters. (26 × 2.125 = 2070, 700c 32 road wheel = 2300)
  trainerInclineZeroOffset = NULL; // trainer incline zero offset adjusted by user to level trainer
  trainerErrSensitivity = 0.5; // default 0.5%
  manAdjPcnt = 0.5; // default 0.5%
  displayUnits = 0; // default to imperial units
  dimmer = 0; // default to off
  levelingMode = EveryTime; // default to EveryTime.
  setPIDDefaults();
}

void saberToothSetup()
{
  Sabertooth ST(128); // default address 128
  SabertoothTXPinSerial.begin(9600); // 9600 is the default baud rate for Sabertooth packet serial.
  ST.autobaud(); // Send the autobaud command to the Sabertooth controller(s).
}

void biColorLED(bool on, bool color1) {

  if (on) {
    if (color1)
    {
      // flip led to green
      digitalWrite(GREEN_LED_PIN, LOW);    //LED green ON
      digitalWrite(RED_LED_PIN, HIGH);     //LED RED off
    } else {
      // flip led to red
      digitalWrite(GREEN_LED_PIN, HIGH);   //LED green off
      digitalWrite(RED_LED_PIN, LOW);      //LED RED on
    }
  } else {
    digitalWrite(RED_LED_PIN, HIGH);        //LED RED off
  }
}

void serialReceive()
{
  if (Serial.available())
  {
    char b = Serial.read();
    Serial.flush();
    double modifier = 0.25;
    switch (b)
    {
      case '+':
        inputGrade = inputGrade + modifier;
        break;
      case '-':
        inputGrade = inputGrade - modifier;
        break;
      case 'p':
        Serial.println("power v");
        powerTrainer = movingAverageFilter_power.process(powerTrainer - 50);
        break;
      case 'P':
        Serial.println("power ^");
        powerTrainer = movingAverageFilter_power.process(powerTrainer + 50);
        break;
      case 's':
        speedTrainer = movingAverageFilter_speed.process(speedTrainer - 5);
        break;
      case 'S':
        speedTrainer = movingAverageFilter_speed.process(speedTrainer + 5);
        break;
    }

    //    Serial.print("serialReceive b:");
    //    Serial.print(b);
    Serial.print("; inputGrade:");
    Serial.println(inputGrade);
  }

}