From 1d7a23cb83df214d5b78594ea8d40934c7507057 Mon Sep 17 00:00:00 2001 From: Li Junru Date: Tue, 12 Dec 2023 10:41:17 +0800 Subject: [PATCH] feature: add foc knob --- .../motor/esp_simplefoc/idf_component.yml | 1 + examples/motor/foc_knob_example/README.md | 38 +- .../components/foc_knob/Kconfig | 10 + .../components/foc_knob/README.md | 31 +- .../components/foc_knob/foc_knob.c | 406 +++++++++++------- .../components/foc_knob/foc_knob_default.c | 50 +-- .../components/foc_knob/include/foc_knob.h | 171 ++++++-- .../foc_knob/include/foc_knob_default.h | 19 +- .../foc_knob_example/main/idf_component.yml | 2 +- examples/motor/foc_knob_example/main/main.cpp | 122 ++++-- examples/motor/foc_velocity_control/README.md | 26 +- .../motor/foc_velocity_control/README_cn.md | 18 +- 12 files changed, 588 insertions(+), 306 deletions(-) create mode 100644 examples/motor/foc_knob_example/components/foc_knob/Kconfig diff --git a/components/motor/esp_simplefoc/idf_component.yml b/components/motor/esp_simplefoc/idf_component.yml index 51764c28b..44dde4277 100644 --- a/components/motor/esp_simplefoc/idf_component.yml +++ b/components/motor/esp_simplefoc/idf_component.yml @@ -20,3 +20,4 @@ dependencies: examples: - path: ../../../examples/motor/foc_openloop_control - path: ../../../examples/motor/foc_velocity_control + - path: ../../../examples/motor/foc_knob_example diff --git a/examples/motor/foc_knob_example/README.md b/examples/motor/foc_knob_example/README.md index a6caa9209..5efddd8dc 100644 --- a/examples/motor/foc_knob_example/README.md +++ b/examples/motor/foc_knob_example/README.md @@ -1,25 +1,21 @@ -| Supported Targets |ESP32-S3 | ESP32-C3 | -| ----------------- |-------- |----------| - -# ESP FOC Knob -This example demonstrates the application of the ESP-32S3 microcontroller to control a motor, effectively transforming it into a knob-like interface. +# ESP FOC Knob +This example demonstrates the application of the ESP32-S3 microcontroller to control a motor, effectively transforming it into a knob-like interface. ## Modes supported -| Mode | Description | -|-----------------------------------|------------------------------------------------------------| -| 0. Fine Value with Detents | Motor can be unbounded with fine rotation and fine dents points. | -| 1. Unbounded, No Detents | Motor can be unbounded with fine rotation and no dent points. | -| 2. Super Dial | Motor can be unbound with fine rotation and noticeable dent points. | -| 3. Fine Values with Detents, Unbounded | Motor can be unbounded with fine rotation and noticeable dents points. | -| 4. Bounded 0-13, No Detents | Motor will be constrained within a specific range (180-360) with fine rotation and no detent points. | -| 5. Coarse Values, Strong Detents | Motor can be unbounded with coarser rotation and strong dent points. | -| 6. Fine Values, No Detents | Motor will be constrained within a specific range (256, 127), fine rotation, no dent points | -| 7. On/Off, Strong Detents | Motor will be constrained within a specific range presents a On/off functionality with noticeable stopping points. | +| Mode | Description | +| --------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| 0. unbound no detents | Motor can be unbounded with fine rotation and no dent points. | +| 1. Unbounded Fine detents | Motor can be unbounded with fine rotation and fine dent points. | +| 2. Unbounded Strong detents | Motor can be unbound with fine rotation and noticeable dent points. | +| 3. Bounded No detents | Motor will be constrained within a specific range (0-360) with fine rotation and no detent points. | +| 4. Bounded Fine detents | Motor can be constrained with coarser rotation and fine dent points. | +| 5. Bounded Strong detents | Motor can be constrained with coarser rotation and strong dent points. | +| 6. On/Off strong detents | Motor will be constrained within a specific range presents a On/off functionality with noticeable stopping points. | However, users have the flexibility to adjust mode parameters to attain various responses. Within the following structure, users can modify these values to customize the functionality according to their specific requirements. -``` +```c typedef struct { int32_t num_positions; // Number of positions int32_t position; // Current position @@ -32,10 +28,10 @@ typedef struct { ``` -### Hardware Required +### Hardware Required This example is specifically designed for ESP32-S3-Motor-LCDkit. -Other components +Other components 1. The BLDC Motor Model 5v 2804 is compatible and can be employed. 2. A Position Sensor (Hall-based) such as MT6701 or AS5600 is suitable for use. @@ -53,9 +49,11 @@ step2: build the project idf.py build ``` -step 3: Flash and monitor +step 3: Flash and monitor Flash the program and launch IDF Monitor: ```bash idf.py flash monitor -``` \ No newline at end of file +``` + +![FOC Knob exmaple](https://dl.espressif.com/ae/esp-iot-solution/foc_knob.gif) \ No newline at end of file diff --git a/examples/motor/foc_knob_example/components/foc_knob/Kconfig b/examples/motor/foc_knob_example/components/foc_knob/Kconfig new file mode 100644 index 000000000..676741a3d --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/Kconfig @@ -0,0 +1,10 @@ +menu "FOC Knob" + + config FOC_KNOB_MAX_VELOCITY + int "FOC Knob Max Velocity" + range 1 100 + default 23 + help + When the maximum velocity is smaller, the rebound velocity will also be smaller. + +endmenu diff --git a/examples/motor/foc_knob_example/components/foc_knob/README.md b/examples/motor/foc_knob_example/components/foc_knob/README.md index b89c06ffb..54b5afe47 100644 --- a/examples/motor/foc_knob_example/components/foc_knob/README.md +++ b/examples/motor/foc_knob_example/components/foc_knob/README.md @@ -1,23 +1,22 @@ -# Component FOC Knob +# Component FOC Knob The FOC knob is a component that offers an extensive API, empowering the user to define various modes of operation. Users have the flexibility to employ different types of BLDC motors to achieve the desired functionality of a haptic feedback knob. ## Modes supported By default, certain modes are pre-defined in the **foc_knob_default.c** file. Users have the option to leverage these default modes for their applications. -| Mode | Description | -|-----------------------------------|------------------------------------------------------------| -| 0. Fine Value with Detents | Motor can be unbounded with fine rotation and fine dents points. | -| 1. Unbounded, No Detents | Motor can be unbounded with fine rotation and no dent points. | -| 2. Super Dial | Motor can be unbound with fine rotation and noticeable dent points. | -| 3. Fine Values with Detents, Unbounded | Motor can be unbounded with fine rotation and noticeable dents points. | -| 4. Bounded 0-13, No Detents | Motor will be constrained within a specific range (180-360) with fine rotation and no detent points. | -| 5. Coarse Values, Strong Detents | Motor can be unbounded with coarser rotation and strong dent points. | -| 6. Fine Values, No Detents | Motor will be constrained within a specific range (256, 127), fine rotation, no dent points | -| 7. On/Off, Strong Detents | Motor will be constrained within a specific range presents a On/off functionality with noticeable stopping points. | +| Mode | Description | +| --------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| 0. unbound no detents | Motor can be unbounded with fine rotation and no dent points. | +| 1. Unbounded Fine detents | Motor can be unbounded with fine rotation and fine dent points. | +| 2. Unbounded Strong detents | Motor can be unbound with fine rotation and noticeable dent points. | +| 3. Bounded No detents | Motor will be constrained within a specific range (0-360) with fine rotation and no detent points. | +| 4. Bounded Fine detents | Motor can be constrained with coarser rotation and fine dent points. | +| 5. Bounded Strong detents | Motor can be constrained with coarser rotation and strong dent points. | +| 6. On/Off strong detents | Motor will be constrained within a specific range presents a On/off functionality with noticeable stopping points. | However, users have the flexibility to adjust mode parameters to attain various responses. Within the following structure, users can modify these values to customize the functionality according to their specific requirements. -``` +```c typedef struct { int32_t num_positions; // Number of positions int32_t position; // Current position @@ -26,15 +25,15 @@ typedef struct { float endstop_strength_unit; // Strength of detent when reaching the end float snap_point; // Snap point for each position const char *descriptor; // Description -} knob_param_t; +} foc_knob_param_t; ``` -### Hardware Required +### Hardware Required This example is specifically designed for ESP32-S3-Motor-LCDkit. -Other components +Other components 1. The BLDC Motor Model 5v 2804 is compatible and can be employed. 2. A Position Sensor (Hall-based) such as MT6701 or AS5600 is suitable for use. -### Example +### Example [FOC_knob_example](../../../foc_knob_example/) \ No newline at end of file diff --git a/examples/motor/foc_knob_example/components/foc_knob/foc_knob.c b/examples/motor/foc_knob_example/components/foc_knob/foc_knob.c index b0932e191..70a34a794 100644 --- a/examples/motor/foc_knob_example/components/foc_knob/foc_knob.c +++ b/examples/motor/foc_knob_example/components/foc_knob/foc_knob.c @@ -9,215 +9,313 @@ #include #include "freertos/FreeRTOS.h" #include "freertos/task.h" +#include "freertos/semphr.h" #include "esp_log.h" #include "esp_timer.h" -#include "math.h" -#include "pid_ctrl.h" +#include "esp_check.h" #include "foc_knob.h" #include "foc_knob_default.h" +#include "pid_ctrl.h" +#include "math.h" -static const char *TAG = "FOC-Knob"; +static const char *TAG = "FOC_Knob"; -/* Macro for checking a condition and logging an error */ -#define KNOB_CHECK(a, str, action) if(!(a)) { \ - ESP_LOGE(TAG,"%s:%d (%s):%s", __FILE__, __LINE__, __FUNCTION__, str); \ - action; \ - } +#define IDLE_VELOCITY_EWMA_ALPHA 0.001 +#define IDLE_VELOCITY_RAD_PER_SEC 0.05 +#define IDLE_CORRECTION_DELAY_MILLIS 500 +#define IDLE_CORRECTION_MAX_ANGLE_RAD (5 * PI / 180) +#define IDLE_CORRECTION_RATE_ALPHA 0.0005 +#define DEAD_ZONE_DETENT_PERCENT 0.2 +#define DEAD_ZONE_RAD (1 * PI / 180) +#define MAX_VELOCITY_CONTROL CONFIG_FOC_KNOB_MAX_VELOCITY -/* Structure to store knob state */ -typedef struct { - float idle_check_velocity_ewma; - float current_detent_center; - float angle_to_detent_center; - float torque; - uint32_t last_idle_start; - bool initialSetupDone; - pid_ctrl_block_handle_t pid; -} knob_state_t; - -/* Structure to store a list of knob parameters and the number of lists */ -typedef struct { - knob_param_t const **param_lists; - uint16_t param_list_num; - knob_state_t state; -} _knob_indicator_t; - -/* Structure for common configuration of knob */ -typedef struct _knob_indicator_com_config { - knob_param_t const **param_lists; - uint16_t param_list_num; -} _knob_indicator_com_config_t; - -/* Function to create a knob using a common configuration */ -static _knob_indicator_t *_knob_indicator_create_com(_knob_indicator_com_config_t *cfg) -{ - KNOB_CHECK(NULL != cfg, "com config can't be NULL!", return NULL); +#define CLAMP(value, low, high) ((value) < (low) ? (low) : ((value) > (high) ? (high) : (value))) - _knob_indicator_t *p_knob_indicator = (_knob_indicator_t *)calloc(1, sizeof(_knob_indicator_t)); - KNOB_CHECK(p_knob_indicator != NULL, "calloc indicator memory failed", return NULL); +#define CALL_EVENT_CB(ev) if(p_knob->cb[ev])p_knob->cb[ev](p_knob, p_knob->usr_data[ev]) - p_knob_indicator->param_lists = cfg->param_lists; - p_knob_indicator->param_list_num = cfg->param_list_num; - return p_knob_indicator; -} +/* Structure to store a list of knob parameters and the number of lists */ +typedef struct { + foc_knob_param_t const **param_lists; + uint16_t param_list_num; + pid_ctrl_block_handle_t pid; + float max_torque_out_limit; + float max_torque; + float idle_check_velocity_ewma; + float current_detent_center; + uint32_t last_idle_start; + int current_mode; + bool center_adjusted; + float angle_to_detent_center; + foc_knob_event_t event; + SemaphoreHandle_t mutex; /*!< Mutex to achieve thread-safe */ + int32_t *position; /*!< Positions for all mode */ + void *usr_data[FOC_KNOB_EVENT_MAX]; /*!< User data for event */ + foc_knob_cb_t cb[FOC_KNOB_EVENT_MAX]; /*!< Event callback */ +} foc_knob_t; /* Function to create a knob handle based on a configuration */ -knob_handle_t knob_create(const knob_config_t *config) +foc_knob_handle_t foc_knob_create(const foc_knob_config_t *config) { - KNOB_CHECK(NULL != config, "config pointer can't be NULL!", NULL) + ESP_RETURN_ON_FALSE(NULL != config, NULL, TAG, "config pointer can't be NULL!"); - _knob_indicator_com_config_t com_cfg = {0}; - _knob_indicator_t *p_knob = NULL; + foc_knob_t *p_knob = (foc_knob_t *)calloc(1, sizeof(foc_knob_t)); + ESP_RETURN_ON_FALSE(NULL != p_knob, NULL, TAG, "calloc failed"); if (config->param_lists == NULL) { - ESP_LOGW(TAG, "param_lists is null, using default param list"); - com_cfg.param_lists = default_knob_param_lst; - com_cfg.param_list_num = DEFAULT_PARAM_LIST_NUM; + ESP_LOGI(TAG, "param_lists is null, using default param list"); + p_knob->param_lists = default_foc_knob_param_lst; + p_knob->param_list_num = DEFAULT_PARAM_LIST_NUM; } else { - com_cfg.param_lists = config->param_lists; - com_cfg.param_list_num = config->param_list_num; + p_knob->param_lists = config->param_lists; + p_knob->param_list_num = config->param_list_num; } - p_knob = _knob_indicator_create_com(&com_cfg); - - return (knob_handle_t)p_knob; -} + esp_err_t ret = ESP_OK; + p_knob->position = (int32_t *)calloc(p_knob->param_list_num, sizeof(int32_t)); + ESP_GOTO_ON_FALSE(NULL != p_knob->position, ESP_ERR_NO_MEM, deinit, TAG, "calloc failed"); -/* Function to get knob parameters for a specific mode */ -knob_param_t *knob_get_param(knob_handle_t handle, int mode) -{ - _knob_indicator_t *p_indicator = (_knob_indicator_t *)handle; + for (int i = 0; i < p_knob->param_list_num; i++) { + p_knob->position[i] = p_knob->param_lists[i]->position; + } - if (!(p_indicator && mode >= 0 && mode < p_indicator->param_list_num)) { - return NULL; + p_knob->max_torque_out_limit = config->max_torque_out_limit; + p_knob->max_torque = config->max_torque; + + const pid_ctrl_config_t pid_ctrl_cfg = { + .init_param = { + .max_output = config->max_torque, + .kp = 0, + .kd = 0, + .min_output = -config->max_torque, + .cal_type = PID_CAL_TYPE_POSITIONAL, + }, + }; + ret = pid_new_control_block(&pid_ctrl_cfg, &p_knob->pid); + ESP_GOTO_ON_ERROR(ret, deinit, TAG, "Failed to create PID control block"); + p_knob->mutex = xSemaphoreCreateMutex(); + ESP_GOTO_ON_FALSE(p_knob->mutex != NULL, ESP_ERR_INVALID_ARG, deinit, TAG, "create mutex failed"); + + return (foc_knob_handle_t)p_knob; +deinit: + if (p_knob->position) { + free(p_knob->position); + } + if (p_knob) { + free(p_knob); } - return (knob_param_t *)(p_indicator->param_lists[mode]); + return NULL; } -/* Macro to clamp a value within a specified range */ -static float CLAMP(const float value, const float low, const float high) +esp_err_t foc_knob_change_mode(foc_knob_handle_t handle, uint16_t mode) { - return value < low ? low : (value > high ? high : value); + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + ESP_RETURN_ON_FALSE(mode < p_knob->param_list_num, ESP_ERR_INVALID_ARG, TAG, "mode out of range"); + ESP_RETURN_ON_FALSE(NULL != p_knob->param_lists[mode], ESP_ERR_INVALID_ARG, TAG, "undefined mode"); + xSemaphoreTake(p_knob->mutex, portMAX_DELAY); + p_knob->current_mode = mode; + p_knob->center_adjusted = false; + xSemaphoreGive(p_knob->mutex); + return ESP_OK; } -float knob_start(knob_handle_t handle, int mode, float shaft_velocity, float shaft_angle) +float foc_knob_run(foc_knob_handle_t handle, float shaft_velocity, float shaft_angle) { - if (handle == NULL) { - ESP_LOGE(TAG, "Invalid knob handle"); - return 0.0; + ESP_RETURN_ON_FALSE(NULL != handle, 0.0, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + const foc_knob_param_t *motor_config = p_knob->param_lists[p_knob->current_mode]; + ESP_RETURN_ON_FALSE(NULL != motor_config, 0.0, TAG, "invalid motor config"); + esp_err_t ret = ESP_OK; + float torque = 0.0; + xSemaphoreTake(p_knob->mutex, portMAX_DELAY); + if (p_knob->center_adjusted == false) { + p_knob->current_detent_center = shaft_angle; + p_knob->center_adjusted = true; } - esp_err_t ret; - - knob_param_t *motor_config = knob_get_param(handle, mode); - _knob_indicator_t *p_indicator = (_knob_indicator_t *)handle; - knob_state_t *state = &p_indicator->state; - - if (!state->initialSetupDone) { -#if XK_INVERT_ROTATION - state->current_detent_center = -shaft_angle; -#else - state->current_detent_center = shaft_angle; -#endif - state->initialSetupDone = true; - } + /*!< Idle check and center adjustment */ + p_knob->idle_check_velocity_ewma = shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + p_knob->idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA); - if (state->pid == NULL) { - // Initialize the PID controller - pid_ctrl_config_t pid_config = { - .init_param = { - .kp = 5.0f, // Proportional gain - .max_output = 5.0, // Maximum output - .min_output = -5.0, // Minimum output - .cal_type = PID_CAL_TYPE_POSITIONAL, // Mode of calculation - }, - }; - - if (pid_new_control_block(&pid_config, &state->pid) != ESP_OK) { - ESP_LOGI(TAG, "Failed to create PID control block"); - return 1; + if (fabsf(p_knob->idle_check_velocity_ewma) > IDLE_VELOCITY_RAD_PER_SEC) { + p_knob->last_idle_start = 0; + } else { + if (p_knob->last_idle_start == 0) { + p_knob->last_idle_start = esp_timer_get_time(); } } - // Idle check and center adjustment - state->idle_check_velocity_ewma = shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + state->idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA); - - if (fabsf(state->idle_check_velocity_ewma) > IDLE_VELOCITY_RAD_PER_SEC) { - state->last_idle_start = 0; - } else { - if (state->last_idle_start == 0) { - state->last_idle_start = esp_timer_get_time(); - } + if (p_knob->last_idle_start > 0 && esp_timer_get_time() - p_knob->last_idle_start > IDLE_CORRECTION_DELAY_MILLIS && fabsf(shaft_angle - p_knob->current_detent_center) < IDLE_CORRECTION_MAX_ANGLE_RAD) { + p_knob->current_detent_center = shaft_angle * IDLE_CORRECTION_RATE_ALPHA + p_knob->current_detent_center * (1 - IDLE_CORRECTION_RATE_ALPHA); } - if (state->last_idle_start > 0 && esp_timer_get_time() - state->last_idle_start > IDLE_CORRECTION_DELAY_MILLIS && fabsf(shaft_angle - state->current_detent_center) < IDLE_CORRECTION_MAX_ANGLE_RAD) { - state->current_detent_center = shaft_angle * IDLE_CORRECTION_RATE_ALPHA + state->current_detent_center * (1 - IDLE_CORRECTION_RATE_ALPHA); + /*!< Angle to the center adjustment */ + float angle_to_detent_center = shaft_angle - p_knob->current_detent_center; + if (p_knob->angle_to_detent_center != angle_to_detent_center) { + p_knob->angle_to_detent_center = angle_to_detent_center; + p_knob->event = FOC_KNOB_ANGLE_CHANGE; + CALL_EVENT_CB(FOC_KNOB_ANGLE_CHANGE); } - // Angle to the center adjustment -#if XK_INVERT_ROTATION - state->angle_to_detent_center = -shaft_angle - state->current_detent_center; -#else - state->angle_to_detent_center = shaft_angle - state->current_detent_center; -#endif - - // Snap point calculation - if (state->angle_to_detent_center > motor_config->position_width_radians * motor_config->snap_point && (motor_config->num_positions <= 0 || motor_config->position > 0)) { - state->current_detent_center += motor_config->position_width_radians; - state->angle_to_detent_center -= motor_config->position_width_radians; - motor_config->position--; - } else if (state->angle_to_detent_center < -motor_config->position_width_radians * motor_config->snap_point && (motor_config->num_positions <= 0 || motor_config->position < motor_config->num_positions - 1)) { - state->current_detent_center -= motor_config->position_width_radians; - state->angle_to_detent_center += motor_config->position_width_radians; - motor_config->position++; + /*!< Snap point calculation */ + if (p_knob->angle_to_detent_center > motor_config->position_width_radians * motor_config->snap_point && (motor_config->num_positions <= 0 || p_knob->position[p_knob->current_mode] > 0)) { + p_knob->current_detent_center += motor_config->position_width_radians; + p_knob->angle_to_detent_center -= motor_config->position_width_radians; + p_knob->position[p_knob->current_mode]--; + p_knob->event = FOC_KNOB_DEC; + CALL_EVENT_CB(FOC_KNOB_DEC); + if (p_knob->position[p_knob->current_mode] == 0 && motor_config->num_positions > 0) { + p_knob->event = FOC_KNOB_L_LIM; + CALL_EVENT_CB(FOC_KNOB_L_LIM); + } + } else if (p_knob->angle_to_detent_center < -motor_config->position_width_radians * motor_config->snap_point && (motor_config->num_positions <= 0 || p_knob->position[p_knob->current_mode] < motor_config->num_positions - 1)) { + p_knob->current_detent_center -= motor_config->position_width_radians; + p_knob->angle_to_detent_center += motor_config->position_width_radians; + p_knob->position[p_knob->current_mode]++; + p_knob->event = FOC_KNOB_INC; + CALL_EVENT_CB(FOC_KNOB_INC); + if (p_knob->position[p_knob->current_mode] == motor_config->num_positions - 1 && motor_config->num_positions > 0) { + p_knob->event = FOC_KNOB_H_LIM; + CALL_EVENT_CB(FOC_KNOB_H_LIM); + } } - // Dead zone adjustment + /*!< Dead zone adjustment */ float dead_zone_adjustment = CLAMP( - state->angle_to_detent_center, + p_knob->angle_to_detent_center, fmaxf(-motor_config->position_width_radians * DEAD_ZONE_DETENT_PERCENT, -DEAD_ZONE_RAD), fminf(motor_config->position_width_radians * DEAD_ZONE_DETENT_PERCENT, DEAD_ZONE_RAD)); - // Out of bounds check + /*!< Out of bounds check */ bool out_of_bounds = motor_config->num_positions > 0 && - ((state->angle_to_detent_center > 0 && motor_config->position == 0) || (state->angle_to_detent_center < 0 && motor_config->position == motor_config->num_positions - 1)); + ((p_knob->angle_to_detent_center > 0 && p_knob->position[p_knob->current_mode] == 0) || (p_knob->angle_to_detent_center < 0 && p_knob->position[p_knob->current_mode] == motor_config->num_positions - 1)); - // Update PID parameters - ret = pid_update_parameters(state->pid, &(pid_ctrl_parameter_t) { - .max_output = out_of_bounds ? 5.0 : 3.0, + /*!< Update PID parameters */ + ret = pid_update_parameters(p_knob->pid, &(pid_ctrl_parameter_t) { + .max_output = out_of_bounds ? p_knob->max_torque_out_limit : p_knob->max_torque, .kp = out_of_bounds ? motor_config->endstop_strength_unit * 4 : motor_config->detent_strength_unit * 4, - .min_output = out_of_bounds ? -5.0 : -3.0, + .kd = 0.01, + .min_output = out_of_bounds ? -p_knob->max_torque_out_limit : -p_knob->max_torque, .cal_type = PID_CAL_TYPE_POSITIONAL, }); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "PID parameters are not updated"); - return 0; - } + ESP_GOTO_ON_FALSE(ret == ESP_OK, ESP_FAIL, fail, TAG, "PID parameters are not updated"); - // Calculate torque - if (fabsf(shaft_velocity) > 60) { - state->torque = 0; + /*!< Calculate torque */ + if (fabsf(shaft_velocity) > MAX_VELOCITY_CONTROL) { + torque = 0; } else { - float input_error = -state->angle_to_detent_center + dead_zone_adjustment; + float input_error = -p_knob->angle_to_detent_center + dead_zone_adjustment; float control_output; - ret = pid_compute(state->pid, input_error, &control_output); - if (ret != ESP_OK) { - ESP_LOGE(TAG, "PID Computation error"); - return 0; - } - state->torque = control_output; -#if XK_INVERT_ROTATION - state->torque = -state->torque; -#endif + ret = pid_compute(p_knob->pid, input_error, &control_output); + ESP_GOTO_ON_FALSE(ret == ESP_OK, ESP_FAIL, fail, TAG, "PID Computation error"); + torque = control_output; } - return (state->torque); + xSemaphoreGive(p_knob->mutex); + return (torque); +fail: + xSemaphoreGive(p_knob->mutex); + return 0; } -esp_err_t knob_delete(knob_handle_t handle) +esp_err_t foc_knob_delete(foc_knob_handle_t handle) { - KNOB_CHECK(NULL != handle, "invalid knob handle", NULL) - _knob_indicator_t *p_indicator = (_knob_indicator_t *)handle; - free(p_indicator); + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + xSemaphoreTake(p_knob->mutex, portMAX_DELAY); + pid_del_control_block(p_knob->pid); + xSemaphoreGive(p_knob->mutex); + vSemaphoreDelete(p_knob->mutex); + p_knob->mutex = NULL; + free(p_knob); ESP_LOGI(TAG, "Knob deleted successfully"); return ESP_OK; } + +esp_err_t foc_knob_register_cb(foc_knob_handle_t handle, foc_knob_event_t event, foc_knob_cb_t cb, void *usr_data) +{ + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + ESP_RETURN_ON_FALSE(event < FOC_KNOB_EVENT_MAX, ESP_ERR_INVALID_ARG, TAG, "invalid event"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + xSemaphoreTake(p_knob->mutex, portMAX_DELAY); + p_knob->cb[event] = cb; + p_knob->usr_data[event] = usr_data; + xSemaphoreGive(p_knob->mutex); + return ESP_OK; +} + +esp_err_t foc_knob_unregister_cb(foc_knob_handle_t handle, foc_knob_event_t event) +{ + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + ESP_RETURN_ON_FALSE(event < FOC_KNOB_EVENT_MAX, ESP_ERR_INVALID_ARG, TAG, "invalid event"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + xSemaphoreTake(p_knob->mutex, portMAX_DELAY); + p_knob->cb[event] = NULL; + p_knob->usr_data[event] = NULL; + xSemaphoreGive(p_knob->mutex); + return ESP_OK; +} + +esp_err_t foc_knob_get_state(foc_knob_handle_t handle, foc_knob_state_t *state) +{ + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + state->angle_to_detent_center = p_knob->angle_to_detent_center; + state->position_width_radians = p_knob->param_lists[p_knob->current_mode]->position_width_radians; + state->num_positions = p_knob->param_lists[p_knob->current_mode]->num_positions; + state->position = p_knob->position[p_knob->current_mode]; + state->descriptor = p_knob->param_lists[p_knob->current_mode]->descriptor; + return ESP_OK; +} + +esp_err_t foc_knob_get_event(foc_knob_handle_t handle, foc_knob_event_t *event) +{ + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + *event = p_knob->event; + return ESP_OK; +} + +esp_err_t foc_knob_set_currect_mode_position(foc_knob_handle_t handle, int32_t position) +{ + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + ESP_RETURN_ON_FALSE(position >= 0 && position < p_knob->param_lists[p_knob->current_mode]->num_positions, ESP_ERR_INVALID_ARG, TAG, "invalid position"); + xSemaphoreTake(p_knob->mutex, portMAX_DELAY); + p_knob->angle_to_detent_center = 0; + p_knob->center_adjusted = false; + p_knob->position[p_knob->current_mode] = position; + xSemaphoreGive(p_knob->mutex); + return ESP_OK; +} + +esp_err_t foc_knob_get_current_mode_position(foc_knob_handle_t handle, int32_t *position) +{ + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + *position = p_knob->position[p_knob->current_mode]; + return ESP_OK; +} + +esp_err_t foc_knob_set_position(foc_knob_handle_t handle, uint16_t mode, int32_t position) +{ + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + ESP_RETURN_ON_FALSE(mode < p_knob->param_list_num, ESP_ERR_INVALID_ARG, TAG, "mode out of range"); + ESP_RETURN_ON_FALSE(NULL != p_knob->param_lists[mode], ESP_ERR_INVALID_ARG, TAG, "undefined mode"); + ESP_RETURN_ON_FALSE(position >= 0 && position < p_knob->param_lists[mode]->num_positions, ESP_ERR_INVALID_ARG, TAG, "invalid position"); + xSemaphoreTake(p_knob->mutex, portMAX_DELAY); + p_knob->angle_to_detent_center = 0; + p_knob->center_adjusted = false; + p_knob->position[mode] = position; + xSemaphoreGive(p_knob->mutex); + return ESP_OK; +} + +esp_err_t foc_knob_get_position(foc_knob_handle_t handle, uint16_t mode, int32_t *position) +{ + ESP_RETURN_ON_FALSE(NULL != handle, ESP_ERR_INVALID_ARG, TAG, "invalid foc knob handle"); + foc_knob_t *p_knob = (foc_knob_t *)handle; + ESP_RETURN_ON_FALSE(mode < p_knob->param_list_num, ESP_ERR_INVALID_ARG, TAG, "mode out of range"); + ESP_RETURN_ON_FALSE(NULL != p_knob->param_lists[mode], ESP_ERR_INVALID_ARG, TAG, "undefined mode"); + *position = p_knob->position[mode]; + return ESP_OK; +} diff --git a/examples/motor/foc_knob_example/components/foc_knob/foc_knob_default.c b/examples/motor/foc_knob_example/components/foc_knob/foc_knob_default.c index bb10073cd..5732faef5 100644 --- a/examples/motor/foc_knob_example/components/foc_knob/foc_knob_default.c +++ b/examples/motor/foc_knob_example/components/foc_knob/foc_knob_default.c @@ -8,50 +8,46 @@ #include "foc_knob_default.h" /* arrays of knob parameters for different knob modes */ -static const knob_param_t unbound_fine_dents[] = { - {0, 0, 5 * PI / 180, 0.7, 0, 1.1, "Fine values with detents"}, -}; -static const knob_param_t unbound_no_dents[] = { - {0, 0, 1 * PI / 180, 0, 0, 1.1, "Unbounded No detents"}, +static const foc_knob_param_t unbound_no_detents[] = { + {0, 0, 1 * PI / 180, 0, 0, 1.1, "Unbounded\nNo detents"}, }; -static const knob_param_t super_dial[] = { - {0, 0, 5 * PI / 180, 2, 0.5, 1.1, "Super Dial"}, +static const foc_knob_param_t unbound_fine_detents[] = { + {0, 0, 5 * PI / 180, 0.8, 0, 1.1, "Unbounded\nFine detents"}, }; -static const knob_param_t unbound_coarse_dents[] = { - {0, 0, 10 * PI / 180, 1.1, 0, 1.1, "Fine values with detents unbound"}, +static const foc_knob_param_t unbound_coarse_detents[] = { + {0, 0, 10 * PI / 180, 1.2, 0, 1.1, "Unbounded\nStrong detents"}, }; -static const knob_param_t bound_no_dents[] = { - {360, 180, 1 * PI / 180, 0.05, 0.5, 1.1, "Bounded 0-13 No detents"}, +static const foc_knob_param_t bound_no_detents[] = { + {360, 180, 1 * PI / 180, 0.05, 0.3, 1.1, "Bounded\nNo detents"}, }; -static const knob_param_t coarse_dents[] = { - {0, 0, 8.225806452 * PI / 180, 2, 0.5, 1.1, "Coarse values strong detents"}, +static const foc_knob_param_t bound_fine_dents[] = { + {32, 0, 5 * PI / 180, 0.6, 0.3, 1.1, "Bounded\nFine detents"}, }; -static const knob_param_t fine_no_dents[] = { - {256, 127, 1 * PI / 180, 0, 0.5, 1.1, "Fine values no detents"}, +static const foc_knob_param_t bound_coarse_dents[] = { + {13, 0, 10 * PI / 180, 1.1, 0.3, 0.55, "Bounded\nStrong detents"}, }; -static const knob_param_t on_off_strong_dents[] = { - {2, 0, 60 * PI / 180, 1, 0.5, 0.55, "On/Off strong detents"}, +static const foc_knob_param_t on_off_strong_dents[] = { + { 2, 0, 60 * PI / 180, 1, 0.3, 0.55, "On/Off strong detents"}, }; /* Initialize an array of pointers to these parameter arrays for easy access */ -knob_param_t const *default_knob_param_lst[] = { - [MOTOR_UNBOUND_FINE_DETENTS] = unbound_fine_dents, //1.Fine values with detents - [MOTOR_UNBOUND_NO_DETENTS] = unbound_no_dents, //2.Unbounded No detents - [MOTOR_SUPER_DIAL] = super_dial, //3.Super Dial - [MOTOR_UNBOUND_COARSE_DETENTS] = unbound_coarse_dents, //4.Fine values with detents unbound - [MOTOR_BOUND_0_12_NO_DETENTS] = bound_no_dents, //5.Bounded 0-13 No detents - [MOTOR_COARSE_DETENTS] = coarse_dents, //6.Coarse values strong detents - [MOTOR_FINE_NO_DETENTS] = fine_no_dents, //7.Fine values no detents - [MOTOR_ON_OFF_STRONG_DETENTS] = on_off_strong_dents, //8.On/Off strong detents" +foc_knob_param_t const *default_foc_knob_param_lst[] = { + [MOTOR_UNBOUND_NO_DETENTS] = unbound_no_detents, /*!< Unbounded No detents */ + [MOTOR_UNBOUND_FINE_DETENTS] = unbound_fine_detents, /*!< Unbounded Fine detents */ + [MOTOR_UNBOUND_COARSE_DETENTS] = unbound_coarse_detents, /*!< Unbounded Strong detents */ + [MOTOR_BOUND_NO_DETENTS] = bound_no_detents, /*!< Bounded No detents */ + [MOTOR_BOUND_FINE_DETENTS] = bound_fine_dents, /*!< Bounded Fine detents */ + [MOTOR_BOUND_COARSE_DETENTS] = bound_coarse_dents, /*!< Bounded Strong detents */ + [MOTOR_ON_OFF_STRONG_DETENTS] = on_off_strong_dents, /*!< On/Off strong detents */ [MOTOR_MAX_MODES] = NULL, }; /* Calculate the number of elements in the default parameter list */ -const int DEFAULT_PARAM_LIST_NUM = (sizeof(default_knob_param_lst) / sizeof(default_knob_param_lst[0]) - 1); +const int DEFAULT_PARAM_LIST_NUM = (sizeof(default_foc_knob_param_lst) / sizeof(default_foc_knob_param_lst[0]) - 1); diff --git a/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob.h b/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob.h index 46e01f71a..7ea75e290 100644 --- a/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob.h +++ b/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob.h @@ -13,42 +13,59 @@ extern "C" { #include #include #include "esp_err.h" +#include "pid_ctrl.h" #ifndef PI #define PI 3.14159265358979f #endif -#define IDLE_VELOCITY_EWMA_ALPHA 0.001 -#define IDLE_VELOCITY_RAD_PER_SEC 0.05 -#define IDLE_CORRECTION_DELAY_MILLIS 500 -#define IDLE_CORRECTION_MAX_ANGLE_RAD (5 * PI / 180) -#define IDLE_CORRECTION_RATE_ALPHA 0.0005 -#define DEAD_ZONE_DETENT_PERCENT 0.2 -#define DEAD_ZONE_RAD (1 * PI / 180) +typedef void (* foc_knob_cb_t)(void *foc_knob_handle, void *user_data); +typedef void *foc_knob_handle_t; /*!< Knob operation handle */ -typedef void *knob_handle_t; /*!< Knob operation handle */ +typedef struct { + float angle_to_detent_center; /*!< The angle from the current position. */ + float position_width_radians; /*!< Width of each position in radians */ + int32_t num_positions; /*!< Number of positions */ + int32_t position; /*!< Current position */ + const char *descriptor; /*!< Description */ +} foc_knob_state_t; /** * @brief Knob parameters * */ typedef struct { - int32_t num_positions; // Number of positions - int32_t position; // Current position - float position_width_radians; // Width of each position in radians - float detent_strength_unit; // Strength of detent during normal rotation - float endstop_strength_unit; // Strength of detent when reaching the end - float snap_point; // Snap point for each position - const char *descriptor; // Description -} knob_param_t; + int32_t num_positions; /*!< Number of positions */ + int32_t position; /*!< Current position */ + float position_width_radians; /*!< Width of each position in radians */ + float detent_strength_unit; /*!< Strength of detent during normal rotation */ + float endstop_strength_unit; /*!< Strength of detent when reaching the end */ + float snap_point; /*!< Snap point for each position */ + const char *descriptor; /*!< Description */ +} foc_knob_param_t; + +/** + * @brief Knob events + * + */ +typedef enum { + FOC_KNOB_INC = 0, /*!< EVENT: Position increase */ + FOC_KNOB_DEC, /*!< EVENT: Position decrease */ + FOC_KNOB_H_LIM, /*!< EVENT: Count reaches maximum limit */ + FOC_KNOB_L_LIM, /*!< EVENT: Count reaches the minimum limit */ + FOC_KNOB_ANGLE_CHANGE, /*!< EVENT: Angle change */ + FOC_KNOB_EVENT_MAX, /*!< EVENT: Number of events */ +} foc_knob_event_t; /** * @brief Knob specified configurations, used when creating a Knob */ typedef struct { - knob_param_t const **param_lists; - uint16_t param_list_num; -} knob_config_t; + foc_knob_param_t const **param_lists; /*!< foc mode lists, if not set use default_foc_knob_param_lst */ + uint16_t param_list_num; /*!< foc mode number */ + float max_torque_out_limit; /*!< max torque out limit */ + float max_torque; /*!< max torque in limit */ +} foc_knob_config_t; /** * @brief Create a knob @@ -57,21 +74,18 @@ typedef struct { * * @return A handle to the created knob */ -knob_handle_t knob_create(const knob_config_t *config); +foc_knob_handle_t foc_knob_create(const foc_knob_config_t *config); /** - * @brief Get knob parameters for a specific mode. - * - * This function retrieves the knob parameters for a specific mode and returns a pointer - * to a 'knob_param_t' structure. + * @brief Change the mode of the knob. * * @param handle A handle to the knob. - * @param mode The mode for which parameters are requested. - * - * @return A pointer to the 'knob_param_t' structure if successful, or NULL if the mode - * is out of range or the handle is invalid. + * @param mode The mode to be set for the knob. + * @return + * - ESP_OK if the successful. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. */ -knob_param_t *knob_get_param(knob_handle_t handle, int mode); +esp_err_t foc_knob_change_mode(foc_knob_handle_t handle, uint16_t mode); /** * @brief Start knob operation. @@ -80,13 +94,12 @@ knob_param_t *knob_get_param(knob_handle_t handle, int mode); * and shaft angle. * * @param handle A handle to the knob. - * @param mode The mode of the knob operation. * @param shaft_velocity The velocity of the knob's shaft. * @param shaft_angle The angle of the knob's shaft. * * @return The torque applied during the knob operation. */ -float knob_start(knob_handle_t handle, int mode, float shaft_velocity, float shaft_angle); +float foc_knob_run(foc_knob_handle_t handle, float shaft_velocity, float shaft_angle); /** * @brief Delete a knob and free associated memory. @@ -97,7 +110,101 @@ float knob_start(knob_handle_t handle, int mode, float shaft_velocity, float sha * @param handle A handle to the knob to be deleted. * @return ESP_OK on success */ -esp_err_t knob_delete(knob_handle_t handle); +esp_err_t foc_knob_delete(foc_knob_handle_t handle); + +/** + * @brief Registers a callback function for a Field-Oriented Control (FOC) knob event. + * + * @param handle The handle to the FOC knob. + * @param event The event type to register the callback for. + * @param cb The callback function to be invoked when the specified event occurs. + * @param usr_data User-defined data that will be passed to the callback function when invoked. + * + * @return + * - ESP_OK if the callback is successfully registered. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. + */ +esp_err_t foc_knob_register_cb(foc_knob_handle_t handle, foc_knob_event_t event, foc_knob_cb_t cb, void *usr_data); + +/** + * @brief Unregisters a callback function for a Field-Oriented Control (FOC) knob event. + * + * @param handle The handle to the FOC knob. + * @param event The event type for which the callback needs to be unregistered. + * + * @return + * - ESP_OK if the callback is successfully unregistered. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. + */ +esp_err_t foc_knob_unregister_cb(foc_knob_handle_t handle, foc_knob_event_t event); + +/** + * @brief Get the current state of the knob. + * + * @param handle The handle to the FOC knob. + * @param state The pointer to the state structure to be filled. + * @return + * - ESP_OK if the successful. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. + */ +esp_err_t foc_knob_get_state(foc_knob_handle_t handle, foc_knob_state_t *state); + +/** + * @brief Get the current event of the knob. + * + * @param handle The handle to the FOC knob. + * @param event The pointer to the event structure to be filled. + * @return + * - ESP_OK if the successful. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. + */ +esp_err_t foc_knob_get_event(foc_knob_handle_t handle, foc_knob_event_t *event); + +/** + * @brief Get the current mode of the knob. + * + * @param handle The handle to the FOC knob. + * @param position The pointer to the position structure to be filled. + * @return + * - ESP_OK if the successful. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. + */ +esp_err_t foc_knob_set_currect_mode_position(foc_knob_handle_t handle, int32_t position); + +/** + * @brief Get the current mode's position of the knob. + * + * @param handle The handle to the FOC knob. + * @param position The pointer to the position structure to be filled. + * @return + * - ESP_OK if the successful. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. + */ +esp_err_t foc_knob_get_current_mode_position(foc_knob_handle_t handle, int32_t *position); + +/** + * @brief Set the position of the knob. + * + * @param handle The handle to the FOC knob. + * @param mode The mode of setting the position + * @param position The desired position value to be set for the FOC knob. + * @return + * - ESP_OK if the successful. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. + */ +esp_err_t foc_knob_set_position(foc_knob_handle_t handle, uint16_t mode, int32_t position); + +/** + * @brief Get the position of the knob. + * + * @param handle The handle to the FOC knob. + * @param mode The mode of getting the position + * @param position The pointer to the position structure to be filled. + * @return + * - ESP_OK if the successful. + * - ESP_ERR_INVALID_ARG if the provided arguments are invalid. + */ +esp_err_t foc_knob_get_position(foc_knob_handle_t handle, uint16_t mode, int32_t *position); #ifdef __cplusplus } diff --git a/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob_default.h b/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob_default.h index a52d74388..a88b34e2f 100644 --- a/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob_default.h +++ b/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob_default.h @@ -12,22 +12,21 @@ extern "C" { /* default knob modes */ enum { - MOTOR_UNBOUND_FINE_DETENTS, //0: Fine values with detents - MOTOR_UNBOUND_NO_DETENTS, //1: Unbounded No detents - MOTOR_SUPER_DIAL, //2: Super Dial - MOTOR_UNBOUND_COARSE_DETENTS, //3: Fine values with detents unbound - MOTOR_BOUND_0_12_NO_DETENTS, //4: Bounded 0-13 No detents - MOTOR_COARSE_DETENTS, //5: Coarse values strong detents - MOTOR_FINE_NO_DETENTS, //6: Fine values no detents - MOTOR_ON_OFF_STRONG_DETENTS, //7: On/Off strong detents - MOTOR_MAX_MODES, //8: Max mode + MOTOR_UNBOUND_NO_DETENTS, /*!< motor unbound with no detents */ + MOTOR_UNBOUND_FINE_DETENTS, /*!< motor unbound with fine detents */ + MOTOR_UNBOUND_COARSE_DETENTS, /*!< motor unbound with coarse detents */ + MOTOR_BOUND_NO_DETENTS, /*!< motor bound with no detents */ + MOTOR_BOUND_FINE_DETENTS, /*!< motor bound with fine detents */ + MOTOR_BOUND_COARSE_DETENTS, /*!< motor bound with coarse detents */ + MOTOR_ON_OFF_STRONG_DETENTS, /*!< motor on/off with strong detents */ + MOTOR_MAX_MODES, /*!< Max mode */ }; /* The number of default parameters in a list */ extern const int DEFAULT_PARAM_LIST_NUM; /* default knob parameters */ -extern knob_param_t const *default_knob_param_lst[]; +extern foc_knob_param_t const *default_foc_knob_param_lst[]; #ifdef __cplusplus } diff --git a/examples/motor/foc_knob_example/main/idf_component.yml b/examples/motor/foc_knob_example/main/idf_component.yml index 4f1cb3ec7..25cdc1f7c 100644 --- a/examples/motor/foc_knob_example/main/idf_component.yml +++ b/examples/motor/foc_knob_example/main/idf_component.yml @@ -3,4 +3,4 @@ dependencies: esp_simplefoc: override_path: "../../../../components/motor/esp_simplefoc" espressif/button: - version: "~3.0.0" + version: "~3.1.0" diff --git a/examples/motor/foc_knob_example/main/main.cpp b/examples/motor/foc_knob_example/main/main.cpp index 84ca923c4..910cd9592 100644 --- a/examples/motor/foc_knob_example/main/main.cpp +++ b/examples/motor/foc_knob_example/main/main.cpp @@ -17,15 +17,33 @@ #include "iot_button.h" #define SWITCH_BUTTON 0 +#define PHASE_U_GPIO 15 +#define PHASE_V_GPIO 16 +#define PHASE_W_GPIO 17 +#define MOTOR_PP 7 +#define MT6701_SPI_HOST SPI2_HOST +#define MT6701_SPI_SCLK_GPIO 12 +#define MT6701_SPI_MISO_GPIO 13 +#define MT6701_SPI_MOSI_GPIO -1 +#define MT6701_SPI_CS_GPIO 11 + #define USING_MCPWM 0 + +#if !USING_MCPWM +#define LEDC_CHAN_0 0 +#define LEDC_CHAN_1 1 +#define LEDC_CHAN_2 2 +#endif + #define TAG "FOC_Knob_Example" -static knob_handle_t knob_handle_0 = NULL; -static int mode = MOTOR_UNBOUND_FINE_DETENTS; +static foc_knob_handle_t foc_knob_handle = NULL; +static int mode = MOTOR_UNBOUND_NO_DETENTS; +static bool motor_shake = false; /*update motor parameters based on hardware design*/ -BLDCDriver3PWM driver = BLDCDriver3PWM(15, 16, 17); -BLDCMotor motor = BLDCMotor(7); -MT6701 mt6701 = MT6701(SPI2_HOST, GPIO_NUM_12, GPIO_NUM_13, (gpio_num_t) -1, GPIO_NUM_11); +BLDCDriver3PWM driver = BLDCDriver3PWM(PHASE_U_GPIO, PHASE_V_GPIO, PHASE_W_GPIO); +BLDCMotor motor = BLDCMotor(MOTOR_PP); +MT6701 mt6701 = MT6701(MT6701_SPI_HOST, (gpio_num_t)MT6701_SPI_SCLK_GPIO, (gpio_num_t)MT6701_SPI_MISO_GPIO, (gpio_num_t)MT6701_SPI_MOSI_GPIO, (gpio_num_t)MT6701_SPI_CS_GPIO); /*Motor initialization*/ void motor_init(void) @@ -40,22 +58,16 @@ void motor_init(void) #if USING_MCPWM driver.init(0); #else - driver.init({0, 1, 2}); + driver.init({LEDC_CHAN_0, LEDC_CHAN_1, LEDC_CHAN_2}); #endif motor.linkDriver(&driver); motor.foc_modulation = SpaceVectorPWM; motor.controller = MotionControlType::torque; - motor.PID_velocity.P = 1.0f; - motor.PID_velocity.I = 0; - motor.PID_velocity.D = 0.01f; - motor.voltage_limit = 4; - motor.LPF_velocity.Tf = 0.01; - motor.velocity_limit = 4; - - motor.init(); // initialize motor - motor.initFOC(); // align sensor and start FOC + motor.useMonitoring(Serial); + motor.init(); // initialize motor + motor.initFOC(); // align sensor and start FOC ESP_LOGI(TAG, "Motor Initialize Successfully"); } @@ -64,9 +76,66 @@ static void button_press_cb(void *arg, void *data) { mode++; if (mode >= MOTOR_MAX_MODES) { - mode = MOTOR_UNBOUND_FINE_DETENTS; + mode = MOTOR_UNBOUND_NO_DETENTS; } + foc_knob_change_mode(foc_knob_handle, mode); ESP_LOGI(TAG, "mode: %d", mode); + motor_shake = true; +} + +static void foc_knob_inc_cb(void *arg, void *data) +{ + foc_knob_state_t state; + foc_knob_get_state(arg, &state); + ESP_LOGI(TAG, "foc_knob_inc_cb: position: %" PRId32 "\n", state.position); +} + +static void foc_knob_dec_cb(void *arg, void *data) +{ + foc_knob_state_t state; + foc_knob_get_state(arg, &state); + ESP_LOGI(TAG, "foc_knob_dec_cb: position: %" PRId32 "\n", state.position); +} + +static void foc_knob_h_lim_cb(void *arg, void *data) +{ + ESP_LOGI(TAG, "foc_knob_h_lim_cb"); +} + +static void foc_knob_l_lim_cb(void *arg, void *data) +{ + ESP_LOGI(TAG, "foc_knob_l_lim_cb"); +} + +float motor_shake_func(float strength, int delay_cnt) +{ + static int time_cnt = 0; + if (time_cnt < delay_cnt) { + time_cnt++; + return strength; + } else if (time_cnt < 2 * delay_cnt) { + time_cnt++; + return -strength; + } else { + time_cnt = 0; + motor_shake = false; + return 0; + } +} + +static void motor_task(void *arg) +{ + static float torque = 0; + while (1) { + motor.loopFOC(); + if (motor_shake) { + torque = motor_shake_func(2, 4); + } else { + torque = foc_knob_run(foc_knob_handle, motor.shaft_velocity, motor.shaft_angle); + } + motor.move(torque); + vTaskDelay(1 / portTICK_PERIOD_MS); + } } extern "C" void app_main(void) @@ -85,18 +154,19 @@ extern "C" void app_main(void) iot_button_register_cb(btn, BUTTON_PRESS_DOWN, button_press_cb, NULL); motor_init(); - knob_config_t cfg = { - .param_lists = default_knob_param_lst, + foc_knob_config_t cfg = { + .param_lists = default_foc_knob_param_lst, .param_list_num = MOTOR_MAX_MODES, + .max_torque_out_limit = 3, + .max_torque = 2, }; - knob_handle_0 = knob_create(&cfg); + foc_knob_handle = foc_knob_create(&cfg); - while (1) { - motor.loopFOC(); - float torque = knob_start(knob_handle_0, mode, motor.shaft_velocity, motor.shaft_angle); - ESP_LOGD(TAG, "Angle: %f, Velocity: %f, torque: %f", motor.shaft_angle, motor.shaft_velocity, torque); - motor.move(torque); - vTaskDelay(1 / portTICK_PERIOD_MS); - } + foc_knob_register_cb(foc_knob_handle, FOC_KNOB_INC, foc_knob_inc_cb, NULL); + foc_knob_register_cb(foc_knob_handle, FOC_KNOB_DEC, foc_knob_dec_cb, NULL); + foc_knob_register_cb(foc_knob_handle, FOC_KNOB_H_LIM, foc_knob_h_lim_cb, NULL); + foc_knob_register_cb(foc_knob_handle, FOC_KNOB_L_LIM, foc_knob_l_lim_cb, NULL); + + xTaskCreate(motor_task, "motor_task", 2048, NULL, 5, NULL); } diff --git a/examples/motor/foc_velocity_control/README.md b/examples/motor/foc_velocity_control/README.md index aa4a2901c..b9cab983b 100644 --- a/examples/motor/foc_velocity_control/README.md +++ b/examples/motor/foc_velocity_control/README.md @@ -1,9 +1,9 @@ # Application example of ESP SIMPLEFOC Speed Control -This example demonstrates how to develop a motor speed control program using the `esp_ speed control component on an ESP32 series chip. -The specific functions of the demonstration are as follows: +This example demonstrates how to develop a motor speed control program using the `esp_ speed control component on an ESP32 series chip. +The specific functions of the demonstration are as follows: -- Motor speed closed loop. +- Motor speed closed loop. - Serial port instruction to control motor speed ## How to use the example @@ -11,7 +11,7 @@ The specific functions of the demonstration are as follows: ### Hardware requirement 1. Hardware connection: - * for ESP32-S3 chips, the serial port is connected to the default RXD0 and TXD0. If you need to use other serial ports, you need to modify the corresponding uart_num, RXD and TXD in the initialization part of the serial port. + * for ESP32-S3 chips, the serial port is connected to the default RXD0 and TXD0. If you need to use other serial ports, you need to modify the corresponding uart_num, RXD and TXD in the initialization part of the serial port. * the sample motor has a pole pair of 14 and a rated voltage of 12V. * the example MOS predrive uses EG2133, and connects LIN with HIN, using 3PWM driver. * the example angle sensor uses AS5600. If you use other angle sensors, you need to manually enter the initialization and angle reading functions into GenericSensor. @@ -68,7 +68,7 @@ I (214) cpu_start: Compile time: Jul 19 2023 10:20:23 I (221) cpu_start: ELF file SHA256: 686b47738... I (226) cpu_start: ESP-IDF: v5.2-dev-1113-g28c643a56d I (232) cpu_start: Min chip rev: v0.0 -I (237) cpu_start: Max chip rev: v0.99 +I (237) cpu_start: Max chip rev: v0.99 I (242) cpu_start: Chip rev: v0.1 I (247) heap_init: Initializing. RAM available for dynamic allocation: I (254) heap_init: At 3FC96AF8 len 00052C18 (331 KiB): DRAM @@ -81,14 +81,14 @@ W (287) spi_flash: Detected size(16384k) larger than the size in the binary imag I (301) sleep: Configure to isolate all GPIO pins in sleep state I (307) sleep: Enable automatic switching of GPIO sleep configuration I (315) app_start: Starting scheduler on CPU0 -I (319) app_start: StartY�I (345) gpio: GPIO[17]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 -I (345) gpio: GPIO[16]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 -I (352) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 +I (319) app_start: StartY�I (345) gpio: GPIO[17]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 +I (345) gpio: GPIO[16]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 +I (352) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 MCPWM Group: 0 is idle Auto. Current Driver uses Mcpwm GroupId:0 -I (368) gpio: GPIO[17]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 -I (377) gpio: GPIO[16]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 -I (386) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 +I (368) gpio: GPIO[17]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 +I (377) gpio: GPIO[16]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 +I (386) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 MOT: Monitor enabled! MOT: Init MOT: Enable driver. @@ -101,4 +101,6 @@ MOT: Ready. T2.0 2.000 -``` \ No newline at end of file +``` + +![FOC exmaple](https://dl.espressif.com/ae/esp-iot-solution/foc_close_loop.gif) diff --git a/examples/motor/foc_velocity_control/README_cn.md b/examples/motor/foc_velocity_control/README_cn.md index 89d6b72af..1cdaf7997 100644 --- a/examples/motor/foc_velocity_control/README_cn.md +++ b/examples/motor/foc_velocity_control/README_cn.md @@ -67,7 +67,7 @@ I (214) cpu_start: Compile time: Jul 19 2023 10:20:23 I (221) cpu_start: ELF file SHA256: 686b47738... I (226) cpu_start: ESP-IDF: v5.2-dev-1113-g28c643a56d I (232) cpu_start: Min chip rev: v0.0 -I (237) cpu_start: Max chip rev: v0.99 +I (237) cpu_start: Max chip rev: v0.99 I (242) cpu_start: Chip rev: v0.1 I (247) heap_init: Initializing. RAM available for dynamic allocation: I (254) heap_init: At 3FC96AF8 len 00052C18 (331 KiB): DRAM @@ -80,14 +80,14 @@ W (287) spi_flash: Detected size(16384k) larger than the size in the binary imag I (301) sleep: Configure to isolate all GPIO pins in sleep state I (307) sleep: Enable automatic switching of GPIO sleep configuration I (315) app_start: Starting scheduler on CPU0 -I (319) app_start: StartY�I (345) gpio: GPIO[17]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 -I (345) gpio: GPIO[16]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 -I (352) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 +I (319) app_start: StartY�I (345) gpio: GPIO[17]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 +I (345) gpio: GPIO[16]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 +I (352) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 0| Pulldown: 0| Intr:0 MCPWM Group: 0 is idle Auto. Current Driver uses Mcpwm GroupId:0 -I (368) gpio: GPIO[17]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 -I (377) gpio: GPIO[16]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 -I (386) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 +I (368) gpio: GPIO[17]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 +I (377) gpio: GPIO[16]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 +I (386) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0 MOT: Monitor enabled! MOT: Init MOT: Enable driver. @@ -100,4 +100,6 @@ MOT: Ready. T2.0 2.000 -``` \ No newline at end of file +``` + +![FOC exmaple](https://dl.espressif.com/ae/esp-iot-solution/foc_close_loop.gif)