diff --git a/connection-plan.drawio.pdf b/connection-plan.drawio.pdf index 28d67e4..6e266c9 100644 Binary files a/connection-plan.drawio.pdf and b/connection-plan.drawio.pdf differ diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 34de854..92ed0e1 100644 --- a/main/CMakeLists.txt +++ b/main/CMakeLists.txt @@ -12,6 +12,7 @@ idf_component_register( "wifi.c" "http.cpp" "auto.cpp" + "currentsensor.cpp" INCLUDE_DIRS "." ) diff --git a/main/config.cpp b/main/config.cpp index e246ef9..518eff5 100644 --- a/main/config.cpp +++ b/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 = { - .msFadeAccel = 2400, //acceleration of the motor (ms it takes from 0% to 100%) +motorctl_config_t configMotorControlLeft = { + .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 motorRight(configDriverRight, configMotorControl); +controlledMotor motorLeft(configDriverLeft, configMotorControlLeft); +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_zeroY_per = 7, + .tolerance_zeroX_per = 7, //6 + .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 - .y_min = 1370, //=> y=-1 - .y_max = 2795, //=> y=1 + .x_max = 2650, //=> x=1 + .y_min = 1390, //=> y=-1 + .y_max = 2640, //=> y=1 //invert adc measurement .x_inverted = true, .y_inverted = true diff --git a/main/currentsensor.cpp b/main/currentsensor.cpp new file mode 100644 index 0000000..2569c6d --- /dev/null +++ b/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 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); +} diff --git a/main/currentsensor.hpp b/main/currentsensor.hpp new file mode 100644 index 0000000..f62fa34 --- /dev/null +++ b/main/currentsensor.hpp @@ -0,0 +1,20 @@ +#include + +//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; +}; diff --git a/main/main.cpp b/main/main.cpp index f37675b..15600ee 100644 --- a/main/main.cpp +++ b/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("motor-control", ESP_LOG_INFO); + //esp_log_level_set("motordriver", 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); //---------------------------------------------- diff --git a/main/motorctl.cpp b/main/motorctl.cpp index 62ff8c6..c811133 100644 --- a/main/motorctl.cpp +++ b/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) { - //copy parameters for controlling the motor - config = config_control; - //copy configured default fading durations to actually used variables - msFadeAccel = config.msFadeAccel; - msFadeDecel = config.msFadeDecel; +controlledMotor::controlledMotor(single100a_config_t config_driver, motorctl_config_t config_control): + motor(config_driver), + cSensor(config_control.currentSensor_adc, config_control.currentSensor_ratedCurrent) { + //copy parameters for controlling the motor + config = config_control; + //copy configured default fading durations to actually used variables + msFadeAccel = config.msFadeAccel; + msFadeDecel = config.msFadeDecel; - init(); - //TODO: add currentsensor object here -} + init(); + //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 diff --git a/main/motorctl.hpp b/main/motorctl.hpp index 322b0f5..2e1b606 100644 --- a/main/motorctl.hpp +++ b/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