Add Duty, Current, Speed control modes, Add traction-control [testing]

very experimental needs testing/debugging,
other control modes can currently be selected by editing the class definition in motorctl.hpp

menu/config
  - add menu item to enable/disable traction control system

main: pass ptr to other motor to motor object

speedsensor: add method to get last update time

motorctl: handle loop:
  - re-arrange some code sections
  - add several methods to get current status (needed from other motor for tcs)
  - add sketchy code for different control modes DUTY, CURRENT, SPEED (very basic implementation)
  - add experimental code for traction control

board_single/main/config.cpp

@@ -45,6 +45,13 @@ void setLoglevels(void)
     esp_log_level_set("chair-adjustment", ESP_LOG_INFO);
     esp_log_level_set("menu", ESP_LOG_INFO);
     esp_log_level_set("encoder", ESP_LOG_INFO);
+
+
+
+    esp_log_level_set("TESTING", ESP_LOG_VERBOSE);
+
+
+
 }
 
 //==================================
@@ -94,6 +101,7 @@ motorctl_config_t configMotorControlLeft = {
     .msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%)
     .msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%)
     .currentLimitEnabled = false,
+    .tractionControlSystemEnabled = false,
     .currentSensor_adc = ADC1_CHANNEL_4, // GPIO32
     .currentSensor_ratedCurrent = 50,
     .currentMax = 30,
@@ -108,6 +116,7 @@ motorctl_config_t configMotorControlRight = {
     .msFadeAccel = 1500, // acceleration of the motor (ms it takes from 0% to 100%)
     .msFadeDecel = 1000, // deceleration of the motor (ms it takes from 100% to 0%)
     .currentLimitEnabled = false,
+    .tractionControlSystemEnabled = false,
     .currentSensor_adc = ADC1_CHANNEL_5, // GPIO33
     .currentSensor_ratedCurrent = 50,
     .currentMax = 30,

board_single/main/main.cpp

@@ -142,16 +142,16 @@ void createObjects()
     // with configuration above
 	//sabertoothDriver = new sabertooth2x60a(sabertoothConfig);
 
-	// create controlled motor instances (motorctl.hpp)
-    // with configurations from config.cpp
-    motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle);
-    motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle);
-
     // create speedsensor instances
     // with configurations from config.cpp
     speedLeft = new speedSensor(speedLeft_config);
     speedRight = new speedSensor(speedRight_config);
 
+	// create controlled motor instances (motorctl.hpp)
+    // with configurations from config.cpp
+    motorLeft = new controlledMotor(setLeftFunc, configMotorControlLeft, &nvsHandle, speedLeft, &motorRight); //note: ptr to ptr of controlledMotor since it isnt defined yet
+    motorRight = new controlledMotor(setRightFunc, configMotorControlRight, &nvsHandle, speedRight, &motorLeft);
+
     // create joystick instance (joystick.hpp)
     joystick = new evaluatedJoystick(configJoystick, &nvsHandle);
 

board_single/main/menu.cpp

@@ -343,6 +343,42 @@ menuItem_t item_decelLimit = {
 };
 
 
+//###################################
+//##### Traction Control System #####
+//###################################
+void tractionControlSystem_action(display_task_parameters_t * objects, SSD1306_t * display, int value)
+{
+    if (value == 1){
+    objects->motorLeft->enableTractionControlSystem();
+    objects->motorRight->enableTractionControlSystem();
+    ESP_LOGW(TAG, "enabled Traction Control System");
+    } else {
+    objects->motorLeft->disableTractionControlSystem();
+    objects->motorRight->disableTractionControlSystem();
+    ESP_LOGW(TAG, "disabled Traction Control System");
+    }
+}
+int tractionControlSystem_currentValue(display_task_parameters_t * objects)
+{
+    return (int)objects->motorLeft->getTractionControlSystemStatus();
+}
+menuItem_t item_tractionControlSystem = {
+    tractionControlSystem_action,       // function action
+    tractionControlSystem_currentValue, // function get initial value or NULL(show in line 2)
+    NULL,                 // function get default value or NULL(dont set value, show msg)
+    0,                    // valueMin
+    1,                    // valueMax
+    1,                    // valueIncrement
+    "TCS / ASR       ",   // title
+    "Traction Control",   // line1 (above value)
+    "     System     ",   // line2 (above value)
+    "",                   // line4 * (below value)
+    "",                   // line5 *
+    "1: enable       ",   // line6
+    "0: disable      ",   // line7
+};
+
+
 //#####################
 //####### RESET #######
 //#####################
