OpenGradeSIM_CombinedCode_100.ino

/*
  OpenGradeSimulator by Matt Ockendon 2019.11.14

  ____                  _____               __      ____ ____ __  ___
 / __ \ ___  ___  ___  / ___/____ ___ _ ___/ /___  / __//  _//  |/  /
/ /_/ // _ \/ -_)/ _ \/ (_ // __// _ `// _  // -_)_\ \ _/ / / /|_/ / 
\____// .__/\__//_//_/\___//_/   \_,_/ \_,_/ \__//___//___//_/  /_/  
     /_/                                                             

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. 

I found the equation of a best fit line from points plotted using an online calculator of frictional losses vs speed
and then took the residual power to calculate the incline being simulated

Rather than using a servo linear actuator (expensive) I'm using the Arduino Nano 33 IoT BLE's built in
accelerometers to find the position of the bicycle. This method is prone to noise and I have tried
some filtering (moving average) to reduce this.

  The circuit:
    Arduino Nano 33 BLE
    3.3 to 5v level shifter
    L298N H bridge
    750Newton 200mm Linear Actuator
    1x2 pushbutton pad
    128x32 I2C OLED display
    3D printed parts and boxes
    At present a NPE CABLE ANT+ to BLE bridge is required 
    (due to the lack of authentication in the
    AdruinoBLE library 1.1.2)

  This code is in the public domain.
  Uses the moving average filter of sebnil https://github.com/sebnil/Moving-Avarage-Filter--Arduino-Library-
  and the Flash Storage library https://github.com/sebnil/Moving-Avarage-Filter--Arduino-Library-
  
*/

#include <FlashStorage.h>
#include <MovingAverageFilter.h>
#include <ArduinoBLE.h>
#include <Arduino_LSM6DS3.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>


#define SCREEN_WIDTH 128     // OLED display width, in pixels
#define SCREEN_HEIGHT 32     // OLED display height, in pixels
#define buttonUpPin 5        // For the keypad
#define buttonDownPin 6      // For the keypad
#define buttonCommonPin 7    // For the keypad
#define actuatorOutPin1 2    // To the level shifter and then to the H Bridge
#define actuatorOutPin2 3    // To the level shifter and then to the H Bridge
#define resetPin 19          // Pin linked to RST to allow software to do hard reset

// Declare our filters
MovingAverageFilter movingAverageFilter_x(9);        // 
MovingAverageFilter movingAverageFilter_y(9);        // Moving average filters for the accelerometers
MovingAverageFilter movingAverageFilter_z(9);        //
MovingAverageFilter movingAverageFilter_power(8);    // 2 second power average at 4 samples per sec
MovingAverageFilter movingAverageFilter_speed(2);    // 0.5 second speed average at 4 samples per sec


// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1         // No reset pin on cheap OLED display
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// For incline declare some variables and set some default values

float versionNumber = 1.01;       // version
long previousMillis = 0;          // last time in ms
long weightPrevMillis = 0;        // last time for weight setting
long weightMillis = 0;            // time for weight setting
long actuatorMillis = 0;          // time for moving the actuator
float smoothRadPitch = 0;         // variable for the pitch
int incline = 0;                  // variable for the % incline (actual per accelerometers) 
int gradeCalculated = 15;         // variable for the calculated grade (aim)
FlashStorage(weight_storage, int);// place to store rider weight
int riderWeight = 95;             // variable for combined rider and bike weight
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
bool weightIsSet = false;         // note whether the weight setting is done
int buttonStateUp = 0;            // variable for reading the UP pushbutton status
int buttonStateDown = 0;          // variable for reading the UP pushbutton status

// For power and speed declare some variables and set some default values

int wheelCircCM = 2300;           // Wheel circumference in centimeters (700c 32 road wheel)
long WheelRevs1;                  // For speed data set 1
long Time_1;                      // For speed data set 1
long WheelRevs2;                  // For speed data set 2
long Time_2;                      // For speed data set 2
bool firstData = true;
int speedKMH;                     // Calculated speed in KM per Hr

