diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..29a69f5 Binary files /dev/null and b/.DS_Store differ diff --git a/OpenGradeSIM_CombinedCode_027.ino b/OpenGradeSIM_CombinedCode_027.ino deleted file mode 100644 index 89f7313..0000000 --- a/OpenGradeSIM_CombinedCode_027.ino +++ /dev/null @@ -1,652 +0,0 @@ -/* - 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 -#include -#include -#include -#include -#include -#include -#include - - -#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 3 // To the level shifter and then to the H Bridge -#define actuatorOutPin2 2 // 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); // 3 second power average at 4 samples per sec -MovingAverageFilter movingAverageFilter_speed(6); // 2 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 = 0.27; // 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 = 2350; // 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(2); - 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 10s since last move - -if ((currentMillis > (actuatorMillis + 5000)) &&(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((- x) , sqrt(y * y + 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 (incline0) && (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(); - - - } - - - diff --git a/OpenGradeSIM_CombinedCode_100.ino b/OpenGradeSIM_CombinedCode_100.ino index 1509765..6aa62f5 100644 --- a/OpenGradeSIM_CombinedCode_100.ino +++ b/OpenGradeSIM_CombinedCode_100.ino @@ -97,8 +97,8 @@ 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 +int PrevSpeedKMH; // Prev speed; // Custom Char Bluetooth Logo @@ -433,24 +433,12 @@ long rawpowerTotal = (rawpowerValue2 + (rawpowerValue3 * 256)); 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; + if (WheelRevs1 > WheelRevs2) { // make sure the bicycle is moving // Find distance difference in cm and convert to km float distanceTravelled = ((WheelRevs2 - WheelRevs1) * wheelCircCM); @@ -467,19 +455,17 @@ long rawpowerTotal = (rawpowerValue2 + (rawpowerValue3 * 256)); Serial.print(" speed: "); Serial.println(speedKMH, DEC); - - // Reject zero values - if (speedKMH < 0){}else{ + // Sometimes the same message is rebroadcast even when at speed. This results in false 0 kpm. Ignore these when PrevSpeed > 4 kph + if (speedKMH > 0 || PrevSpeedKMH < 4){ speedTrainer = movingAverageFilter_speed.process(speedKMH); // use moving average filter to find 3s average speed // speedTrainer = speedKMH; // redundant step to allow experiments with filters - - - } + WheelRevs2 = WheelRevs1; + Time_2 = Time_1; + PrevSpeedKMH = speedKMH; } } - - } + // we only need to do all this 4 or 5 times a second! delay(200);