@@ -472,8 +508,8 @@ menuItem_t item_last = {
 //####################################################
 //### store all configured menu items in one array ###
 //####################################################
-const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_maxDuty, item_accelLimit, item_decelLimit, item_statusScreen, item_reset, item_example, item_last};
+const menuItem_t menuItems[] = {item_centerJoystick, item_calibrateJoystick, item_debugJoystick, item_statusScreen, item_accelLimit, item_decelLimit, item_tractionControlSystem, item_reset, item_example, item_last};
-const int itemCount = 8;
+const int itemCount = 9;
 
 
 

common/motorctl.cpp

@@ -28,7 +28,8 @@ void task_motorctl( void * ptrControlledMotor ){
 //======== constructor ========
 //=============================
 //constructor, simultaniously initialize instance of motor driver 'motor' and current sensor 'cSensor' with provided config (see below lines after ':')
-controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc,  motorctl_config_t config_control, nvs_handle_t * nvsHandle_f): 
+controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc,  motorctl_config_t config_control, nvs_handle_t * nvsHandle_f, speedSensor * speedSensor_f, controlledMotor ** otherMotor_f):
+    //create current sensor
 	cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent, config_control.currentSnapToZeroThreshold, config_control.currentInverted) {
 		//copy parameters for controlling the motor
 		config = config_control;
@@ -36,6 +37,10 @@ controlledMotor::controlledMotor(motorSetCommandFunc_t setCommandFunc,  motorctl
 		motorSetCommand = setCommandFunc;
         //pointer to nvs handle
         nvsHandle = nvsHandle_f;
+        //pointer to other motor object
+        ppOtherMotor = otherMotor_f;
+        //pointer to speed sensor
+        sSensor = speedSensor_f;
 
         //create queue, initialize config values
 		init();
@@ -105,7 +110,7 @@ void controlledMotor::handle(){
 
     //TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor
 
-    //--- receive commands from queue ---
+    //--- RECEIVE DATA FROM QUEUE ---
     if( xQueueReceive( commandQueue, &commandReceive, timeoutWaitForCommand / portTICK_PERIOD_MS ) ) //wait time is always 0 except when at target duty already
     {
         ESP_LOGV(TAG, "[%s] Read command from queue: state=%s, duty=%.2f", config.name, motorstateStr[(int)commandReceive.state], commandReceive.duty);
@@ -114,10 +119,29 @@ void controlledMotor::handle(){
 		receiveTimeout = false;
 		timestamp_commandReceived = esp_log_timestamp();
 
+    }
+
+
+
+
+
+// ----- EXPERIMENTAL, DIFFERENT MODES -----
+// define target duty differently depending on current contro-mode
+#define CURRENT_CONTROL_ALLOWED_AMPERE_DIFF 2
+#define SPEED_CONTROL_MAX_SPEED_KMH 9
+#define SPEED_CONTROL_ALLOWED_KMH_DIFF 1
+//declare variables used inside switch
+float ampereNow, ampereTarget, ampereDiff;
+float speedDiff;
+    switch (mode)
+    {
+    case motorControlMode_t::DUTY: // regulate to desired duty (as originally)
         //--- convert duty ---
         // define target duty (-100 to 100) from provided duty and motorstate
-        //this value is more suitable for the fading algorithm
+        // this value is more suitable for t
-        switch(commandReceive.state){
+        // todo scale target input with DUTY-MAX here instead of in joysick cmd generationhe fading algorithm
+        switch (commandReceive.state)
+        {
         case motorstate_t::BRAKE:
             // update state
             state = motorstate_t::BRAKE;
@@ -134,29 +158,96 @@ void controlledMotor::handle(){
             dutyTarget = -fabs(commandReceive.duty);
             break;
         }
+        break;
+
+    case motorControlMode_t::CURRENT: // regulate to desired current flow
+        ampereNow = cSensor.read();
+        ampereTarget = config.currentMax * commandReceive.duty / 100; // TODO ensure input data is 0-100 (no duty max), add currentMax to menu/config
+        ampereDiff = ampereTarget - ampereNow;
+        ESP_LOGV("TESTING", "CURRENT-CONTROL: ampereNow=%.2f, ampereTarget=%.2f, diff=%.2f", ampereNow, ampereTarget, ampereDiff); // todo handle brake
+        if (fabs(ampereDiff) > CURRENT_CONTROL_ALLOWED_AMPERE_DIFF)
+        {
+            if (ampereDiff > 0 && commandReceive.state == motorstate_t::FWD) // forward need to increase current
+            {
+                dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times
+            }
+            else if (ampereDiff < 0 && commandReceive.state == motorstate_t::REV) // backward need to increase current (more negative)
+            {
+                dutyTarget = -100;
+            }
+            else // fwd too much, rev too much -> decrease
+            {
+                dutyTarget = 0;
+            }
+            ESP_LOGV("TESTING", "CURRENT-CONTROL: set target to %.0f%%", dutyTarget);
+        }
+        else
+        {
+            dutyTarget = dutyNow; // target current reached
+            ESP_LOGD("TESTING", "CURRENT-CONTROL: target current %.3f reached", dutyTarget);
+        }
+        break;
+
+    case motorControlMode_t::SPEED: // regulate to desired speed
+        speedNow = sSensor->getKmph();
+        speedTarget = SPEED_CONTROL_MAX_SPEED_KMH * commandReceive.duty / 100; // TODO add maxSpeed to config
+        // target speed negative when driving reverse
+        if (commandReceive.state == motorstate_t::REV)
+            speedTarget = -speedTarget;
+        speedDiff = speedTarget - speedNow;
+        ESP_LOGV("TESTING", "SPEED-CONTROL: target-speed=%.2f, current-speed=%.2f, diff=%.3f", speedTarget, speedNow, speedDiff);
+        if (fabs(speedDiff) > SPEED_CONTROL_ALLOWED_KMH_DIFF)
+        {
+            if (speedDiff > 0 && commandReceive.state == motorstate_t::FWD) // forward need to increase speed
+            {
+                // TODO retain max duty here
+                dutyTarget = 100; // todo add custom fading depending on diff? currently very dependent of fade times
+            }
+            else if (speedDiff < 0 && commandReceive.state == motorstate_t::REV) // backward need to increase speed (more negative)
+            {
+                dutyTarget = -100;
+            }
+            else // fwd too much, rev too much -> decrease
+            {
+                dutyTarget = 0;
+            }
+            ESP_LOGV("TESTING", "CURRENT-CONTROL: set target to %.0f%%", dutyTarget);
+        }
+        else
+        {
+            dutyTarget = dutyNow; // target current reached
+            ESP_LOGD("TESTING", "SPEED-CONTROL: target speed %.3f reached", speedTarget);
         }
 
-    //--- timeout, no data ---
+        break;
+    }
+
+
+
+
+//--- TIMEOUT NO DATA ---
 // turn motors off if no data received for a long time (e.g. no uart data or control task offline)
 if (dutyNow != 0 && esp_log_timestamp() - timestamp_commandReceived > TIMEOUT_IDLE_WHEN_NO_COMMAND && !receiveTimeout)
 {
     ESP_LOGE(TAG, "[%s] TIMEOUT, motor active, but no target data received for more than %ds -> switch from duty=%.2f to IDLE", config.name, TIMEOUT_IDLE_WHEN_NO_COMMAND / 1000, dutyTarget);
     receiveTimeout = true;
     state = motorstate_t::IDLE;
-        dutyTarget = 0;
+    dutyTarget = 0; // todo put this in else section of queue (no data received) and add control mode "timeout"?
 }
 
-    //--- calculate difference ---
+
+    //--- CALCULATE DUTY-DIFF ---
         dutyDelta = dutyTarget - dutyNow;
     //positive: need to increase by that value
     //negative: need to decrease
 
-    //--- already at target ---
+
+    //--- DETECT ALREADY AT TARGET ---
     // when already at exact target duty there is no need to run very fast to handle fading
     //-> slow down loop by waiting significantly longer for new commands to arrive
-    if ((dutyDelta == 0 && !config.currentLimitEnabled) || (dutyTarget == 0 && dutyNow == 0)) //when current limit enabled only slow down when duty is 0
+    if ((dutyDelta == 0 && !config.currentLimitEnabled && !config.tractionControlSystemEnabled) || (dutyTarget == 0 && dutyNow == 0)) //when current limit or tcs enabled only slow down when duty is 0
     {
-        //increase timeout once when duty is the same (once)
+        //increase queue timeout when duty is the same (once)
         if (timeoutWaitForCommand == 0)
         { // TODO verify if state matches too?
             ESP_LOGI(TAG, "[%s] already at target duty %.2f, slowing down...", config.name, dutyTarget);
@@ -175,23 +266,6 @@ void controlledMotor::handle(){
     //TODO skip rest of the handle function below using return? Some regular driver updates sound useful though
 
 
-    //--- calculate increment ---
-    //calculate increment for fading UP with passed time since last run and configured fade time
-    int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
-    if (msFadeAccel > 0){
-    dutyIncrementAccel = ( usPassed / ((float)msFadeAccel * 1000) ) * 100; //TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
-    } else {
-        dutyIncrementAccel = 100;
-    }
-
-    //calculate increment for fading DOWN with passed time since last run and configured fade time
-    if (msFadeDecel > 0){
-        dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100; 
-    } else {
-        dutyIncrementDecel = 100;
-    }
-
-
 	//--- BRAKE ---
 	//brake immediately, update state, duty and exit this cycle of handle function
 	if (state == motorstate_t::BRAKE){
@@ -203,9 +277,25 @@ void controlledMotor::handle(){
 	}
 
 
-
-
 	//----- FADING -----
+    //calculate passed time since last run
+    int64_t usPassed = esp_timer_get_time() - timestampLastRunUs;
+
+    //--- calculate increment ---
+    //calculate increment for fading UP with passed time since last run and configured fade time
+    if (tcs_isExceeded) // disable acceleration when slippage is currently detected
+        dutyIncrementAccel = 0;
+    else if (msFadeAccel > 0)
+        dutyIncrementAccel = (usPassed / ((float)msFadeAccel * 1000)) * 100; // TODO define maximum increment - first run after startup (or long) pause can cause a very large increment
+    else //no accel limit (immediately set to 100)
+        dutyIncrementAccel = 100;
+
+    //calculate increment for fading DOWN with passed time since last run and configured fade time
+    if (msFadeDecel > 0)
+        dutyIncrementDecel = ( usPassed / ((float)msFadeDecel * 1000) ) * 100; 
+    else //no decel limit (immediately reduce to 0)
+        dutyIncrementDecel = 100;
+    
     //fade duty to target (up and down)
     //TODO: this needs optimization (can be more clear and/or simpler)
     if (dutyDelta > 0) { //difference positive -> increasing duty (-100 -> 100)
@@ -245,6 +335,72 @@ void controlledMotor::handle(){
 	}
 
 
+    //----- TRACTION CONTROL -----
+    //reduce duty when turning faster than expected
+    //TODO only run this when speed sensors actually updated
+    //handle tcs when enabled and new speed sensor data is available  TODO: currently assumes here that speed sensor data of other motor updated as well
+    #define TCS_MAX_ALLOWED_RATIO_DIFF 0.1 //when motor speed ratio differs more than that, one motor is slowed down
+    if (config.tractionControlSystemEnabled && sSensor->getTimeLastUpdate() != tcs_timestampLastSpeedUpdate){
+        //update last speed update received
+        tcs_timestampLastSpeedUpdate = sSensor->getTimeLastUpdate(); //TODO: re-use tcs_timestampLastRun in if statement, instead of having additional variable SpeedUpdate
+
+        //calculate time passed since last run
+        uint32_t tcs_usPassed = esp_timer_get_time() - tcs_timestampLastRun; // passed time since last time handled
+        tcs_timestampLastRun = esp_timer_get_time();
+
+        //get motor stats
+        float speedNowThis = sSensor->getRpm();
+        float speedNowOther = (*ppOtherMotor)->getDuty();
+        float speedTargetThis = speedTarget;
+        float speedTargetOther = (*ppOtherMotor)->getTargetSpeed();
+        float dutyTargetOther = (*ppOtherMotor)->getTargetDuty();
+        float dutyTargetThis = dutyTarget;
+        float dutyNowOther = (*ppOtherMotor)->getDuty();
+        float dutyNowThis = dutyNow;
+
+
+        //calculate expected ratio
+        float ratioSpeedTarget = speedTargetThis / speedTargetOther;
+        //calculate current ratio of actual measured rotational speed
+        float ratioSpeedNow = speedNowThis / speedNowOther;
+        //calculate current duty ration (logging only)
+        float ratioDutyNow = dutyNowThis / dutyNowOther;
+
+        //calculate unexpected difference
+        float ratioDiff = ratioSpeedNow - ratioSpeedTarget;
+        ESP_LOGD("TESTING", "[%s] TCS: ratioSpeedTarget=%.3f, ratioSpeedNow=%.3f, ratioDutyNow=%.3f, diff=%.3f", config.name, ratioSpeedTarget, ratioSpeedNow, ratioDutyNow, ratioDiff);
+
+        //-- handle rotating faster than expected --
+        //TODO also increase duty when other motor is slipping? (diff negative)
+        if (ratioDiff > TCS_MAX_ALLOWED_RATIO_DIFF) // motor turns too fast compared to expected target ratio
+        {
+            if (!tcs_isExceeded) // just started being too fast
+            {
+                tcs_timestampBeginExceeded = esp_timer_get_time();
+                tcs_isExceeded = true; //also blocks further acceleration (fade)
+                ESP_LOGW("TESTING", "[%s] TCS: now exceeding max allowed ratio diff! diff=%.2f max=%.2f", config.name, ratioDiff, TCS_MAX_ALLOWED_RATIO_DIFF);
+            }
+            else
+            { // too fast for more than 2 cycles already
+                tcs_usExceeded = esp_timer_get_time() - tcs_timestampBeginExceeded; //time too fast already
+                ESP_LOGI("TESTING", "[%s] TCS: faster than expected since %dms, current ratioDiff=%.2f  -> slowing down", config.name, tcs_usExceeded/1000, ratioDiff);
+                // calculate amount duty gets decreased
+                float dutyDecrement = (tcs_usPassed / ((float)msFadeDecel * 1000)) * 100; //TODO optimize dynamic increment: P:scale with ratio-difference, I: scale with duration exceeded
+                // decrease duty
+                ESP_LOGI("TESTING", "[%s] TCS: msPassed=%.3f, reducing duty by %.3f%%", config.name, (float)tcs_usPassed/1000, dutyDecrement);
+                fade(&dutyNow, 0, -dutyDecrement); //reduce duty but not less than 0
+            }
+        }
+        else
+        { // not exceeded
+            tcs_isExceeded = false;
+            tcs_usExceeded = 0;
+        }
+    }
+
+	
+
+
     //--- define new motorstate --- (-100 to 100 => direction)
 	state=getStateFromDuty(dutyNow);
 
@@ -254,6 +410,7 @@ void controlledMotor::handle(){
 	//FWD -> IDLE -> FWD  continue without waiting
 	//FWD -> IDLE -> REV  wait for dead-time in IDLE
 	//TODO check when changed only?
+    if (config.deadTimeMs > 0) { //deadTime is enabled
 	    if (	//not enough time between last direction state
 	    		(   state == motorstate_t::FWD && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::REV] < config.deadTimeMs))
 	    		|| (state == motorstate_t::REV && (esp_log_timestamp() - timestampsModeLastActive[(int)motorstate_t::FWD] < config.deadTimeMs))
@@ -273,6 +430,7 @@ void controlledMotor::handle(){
 	    	}
 	    	ESP_LOGV(TAG, "deadtime: no change below deadtime detected... dir=%s, duty=%.1f", motorstateStr[(int)state], dutyNow);
 	    }
+    }
 			
 
 	//--- save current actual motorstate and timestamp ---

common/motorctl.hpp

@@ -13,6 +13,7 @@ extern "C"
 
 #include "motordrivers.hpp"
 #include "currentsensor.hpp"
+#include "speedsensor.hpp"
 
 
 //=======================================
@@ -30,11 +31,18 @@ typedef void (*motorSetCommandFunc_t)(motorCommand_t cmd);
 class controlledMotor {
     public:
         //--- functions ---
-        controlledMotor(motorSetCommandFunc_t setCommandFunc,  motorctl_config_t config_control, nvs_handle_t * nvsHandle); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor
+        //TODO move speedsensor object creation in this class to (pass through / wrap methods)
+        controlledMotor(motorSetCommandFunc_t setCommandFunc,  motorctl_config_t config_control, nvs_handle_t * nvsHandle, speedSensor * speedSensor, controlledMotor ** otherMotor); //constructor with structs for configuring motordriver and parameters for control TODO: add configuration for currentsensor
         void handle(); //controls motor duty with fade and current limiting feature (has to be run frequently by another task)
         void setTarget(motorstate_t state_f, float duty_f = 0); //adds target command to queue for handle function
         void setTarget(motorCommand_t command); 
         motorCommand_t getStatus(); //get current status of the motor (returns struct with state and duty)
+        float getDuty() {return dutyNow;};
+        float getTargetDuty() {return dutyTarget;};
+        float getTargetSpeed() {return speedTarget;};
+        void enableTractionControlSystem() {config.tractionControlSystemEnabled = true;};
+        void disableTractionControlSystem() {config.tractionControlSystemEnabled = false; tcs_isExceeded = false;};
+        bool getTractionControlSystemStatus() {return config.tractionControlSystemEnabled;};
 
         uint32_t getFade(fadeType_t fadeType); //get currently set acceleration or deceleration fading time
         uint32_t getFadeDefault(fadeType_t fadeType); //get acceleration or deceleration fading time from config
@@ -60,6 +68,11 @@ class controlledMotor {
         QueueHandle_t commandQueue = NULL;
 		//current sensor
 		currentSensor cSensor;
+        //speed sensor
+        speedSensor * sSensor;
+        //other motor (needed for traction control)
+        controlledMotor ** ppOtherMotor; //ptr to ptr of controlledMotor (because not created at initialization yet)
+
 
 		//function pointer that sets motor duty (driver)
 		motorSetCommandFunc_t motorSetCommand;
@@ -69,14 +82,21 @@ class controlledMotor {
         //struct for storing control specific parameters
         motorctl_config_t config;
         motorstate_t state = motorstate_t::IDLE;
+        motorControlMode_t mode = motorControlMode_t::DUTY;
         //handle for using the nvs flash (persistent config variables)
         nvs_handle_t * nvsHandle;
 
         float currentMax;
         float currentNow;
 
+        //speed mode
+        float speedTarget = 0;
+        float speedNow = 0;
+
+
         float dutyTarget = 0;
         float dutyNow = 0;
+
         float dutyIncrementAccel;
         float dutyIncrementDecel;
         float dutyDelta;
@@ -97,6 +117,13 @@ class controlledMotor {
 
 		uint32_t timestamp_commandReceived = 0;
 		bool receiveTimeout = false;
+
+        //traction control system
+        uint32_t tcs_timestampLastSpeedUpdate = 0; //track speedsensor update
+        int64_t tcs_timestampBeginExceeded = 0; //track start of event
+        uint32_t tcs_usExceeded = 0; //sum up time
+        bool tcs_isExceeded = false; //is currently too fast
+        int64_t tcs_timestampLastRun = 0;
 };
 
 //====================================

common/speedsensor.cpp

@@ -93,6 +93,7 @@ void IRAM_ATTR onEncoderRising(void *arg)
 		// calculate rotational speed
 		uint64_t pulseSum = pulse1 + pulse2 + pulse3;
 		sensor->currentRpm = direction * (sensor->config.degreePerGroup / 360.0 * 60.0 / ((double)pulseSum / 1000000.0));
+		sensor->timeLastUpdate = currentTime;
 	}
 }
 

common/speedsensor.hpp

@@ -33,6 +33,7 @@ public:
 	float getKmph(); //kilometers per hour
 	float getMps(); //meters per second
 	float getRpm();  //rotations per minute
+	float getTimeLastUpdate() {return timeLastUpdate;};
 
 	//variables for handling the encoder (public because ISR needs access)
 	speedSensor_config_t config;
@@ -45,6 +46,7 @@ public:
 	int debugCount = 0;
 	uint32_t debug_countIgnoredSequencesTooShort = 0;
 	double currentRpm = 0;
+	uint32_t timeLastUpdate = 0;
 
 private:
 	static bool isrIsInitialized; // default false due to static

common/types.hpp

@@ -22,6 +22,7 @@ enum class motorstate_t {IDLE, FWD, REV, BRAKE};
 //definition of string array to be able to convert state enum to readable string (defined in motordrivers.cpp)
 extern const char* motorstateStr[4];
 
+enum class motorControlMode_t {DUTY, CURRENT, SPEED};
 
 
 //===========================
@@ -45,6 +46,7 @@ typedef struct motorctl_config_t {
     uint32_t msFadeAccel; //acceleration of the motor (ms it takes from 0% to 100%)
     uint32_t msFadeDecel; //deceleration of the motor (ms it takes from 100% to 0%)
 	bool currentLimitEnabled;
+    bool tractionControlSystemEnabled;
 	adc1_channel_t currentSensor_adc;
 	float currentSensor_ratedCurrent;
     float currentMax;