// Custom Char Bluetooth Logo

byte customChar[] = {
  B00000,
  B00110,
  B00101,
  B10110,
  B01100,
  B10110,
  B00101,
  B00110
};

// Our BLE peripheral and characteristics

BLEDevice cablePeripheral;
BLECharacteristic speedCharacteristic;
BLECharacteristic powerCharacteristic;

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

void setup() {
  Serial.begin(9600);

//  riderWeight = weight_storage.read();    // Need to sort out saving the weight in the weightset method

  
  delay(2000);

//  setup control pins and set to lower trainer by default
  pinMode(actuatorOutPin1, OUTPUT);
  pinMode(actuatorOutPin2, OUTPUT);
  digitalWrite(actuatorOutPin1, LOW);
  digitalWrite(actuatorOutPin2, HIGH);

//  setup input pins for keypad and set adjacent pin to output low to act as a sink
  pinMode (buttonUpPin, INPUT_PULLUP);
  pinMode (buttonDownPin, INPUT_PULLUP);
  pinMode (buttonCommonPin, OUTPUT);
  digitalWrite (buttonCommonPin, LOW);

//   setup a pin connected to RST (A5, pin 19) to pull reset low if reset is required
  pinMode (resetPin, OUTPUT);
  digitalWrite (resetPin, HIGH);
  
  Serial.begin(9600);
   
   if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
   Serial.println(F("SSD1306 allocation failed"));
   resetSystem();
   }

// Show initial display buffer contents on the screen --
// the library initializes this with a splash screen (edit the splash.h in the library).

  display.setRotation(0);
  display.display();
  delay(1000);                    // Pause for 1 seconds
  display.clearDisplay();

// Show the firmware version

  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(5, 10);
  display.print(F("FW Version: "));
  display.print(versionNumber);
  display.display(); 
  delay(1000);                    // Pause for 2 seconds
  display.clearDisplay();


// Check that the accelerometer is up and running else reset

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    resetSystem();
  }


  // begin BLE initialization reset if fails
  if (!BLE.begin()) {
    Serial.println("starting BLE failed!");
    resetSystem();
  }

}

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

void loop() {

// BLE setup begins

  while (!cablePeripheral.connected()) {  
    Serial.println("BLE Central");
    Serial.println("Turn on trainer and CABLE module and check batteries");
     // Scan or rescan for BLE services
     
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(5, 10);
  display.print(F("BLE Scanning"));
  display.setCursor(5, 20);
  display.print(F("for CABLE Device"));
  display.display(); 
  display.clearDisplay();


     
    BLE.scan();

  // 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("Found ");
    Serial.print(cablePeripheral.address());
    Serial.print(" '");
    Serial.print(cablePeripheral.localName());
    Serial.print("' ");
    Serial.print(cablePeripheral.advertisedServiceUuid());
    Serial.println();


    if (cablePeripheral.localName() == ">CABLE") {
      // stop scanning
      BLE.stopScan();
      Serial.println("got CABLE device scan stopped");

      display.setTextSize(1);
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(5, 10);
      display.print(F("CABLE Found"));
      display.setCursor(5, 20);
      display.print(F("Authenticating"));
      display.display(); 
      display.clearDisplay();


      // Do the BLE niceties and subscribe to speed and power 
      getsubscribedtoSensor(cablePeripheral);
    
    }
  }
  }

  // Get any updated data
  refreshSpeedandpower();
 
  long currentMillis = millis();

// Call the set weight method

if (weightIsSet==false){
  weightMillis = millis();
  setWeight();

   if (weightMillis - weightPrevMillis >=5000){weightIsSet = true;}   // times up, set weight set 
}


// if 100ms have passed and weight is set, check the variables and update the system
    if ((currentMillis - previousMillis >= 100) && (weightIsSet)){
    previousMillis = currentMillis;

// read the accelerometer
      findTrainerIncline();

// Calculate the incline
      calculateGrade();

// Display the current data
      lcdDisplayData();

// Update the actuator positon only if the trainer is in use and time is at least 2.5s since last move 

if ((currentMillis > (actuatorMillis + 2500)) &&(powerTrainer > 40) && (speedTrainer > 5))
{   moveActuator();
    actuatorMillis = currentMillis;
}
  
}
    }  // end of loop

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

