Add currentsensor support - current limit functional

- add currentsensor class
- add current limit to motorctl handle function
- add config options for curring limit for each motor
- update connection plan with more detailed driver-box overview

main/CMakeLists.txt

@@ -12,6 +12,7 @@ idf_component_register(
         "wifi.c"
         "http.cpp"
         "auto.cpp"
+		"currentsensor.cpp"
     INCLUDE_DIRS 
         "."
     )

main/config.cpp

@@ -29,16 +29,28 @@ single100a_config_t configDriverRight = {
     .pwmFreq = 10000
 };
 
+//TODO add motor name string -> then use as log tag?
 //--- configure motor contol ---
-motorctl_config_t configMotorControl = {
+motorctl_config_t configMotorControlLeft = {
-    .msFadeAccel = 2400, //acceleration of the motor (ms it takes from 0% to 100%)
+    .msFadeAccel = 1900, //acceleration of the motor (ms it takes from 0% to 100%)
     .msFadeDecel = 1000, //deceleration of the motor (ms it takes from 100% to 0%)
-    .currentMax = 10
+	.currentLimitEnabled = true,
+	.currentSensor_adc =  ADC1_CHANNEL_6, //GPIO34
+	.currentSensor_ratedCurrent = 50,
+    .currentMax = 30
+};
+motorctl_config_t configMotorControlRight = {
+    .msFadeAccel = 1900, //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 = true,
+	.currentSensor_adc =  ADC1_CHANNEL_4, //GPIO32
+	.currentSensor_ratedCurrent = 50,
+    .currentMax = 30
 };
 
 //create controlled motor instances
-controlledMotor motorLeft(configDriverLeft, configMotorControl);
+controlledMotor motorLeft(configDriverLeft, configMotorControlLeft);
-controlledMotor motorRight(configDriverRight, configMotorControl);
+controlledMotor motorRight(configDriverRight, configMotorControlRight);
 
 
 
@@ -75,18 +87,18 @@ joystick_config_t configJoystick = {
     .adc_x = ADC1_CHANNEL_3, //GPIO39
     .adc_y = ADC1_CHANNEL_0, //GPIO36
     //percentage of joystick range the coordinate of the axis snaps to 0 (0-100)
-    .tolerance_zeroX_per = 6,
+    .tolerance_zeroX_per = 7, //6
-    .tolerance_zeroY_per = 7,
+    .tolerance_zeroY_per = 10, //7
     //percentage of joystick range the coordinate snaps to -1 or 1 before configured "_max" or "_min" threshold (mechanical end) is reached (0-100)
     .tolerance_end_per = 4, 
     //threshold the radius jumps to 1 before the stick is at max radius (range 0-1)
-    .tolerance_radius = 0.08,
+    .tolerance_radius = 0.09,
 
     //min and max adc values of each axis, !!!AFTER INVERSION!!! is applied:
     .x_min = 1392, //=> x=-1
-    .x_max = 2815, //=> x=1
+    .x_max = 2650, //=> x=1
-    .y_min = 1370, //=> y=-1
+    .y_min = 1390, //=> y=-1
-    .y_max = 2795, //=> y=1
+    .y_max = 2640, //=> y=1
     //invert adc measurement
     .x_inverted = true,
     .y_inverted = true

main/currentsensor.cpp

@@ -0,0 +1,75 @@
+extern "C" {
+#include "hal/timer_types.h"
+#include "esp_log.h"
+}
+
+#include "currentsensor.hpp"
+
+//tag for logging
+static const char * TAG = "current-sensors";
+
+
+
+//--------------------------
+//------- getVoltage -------
+//--------------------------
+//local function to get average voltage from adc
+float getVoltage(adc1_channel_t adc, uint32_t samples){
+	//measure voltage
+	int measure = 0;
+	for (int j=0; j<samples; j++){
+		measure += adc1_get_raw(adc);
+		ets_delay_us(50);
+	}
+	return (float)measure / samples / 4096 * 3.3;
+}
+
+
+
+//=============================
+//======== constructor ========
+//=============================
+currentSensor::currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent_f){
+	//copy config
+	adcChannel = adcChannel_f;
+	ratedCurrent = ratedCurrent_f;
+	//init adc
+	adc1_config_width(ADC_WIDTH_BIT_12); //max resolution 4096
+	adc1_config_channel_atten(adcChannel, ADC_ATTEN_DB_11); //max voltage
+	//calibrate
+	calibrateZeroAmpere();
+}
+
+
+
+//============================
+//=========== read ===========
+//============================
+float currentSensor::read(void){
+	//measure voltage
+	voltage = getVoltage(adcChannel, 30);
+
+	//scale voltage to current
+	if (voltage < centerVoltage){
+		current = (1 - voltage / centerVoltage) * -ratedCurrent;
+	} else if (voltage > centerVoltage){
+		current = (voltage - centerVoltage) / (3.3 - centerVoltage) * ratedCurrent;
+	}else {
+		current = 0;
+	}
+
+	ESP_LOGI(TAG, "read sensor adc=%d: voltage=%.3fV, centerVoltage=%.3fV => current=%.3fA", (int)adcChannel, voltage, centerVoltage, current);
+	return current;
+}
+
+
+
+//===============================
+//===== calibrateZeroAmpere =====
+//===============================
+void currentSensor::calibrateZeroAmpere(void){
+	//measure voltage
+	float prev = centerVoltage;
+	centerVoltage = getVoltage(adcChannel, 100);
+	ESP_LOGW(TAG, "defined centerVoltage (0A) to %.3f (previous %.3f)", centerVoltage, prev);
+}

main/currentsensor.hpp

@@ -0,0 +1,20 @@
+#include <driver/adc.h>
+
+//supported current sensor working method:
+  //0V = -ratedCurrent  
+  //centerVoltage = 0A
+  //3.3V = ratedCurrent
+
+class currentSensor{
+	public:
+		currentSensor (adc1_channel_t adcChannel_f, float ratedCurrent);
+		void calibrateZeroAmpere(void); //set current voltage to voltage representing 0A
+		float read(void); //get current ampere
+	private:
+		adc1_channel_t adcChannel;
+		float ratedCurrent;
+		uint32_t measure;
+		float voltage;
+		float current;
+		float centerVoltage = 3.3/2;
+};

main/main.cpp

@@ -141,8 +141,8 @@ extern "C" void app_main(void) {
     //--- set loglevel for individual tags ---
     esp_log_level_set("main", ESP_LOG_INFO);
     esp_log_level_set("buzzer", ESP_LOG_ERROR);
-    //esp_log_level_set("motordriver", ESP_LOG_DEBUG);
+    //esp_log_level_set("motordriver", ESP_LOG_INFO);
-    esp_log_level_set("motor-control", ESP_LOG_INFO);
+    //esp_log_level_set("motor-control", ESP_LOG_DEBUG);
     //esp_log_level_set("evaluatedJoystick", ESP_LOG_DEBUG);
     //esp_log_level_set("joystickCommands", ESP_LOG_DEBUG);
     esp_log_level_set("button", ESP_LOG_INFO);
@@ -151,6 +151,7 @@ extern "C" void app_main(void) {
     esp_log_level_set("wifi", ESP_LOG_INFO);
     esp_log_level_set("http", ESP_LOG_INFO);
     esp_log_level_set("automatedArmchair", ESP_LOG_DEBUG);
+    //esp_log_level_set("current-sensors", ESP_LOG_INFO);
 
     
     //----------------------------------------------

main/motorctl.cpp

@@ -8,16 +8,19 @@ static const char * TAG = "motor-control";
 //======== constructor ========
 //=============================
 //constructor, simultaniously initialize instance of motor driver 'motor' with provided config (see below line after ':')
-controlledMotor::controlledMotor(single100a_config_t config_driver,  motorctl_config_t config_control): motor(config_driver) {
+controlledMotor::controlledMotor(single100a_config_t config_driver,  motorctl_config_t config_control): 
-    //copy parameters for controlling the motor
+	motor(config_driver), 
-    config = config_control;
+	cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent) {
-    //copy configured default fading durations to actually used variables
+		//copy parameters for controlling the motor
-    msFadeAccel = config.msFadeAccel;
+		config = config_control;
-    msFadeDecel = config.msFadeDecel;
+		//copy configured default fading durations to actually used variables
+		msFadeAccel = config.msFadeAccel;
+		msFadeDecel = config.msFadeDecel;
 
-    init();
+		init();
-    //TODO: add currentsensor object here
+		//TODO: add currentsensor object here
-}
+		//currentSensor cSensor(config.currentSensor_adc, config.currentSensor_ratedCurrent);
+	}
 
 
 
@@ -26,6 +29,7 @@ controlledMotor::controlledMotor(single100a_config_t config_driver,  motorctl_co
 //============================
 void controlledMotor::init(){
     commandQueue = xQueueCreate( 1, sizeof( struct motorCommand_t ) );
+	//cSensor.calibrateZeroAmpere(); //TODO do this regularly e.g. in idle?
 }
 
 
@@ -56,9 +60,8 @@ void fade(float * dutyNow, float dutyTarget, float dutyIncrement){
 //function that controls the motor driver and handles fading/ramp and current limit
 void controlledMotor::handle(){
 
-    //TODO: current sensor
     //TODO: delay when switching direction?
-    //TODO: History: skip fading when motor was running fast recently
+    //TODO: History: skip fading when motor was running fast recently / alternatively add rot-speed sensor
 
     //--- receive commands from queue ---
     if( xQueueReceive( commandQueue, &commandReceive, ( TickType_t ) 0 ) )
@@ -123,7 +126,8 @@ void controlledMotor::handle(){
 
 
 
-    //--- fade duty to target (up and down) ---
+	//----- fading -----
+    //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)
         if (dutyNow < 0) { //reverse, decelerating
@@ -143,7 +147,6 @@ void controlledMotor::handle(){
     }
 
 
-
     //previous approach: (resulted in bug where accel/decel fade is swaped in reverse)
 //    //--- fade up ---
 //    //dutyDelta is higher than IncrementUp -> fade up
@@ -168,6 +171,24 @@ void controlledMotor::handle(){
 //    }
     
 
+	//----- current limit -----
+	if ((config.currentLimitEnabled) && (dutyDelta != 0)){
+		currentNow = cSensor.read();
+		if (fabs(currentNow) > config.currentMax){
+			float dutyOld = dutyNow;
+			//adaptive decrement:
+			//Note current exceeded twice -> twice as much decrement: TODO: decrement calc needs finetuning, currently random values
+			dutyIncrementDecel = (currentNow/config.currentMax) * ( usPassed / ((float)msFadeDecel * 1500) ) * 100; 
+			float currentLimitDecrement = ( (float)usPassed / ((float)1000 * 1000) ) * 100; //1000ms from 100 to 0
+			if (dutyNow < -currentLimitDecrement) {
+				dutyNow += currentLimitDecrement;
+			} else if (dutyNow > currentLimitDecrement) {
+				dutyNow -= currentLimitDecrement;
+			}
+			ESP_LOGW(TAG, "current limit exceeded! now=%.3fA max=%.1fA => decreased duty from %.3f to %.3f", currentNow, config.currentMax, dutyOld, dutyNow);
+		}
+	}
+
 
     //define motorstate from converted duty -100 to 100
     //apply target duty and state to motor driver

main/motorctl.hpp

@@ -10,6 +10,7 @@ extern "C"
 }
 
 #include "motordrivers.hpp"
+#include "currentsensor.hpp"
 
 
 //-------------------------------------
@@ -32,6 +33,9 @@ typedef struct motorCommands_t {
 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;
+	adc1_channel_t currentSensor_adc;
+	float currentSensor_ratedCurrent;
     float currentMax;
 } motorctl_config_t;
 
@@ -52,6 +56,8 @@ class controlledMotor {
         void setFade(fadeType_t fadeType, bool enabled); //enable/disable acceleration or deceleration fading
         void setFade(fadeType_t fadeType, uint32_t msFadeNew); //set acceleration or deceleration fade time
         bool toggleFade(fadeType_t fadeType); //toggle acceleration or deceleration on/off
+											  
+		//TODO set current limit
 
 
     private:
@@ -59,11 +65,12 @@ class controlledMotor {
         void init(); //creates currentsensor objects, motordriver objects and queue
 
         //--- objects ---
-        //TODO: add currentsensor object
         //motor driver
         single100a motor;
         //queue for sending commands to the separate task running the handle() function very fast
         QueueHandle_t commandQueue = NULL;
+		//current sensor
+		currentSensor cSensor;
 
         //--- variables ---
         //struct for storing control specific parameters