diff --git a/pedaal/AnalogSmooth.cpp b/pedaal/AnalogSmooth.cpp deleted file mode 100644 index db4f185..0000000 --- a/pedaal/AnalogSmooth.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/* - AnalogSmooth.cpp - Library to smooth analog signal - jitter by averaging concurrent readings. - Created by Michael Thessel. - https://github.com/MichaelThessel/arduino-analog-smooth/blob/master/AnalogSmooth.ccp -*/ -#include "Arduino.h" -#include "AnalogSmooth.h" - -/* - Constructor -*/ -AnalogSmooth::AnalogSmooth() -{ - this->_init(10); -} - -/* - Constructor -*/ -AnalogSmooth::AnalogSmooth(unsigned int windowSize) -{ - this->_init(windowSize); -} - -/* - Initialzie the environment -*/ -void AnalogSmooth::_init(unsigned int windowSize) -{ - // Restrict the size of the history array - // to >= 1 and < 100 items - if (windowSize > 100) { windowSize = 100; } - if (windowSize < 1) { windowSize = 1; } - this->_windowSize = windowSize; - - this->_analogPointer = 0; - this->_maxPointer = 0; -} - -/* - Perform smooting of analog input from given value -*/ -float AnalogSmooth::smooth(float value) -{ - // Return if we only keep track of 1 value - if (this->_windowSize == 1) { - return value; - } - - // Save the value to the history array - this->_analog[this->_analogPointer] = value; - - // Calculate the moving average - float total = 0; - for (int i = 0; i <= this->_maxPointer; i++) { - total = total + this->_analog[i]; - } - float avg = total / (this->_maxPointer + 1); - - // Keep track of how many items we have in the array - if (this->_maxPointer < this->_windowSize - 1) { - this->_maxPointer++; - } - - // Update the array pointer - this->_analogPointer++; - if (this->_analogPointer == this->_windowSize) { - this->_analogPointer = 0; - } - - // Retrun the average - return avg; -} - -/* - Perform smooting of analog input from given pin -*/ -float AnalogSmooth::analogReadSmooth(uint8_t pin) -{ - // Read the pin - float current = analogRead(pin); - - return this->smooth(current); -} diff --git a/pedaal/AnalogSmooth.h b/pedaal/AnalogSmooth.h deleted file mode 100644 index a916144..0000000 --- a/pedaal/AnalogSmooth.h +++ /dev/null @@ -1,28 +0,0 @@ -/* - AnalogSmooth.h - Library to smooth analog signal - jitter by averaging concurrent readings. - Created by Michael Thessel. - https://github.com/MichaelThessel/arduino-analog-smooth/blob/master/AnalogSmooth.h -*/ - -#ifndef AnalogSmooth_h -#define AnalogSmooth_h - -#include "Arduino.h" - -class AnalogSmooth -{ - public: - AnalogSmooth(unsigned int windowSize); - AnalogSmooth(); - float analogReadSmooth(uint8_t pin); - float smooth(float value); - private: - unsigned int _windowSize; - uint8_t _pin; - float _analog[100]; - unsigned int _analogPointer; - unsigned int _maxPointer; - void _init(unsigned int windowSize); -}; -#endif diff --git a/pedaal/Pedal.h b/pedaal/Pedal.h index 68a7541..1fa3514 100644 --- a/pedaal/Pedal.h +++ b/pedaal/Pedal.h @@ -10,7 +10,7 @@ #ifndef UtilLib #include "UtilLibrary.h" -#include "AnalogSmooth.h" +#include "Smoothed.h" #include @@ -19,8 +19,6 @@ // init util library UtilLib utilLib; -// set to window 10 -AnalogSmooth as = AnalogSmooth(10); class Pedal { @@ -29,6 +27,8 @@ class Pedal //initialise pedal Pedal(String prefix) { _prefix = prefix; + _mySensor.begin(SMOOTHED_EXPONENTIAL, 10); + _mySensor.clear(); } void Pedal::ConfigAnalog ( byte analogInput) { @@ -84,7 +84,6 @@ class Pedal Pedal::updatePedal(rawValue); } - void Pedal::setSmoothValues(int smoothValues) { _smooth = smoothValues; } @@ -104,7 +103,12 @@ class Pedal //////////////////// void Pedal::resetCalibrationValues(int EEPROMSpace) { int resetMap[4] = {0, SENSOR_RANGE, 0, SENSOR_RANGE}; + _calibration[0] = resetMap[0]; + _calibration[1] = resetMap[1]; + _calibration[2] = resetMap[2]; + _calibration[3] = resetMap[3]; utilLib.writeStringToEEPROM(EEPROMSpace, utilLib.generateStringMapCali(resetMap)); + } void Pedal::getEEPROMCalibrationValues(int EEPROMSpace) { @@ -161,11 +165,12 @@ class Pedal String _pedalString; int _afterHID; int _signal = 0; + Smoothed _mySensor; HX711 _loadCell; ADS1115 _ads1015; int _analogInput = 0; int _inverted = 0; //0 = false / 1 - true - int _smooth = 0; + int _smooth = 0; //0 = false / 1 - true int _inputMap[6] = { 0, 20, 40, 60, 80, 100 }; int _outputMap[6] = { 0, 20, 40, 60, 80, 100 }; int _calibration[4] = {0, SENSOR_RANGE, 0, SENSOR_RANGE}; // calibration low, calibration high, deadzone low, deadzone high @@ -181,7 +186,8 @@ class Pedal //////////////////////////////////////////////////////////////////////////////// if (_smooth == 1) { - rawValue = as.smooth(rawValue); + _mySensor.add(rawValue); + rawValue = _mySensor.get(); } if (_inverted == 1) { diff --git a/pedaal/Smoothed.cpp b/pedaal/Smoothed.cpp new file mode 100644 index 0000000..0e5dc7a --- /dev/null +++ b/pedaal/Smoothed.cpp @@ -0,0 +1,7 @@ +/* + * Smoothed.cpp + * Store and calculate smoothed values from sensors. + * Created by Matt Fryer on 2017-11-17. + * Licensed under LGPL (free to modify and use as you wish) + */ + diff --git a/pedaal/Smoothed.h b/pedaal/Smoothed.h new file mode 100644 index 0000000..9fcd7fe --- /dev/null +++ b/pedaal/Smoothed.h @@ -0,0 +1,199 @@ +/* + * Smoothed.h + * Store and calculate smoothed values from sensors. + * Created by Matt Fryer on 2017-11-17. + * Licensed under LGPL (free to modify and use as you wish) + */ + +#pragma once + +#define SMOOTHED_AVERAGE 1 +#define SMOOTHED_EXPONENTIAL 2 + +// A class used to store and calculate the values to be smoothed. +template +class Smoothed { + private: + byte smoothMode; + uint16_t smoothReadingsFactor = 10; // The smoothing factor. In average mode, this is the number of readings to average. + uint16_t smoothReadingsPosition = 0; // Current position in the array + uint16_t smoothReadingsNum = 0; // Number of readings currently being averaged + T *smoothReading; // Array of readings + public: + Smoothed(); + ~Smoothed(); // Destructor to clean up when class instance killed + bool begin (byte smoothMode, uint16_t smoothFactor = 10); + bool add (T newReading); + T get (); + T getLast (); + bool clear (); +}; + +// Constructor +template +Smoothed::Smoothed () { // Constructor + +} + +// Destructor +template +Smoothed::~Smoothed () { // Destructor + delete[] smoothReading; +} + +// Inintialise the array for storing sensor values +template +bool Smoothed::begin (byte mode, uint16_t smoothFactor) { + smoothMode = mode; + smoothReadingsFactor = smoothFactor; + + switch (smoothMode) { + case SMOOTHED_AVERAGE : // SMOOTHED_AVERAGE + + smoothReading = new T[smoothReadingsFactor]; // Create the actual array of the required size + + // Initialise all the values in the array to zero + for (int thisReading = 0; thisReading < smoothReadingsNum; thisReading++) { + smoothReading[thisReading] = 0; + } + + return true; + break; + + case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL + + smoothReading = new T[2]; + smoothReading[0] = 0; + smoothReading[1] = 0; // Second value in array used for storing last value added + + return true; + break; + + default : + return false; + break; + } + +} + +// Add a value to the array +template +bool Smoothed::add (T newReading) { + switch (smoothMode) { + case SMOOTHED_AVERAGE : // SMOOTHED_AVERAGE + + if(smoothReadingsNum < smoothReadingsFactor) { smoothReadingsNum++; } // Keep record of the number of readings being averaged. This will count up to the arrany saize then stay at that number + + smoothReading[smoothReadingsPosition] = newReading; // Add the new value + + if (smoothReadingsPosition == (smoothReadingsFactor - 1)) { // If at the end of the array + smoothReadingsPosition = 0; // Increment to the beginning of the array + } else { + smoothReadingsPosition++; // Increment to next array position position + } + + return true; + break; + + case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL + + if( smoothReadingsNum == 0 ) { + smoothReadingsNum++; + smoothReading[0] = newReading; + } else { + smoothReading[0] = (T)(((long double)smoothReadingsFactor/100) * newReading + (1 - ((long double)smoothReadingsFactor/100)) * smoothReading[0]); + } + + smoothReading[1] = newReading; // Update the last value added + + return true; + break; + + default : + return false; + break; + } +} + +// Get the smoothed result +template +T Smoothed::get () { + switch (smoothMode) { + case SMOOTHED_AVERAGE : { // SMOOTHED_AVERAGE + T runningTotal = 0; + // calculating a `SUM(smoothReadings) / smoothReadingsNum` can lead to overflows. + T tmpRes = 0; + T remainder = 0; + for (int x = 0; x < smoothReadingsNum; x++) { + tmpRes = smoothReading[x] / smoothReadingsNum; + remainder += smoothReading[x] - tmpRes * smoothReadingsNum; + runningTotal += tmpRes; + if (remainder > smoothReadingsNum) { + tmpRes = remainder / smoothReadingsNum; + remainder -= tmpRes * smoothReadingsNum; + runningTotal += tmpRes; + } + } + return runningTotal; + } + break; + + case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL + return smoothReading[0]; + break; + + default : + return false; + break; + } +} + +// Gets the last result stored +template +T Smoothed::getLast () { + switch (smoothMode) { + case SMOOTHED_AVERAGE : // SMOOTHED_AVERAGE + // Just return the last reading + if (smoothReadingsPosition == 0) { + return smoothReading[smoothReadingsFactor-1]; + } else { + return smoothReading[smoothReadingsPosition-1]; + } + break; + + case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL + return smoothReading[1]; + break; + + default : + return false; + break; + } +} + +// Clears all stored values +template +bool Smoothed::clear () { + switch (smoothMode) { + case SMOOTHED_AVERAGE : // SMOOTHED_AVERAGE + // Reset the counters + smoothReadingsPosition = 0; + smoothReadingsNum = 0; + + // Set all the values in the array to zero. Not really needed + for (int thisReading = 0; thisReading < smoothReadingsNum; thisReading++) { + smoothReading[thisReading] = 0; + } + break; + + case SMOOTHED_EXPONENTIAL : // SMOOTHED_EXPONENTIAL + smoothReadingsNum = 0; + smoothReading[0] = 0; + smoothReading[1] = 0; + break; + + default : + return false; + break; + } +} \ No newline at end of file diff --git a/pedaal/pedaal.ino b/pedaal/pedaal.ino index bcecc4d..ff98f32 100644 --- a/pedaal/pedaal.ino +++ b/pedaal/pedaal.ino @@ -105,6 +105,12 @@ void loop() { throttle.setOutputMapValues(tmap, E_THROTTLE); } + if (msg.indexOf("CALIRESET") >= 0) { + clutch.resetCalibrationValues(E_CALIBRATION_C); + brake.resetCalibrationValues(E_CALIBRATION_B); + throttle.resetCalibrationValues(E_CALIBRATION_T); + } + if (msg.indexOf("CCALI:") >= 0 && msg.indexOf("BCALI:") >= 0 && msg.indexOf("TCALI:") >= 0) { String splitTCALI = utilLib.getValue(msg, ',', 0); splitTCALI.replace("TCALI:", ""); @@ -116,7 +122,7 @@ void loop() { String splitCCALI = utilLib.getValue(msg, ',', 2); splitCCALI.replace("CCALI:", ""); - brake.setCalibrationValues(splitCCALI, E_CALIBRATION_C); + clutch.setCalibrationValues(splitCCALI, E_CALIBRATION_C); } updateInverted(msg); @@ -162,7 +168,7 @@ void loadDeviceSettings() { String EEPROM_SmoothMap = utilLib.readStringFromEEPROM(E_PEDAL_SMOOTH_MAP); String SMOOTH = "SMOOTH:"; - updateSmooth(EEPROM_SmoothMap); + updateSmooth(SMOOTH + EEPROM_SmoothMap); clutch.getEEPROMCalibrationValues(E_CALIBRATION_C); @@ -183,7 +189,7 @@ void resetDeviceSettings() { utilLib.writeStringToEEPROM(E_PEDAL_INVERTED_MAP, "0-0-0"); // 0 = false / 1 = true - utilLib.writeStringToEEPROM(E_PEDAL_SMOOTH_MAP, "0-0-0"); + utilLib.writeStringToEEPROM(E_PEDAL_SMOOTH_MAP, "1-1-1"); clutch.resetCalibrationValues(E_CALIBRATION_C); brake.resetCalibrationValues(E_CALIBRATION_B);