void getsubscribedtoSensor(BLEDevice cablePeripheral) {
  //   connect to the peripheral
  Serial.println("Connecting ...");
  if (cablePeripheral.connect()) {
    Serial.println("Connected");

  } else {
    Serial.println("Failed to connect to CABLE device");
    return;
  }

  // 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;
  }

  // retrieve the characteristics

  speedCharacteristic = cablePeripheral.characteristic("2a5B");
  powerCharacteristic = cablePeripheral.characteristic("2a63");


  // subscribe to the characteristics (note authentication not supported on ArduinoBLE library v1.1.2)
  
  if (!speedCharacteristic.subscribe()) {
    Serial.println("can not subscribe to speed");
    }else{
    Serial.println("subscribed to speed");
    };
  
  
  if (!powerCharacteristic.subscribe()) {
  Serial.println("can not subscribe to speed and power");

  // outcome display on OLED
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(5, 10);
  display.print(F("Subscribe FAILED"));
  display.setCursor(5, 20);
  display.print(F("Speed and Power"));
  display.display(); 
  display.clearDisplay();

  delay(5000);
  resetSystem();
    
  } else {
  Serial.println("subscribed to speed and power");

  // outcome display on OLED
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(5, 10);
  display.print(F("Subscribed to"));
  display.setCursor(5, 20);
  display.print(F("Speed and Power"));
  display.display(); 
  display.clearDisplay();

  };

    //  The time consuming BLE setup is done, set timer for the weight setting routine
    weightPrevMillis = millis();  


}

void refreshSpeedandpower(void){  
  
// 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: ");
 //   Serial.println(rawpowerTotal);
    
 // Use moving average filter to give '3s power'
    powerTrainer = movingAverageFilter_power.process(rawpowerTotal);

       Serial.print("rawpowerValue2");
       Serial.println(rawpowerValue2);
       Serial.print("rawpowerValue3");
       Serial.println(rawpowerValue3);
    
}

// Get speed - a bit more complication as the GATT specification calls for Cumulative Wheel Rotations and Time since wheel event
// So we'll need to do some maths

    if (speedCharacteristic.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

      speedCharacteristic.readValue(holdvalues, 7);
      byte rawValue0 = holdvalues[0];       // binary flags 8 bit int
      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 (firstData) {
        // Get cumulative wheel revolutions as little endian hex in loc 2,3 and 4 (least significant octet first)
        WheelRevs1 = (rawValue1 + (rawValue2 * 256) + (rawValue3 * 65536) + (rawValue4 * 16777216));
        // Get time since last wheel event in 1024ths of a second
        Time_1 = (rawValue5 + (rawValue6 * 256));

        firstData = false;

      } else {

        // Get second set of data
        long WheelRevsTemp = (rawValue1 + (rawValue2 * 256) + (rawValue3 * 65536) + (rawValue4 * 16777216));
        long TimeTemp = (rawValue5 + (rawValue6 * 256));

        if (WheelRevsTemp > WheelRevs1) {           // make sure the bicycle is moving
          WheelRevs2 = WheelRevsTemp;
          Time_2 = TimeTemp;
          firstData = true;

          // Find distance difference in cm and convert to km
          float distanceTravelled = ((WheelRevs2 - WheelRevs1) * wheelCircCM);
          float kmTravelled = distanceTravelled / 1000000;

          // Find time in 1024ths of a second and convert to hours
          float timeDifference = (Time_2 - Time_1);
          float timeSecs = timeDifference / 1024;
          float timeHrs = timeSecs / 3600;

          // Find speed kmh
          speedKMH = (kmTravelled / timeHrs);

          Serial.print("  speed: ");
          Serial.println(speedKMH, DEC);


          // Reject zero values
          if (speedKMH < 0){}else{
          speedTrainer = movingAverageFilter_speed.process(speedKMH);  // use moving average filter to find 3s average speed
         // speedTrainer =  speedKMH;               // redundant step to allow experiments with filters
 

          
          }
        }
      }

    }
    
// we only need to do all this 4 or 5 times a second!
  delay(200);
  }

void findTrainerIncline(void){
    //Serial.print("findTrainerIncline");
       float rawx, rawy, rawz;
       float x, y, z;
  
    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);      //
      
      // find pitch in radians
      float radpitch = atan2(( y) , sqrt(x * x + z * z)) ;

      smoothRadPitch = radpitch;

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

    }}
  
void lcdDisplayData(void) {


  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

// Display power top left

  display.setCursor(0, 0);
  display.print(powerTrainer);
  display.print(F(" W"));

// Display speed top right if more than 4kph

if(speedTrainer>4){
  display.setCursor(80, 0);
  display.print(speedTrainer);
  display.print(F(" kph"));}
  else{
  display.setCursor(80, 0);
  display.print("-- ");
  display.print(F(" kph"));
  }

// Display weight bottom left

  display.setCursor(0, 24);
  display.print(riderWeight);
  display.print(F(" kg"));

// Display target incline bottom right
if(gradeCalculated>0){
  display.setCursor(80, 24);
  display.print(gradeCalculated);
  display.print(F(" %"));}
  else{
  display.setCursor(80, 24);
  display.print(F("0 %"));}
  

//  Display current incline centred and large
  display.setTextSize(2); // Draw 2X-scale text
  display.setCursor(50, 9);
  display.print(incline);
  display.print(F("%"));

//  Update the display
  display.display(); 
  
  }

void moveActuator(void) {

// This method is ugly - just pausing the script while the actuator moves - there are many better ways - if only I had the time!
// That said there will be more noise whilst moving so maybe some advantage

int difference = incline-gradeCalculated;   // Find the difference
int absDifference = abs(difference);        // Find the absolute (like rms)


if (incline>gradeCalculated){
  digitalWrite(actuatorOutPin1, LOW);                           
  digitalWrite(actuatorOutPin2, HIGH);                           
  }                                     
  else if (incline<gradeCalculated){                
  digitalWrite(actuatorOutPin1, HIGH);                           
  digitalWrite(actuatorOutPin2, LOW);                          
  }
  
if ((absDifference >0) && (absDifference <2)){
  delay(1000);
} 
if ((absDifference >=2) && (absDifference <3)){
  delay(2000);
} 
if ((absDifference >=3) && (absDifference <4)){
  delay(3000);
} 
if (absDifference >=4) {
  delay(4000);
} 

  digitalWrite(actuatorOutPin1, LOW);
  digitalWrite(actuatorOutPin2, LOW);


  
  }

void calculateGrade(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
  speedMpersec = speedTrainer/3.6;                                                    // find speed in SI units
  gradeCalculated = ((powerMinusResistance/(riderWeight*9.8))/speedMpersec)*100;      // calculate grade of climb in %

// Sense check
if (gradeCalculated < -10){gradeCalculated = -10;}
if (gradeCalculated > 20){gradeCalculated = 20;}
  }  

void resetSystem(void){
 digitalWrite (19, LOW);
  }  

void setWeight(void){

// Read the buttons
// If button state chaged then update value and reset timer

  buttonStateUp = digitalRead(buttonUpPin);
  buttonStateDown = digitalRead(buttonDownPin);

    if (buttonStateUp == LOW) {
    // turn LED on:
    riderWeight = riderWeight+1;
    delay (200);                             // low tech button debounce and limit autorepeat rate
    weightPrevMillis = weightMillis;
  } 

    if (buttonStateDown == LOW) {
    // turn LED on:
    riderWeight = riderWeight-1;
    delay (200);
    weightPrevMillis = weightMillis;
  } 
  
// Update the display
 
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(50, 9);
  display.print(riderWeight);
  display.print(F(" Kg"));
  display.display(); 

  
  }