diff --git a/.gitlab/ci/build.yml b/.gitlab/ci/build.yml index cad4494ef..c49bb5268 100644 --- a/.gitlab/ci/build.yml +++ b/.gitlab/ci/build.yml @@ -338,6 +338,16 @@ build_example_motor_foc_velocity_control: variables: EXAMPLE_DIR: examples/motor/foc_velocity_control +build_example_motor_foc_knob: + extends: + - .build_examples_template + - .rules:build:example_motor_foc_knob + parallel: + matrix: + - IMAGE: espressif/idf:release-v5.0 + variables: + EXAMPLE_DIR: examples/motor/foc_knob_example + build_example_ota_simple_ota_example: extends: - .build_examples_template diff --git a/.gitlab/ci/rules.yml b/.gitlab/ci/rules.yml index 2c4105b42..788c4e460 100644 --- a/.gitlab/ci/rules.yml +++ b/.gitlab/ci/rules.yml @@ -316,6 +316,9 @@ .patterns-example_motor_foc_velocity_control: &patterns-example_motor_foc_velocity_control - "examples/motor/foc_velocity_control/**/*" +.patterns-example_motor_foc_knob: &patterns-example_motor_foc_knob + - "examples/motor/foc_knob_example/**/*" + .patterns-example_ota_simple_ota_example: &patterns-example_ota_simple_ota_example - "examples/ota/simple_ota_example/**/*" @@ -690,6 +693,18 @@ - <<: *if-dev-push changes: *patterns-example_motor_foc_velocity_control +.rules:build:example_motor_foc_knob: + rules: + - <<: *if-protected + - <<: *if-label-build + - <<: *if-trigger-job + - <<: *if-dev-push + changes: *patterns-build_system + - <<: *if-dev-push + changes: *patterns-components_motor_esp_simplefoc + - <<: *if-dev-push + changes: *patterns-example_motor_foc_knob + .rules:build:example_ota_simple_ota_example: rules: - <<: *if-protected 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/.build-rules.yml b/examples/.build-rules.yml index f95a6b939..a9fc8793d 100644 --- a/examples/.build-rules.yml +++ b/examples/.build-rules.yml @@ -103,6 +103,10 @@ examples/motor/foc_velocity_control: disable: - if: IDF_TARGET == "esp32c2" +examples/motor/foc_knob_example: + enable: + - if: IDF_TARGET in ["esp32","esp32s3"] + examples/ota/simple_ota_example: enable: - if: IDF_TARGET in ["esp32c2","esp32c3","esp32"] and (IDF_VERSION_MAJOR == 5 and IDF_VERSION_MINOR == 0) diff --git a/examples/motor/foc_knob_example/CMakeLists.txt b/examples/motor/foc_knob_example/CMakeLists.txt new file mode 100644 index 000000000..52181d502 --- /dev/null +++ b/examples/motor/foc_knob_example/CMakeLists.txt @@ -0,0 +1,9 @@ +# For more information about build system see +# https://docs.espressif.com/projects/esp-idf/en/latest/api-guides/build-system.html +# The following five lines of boilerplate have to be in your project's +# CMakeLists in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.16) + +include($ENV{IDF_PATH}/tools/cmake/project.cmake) +add_compile_options(-fdiagnostics-color=always) +project(foc_knob_example) diff --git a/examples/motor/foc_knob_example/README.md b/examples/motor/foc_knob_example/README.md new file mode 100644 index 000000000..5efddd8dc --- /dev/null +++ b/examples/motor/foc_knob_example/README.md @@ -0,0 +1,59 @@ +# 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. 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 + 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; + +``` + +### Hardware Required +This example is specifically designed for ESP32-S3-Motor-LCDkit. + +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. + +### Configure the project + +step1: chose your target chip. + +```` +idf.py set-target esp32s3 +```` + +step2: build the project + +``` +idf.py build +``` + +step 3: Flash and monitor +Flash the program and launch IDF Monitor: + +```bash +idf.py flash monitor +``` + +![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/CMakeLists.txt b/examples/motor/foc_knob_example/components/foc_knob/CMakeLists.txt new file mode 100644 index 000000000..8274e602c --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/CMakeLists.txt @@ -0,0 +1,11 @@ +set(srcs "foc_knob.c" + "foc_knob_default.c" + ) + +set(includes + "include" + ) + +idf_component_register(SRCS ${srcs} + INCLUDE_DIRS "${includes}" + REQUIRES driver esp_timer) 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 new file mode 100644 index 000000000..54b5afe47 --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/README.md @@ -0,0 +1,39 @@ +# 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. 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 + 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; + +``` +### Hardware Required +This example is specifically designed for ESP32-S3-Motor-LCDkit. + +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 +[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 new file mode 100644 index 000000000..70a34a794 --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/foc_knob.c @@ -0,0 +1,321 @@ +/* + * SPDX-FileCopyrightText: 2022-2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/semphr.h" +#include "esp_log.h" +#include "esp_timer.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"; + +#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 + +#define CLAMP(value, low, high) ((value) < (low) ? (low) : ((value) > (high) ? (high) : (value))) + +#define CALL_EVENT_CB(ev) if(p_knob->cb[ev])p_knob->cb[ev](p_knob, p_knob->usr_data[ev]) + +/* 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 */ +foc_knob_handle_t foc_knob_create(const foc_knob_config_t *config) +{ + ESP_RETURN_ON_FALSE(NULL != config, NULL, TAG, "config pointer can't be 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_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 { + p_knob->param_lists = config->param_lists; + p_knob->param_list_num = config->param_list_num; + } + + 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"); + + for (int i = 0; i < p_knob->param_list_num; i++) { + p_knob->position[i] = p_knob->param_lists[i]->position; + } + + 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 NULL; +} + +esp_err_t foc_knob_change_mode(foc_knob_handle_t handle, uint16_t mode) +{ + 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 foc_knob_run(foc_knob_handle_t handle, float shaft_velocity, float shaft_angle) +{ + 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; + } + + /*!< 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 (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(); + } + } + + 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); + } + + /*!< 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); + } + + /*!< 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 */ + float dead_zone_adjustment = CLAMP( + 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 */ + bool out_of_bounds = motor_config->num_positions > 0 && + ((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(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, + .kd = 0.01, + .min_output = out_of_bounds ? -p_knob->max_torque_out_limit : -p_knob->max_torque, + .cal_type = PID_CAL_TYPE_POSITIONAL, + }); + ESP_GOTO_ON_FALSE(ret == ESP_OK, ESP_FAIL, fail, TAG, "PID parameters are not updated"); + + /*!< Calculate torque */ + if (fabsf(shaft_velocity) > MAX_VELOCITY_CONTROL) { + torque = 0; + } else { + float input_error = -p_knob->angle_to_detent_center + dead_zone_adjustment; + float control_output; + 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; + } + xSemaphoreGive(p_knob->mutex); + return (torque); +fail: + xSemaphoreGive(p_knob->mutex); + return 0; +} + +esp_err_t foc_knob_delete(foc_knob_handle_t handle) +{ + 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 new file mode 100644 index 000000000..5732faef5 --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/foc_knob_default.c @@ -0,0 +1,53 @@ +/* + * SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include "foc_knob.h" +#include "foc_knob_default.h" + +/* arrays of knob parameters for different knob modes */ + +static const foc_knob_param_t unbound_no_detents[] = { + {0, 0, 1 * PI / 180, 0, 0, 1.1, "Unbounded\nNo detents"}, +}; + +static const foc_knob_param_t unbound_fine_detents[] = { + {0, 0, 5 * PI / 180, 0.8, 0, 1.1, "Unbounded\nFine detents"}, +}; + +static const foc_knob_param_t unbound_coarse_detents[] = { + {0, 0, 10 * PI / 180, 1.2, 0, 1.1, "Unbounded\nStrong 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 foc_knob_param_t bound_fine_dents[] = { + {32, 0, 5 * PI / 180, 0.6, 0.3, 1.1, "Bounded\nFine 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 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 */ +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_foc_knob_param_lst) / sizeof(default_foc_knob_param_lst[0]) - 1); diff --git a/examples/motor/foc_knob_example/components/foc_knob/idf_component.yml b/examples/motor/foc_knob_example/components/foc_knob/idf_component.yml new file mode 100644 index 000000000..9774f9fad --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/idf_component.yml @@ -0,0 +1,3 @@ +dependencies: + espressif/pid_ctrl: "^0.2.0" + idf: ">=4.4" 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 new file mode 100644 index 000000000..7ea75e290 --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob.h @@ -0,0 +1,211 @@ +/* + * SPDX-FileCopyrightText: 2016-2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "esp_err.h" +#include "pid_ctrl.h" + +#ifndef PI +#define PI 3.14159265358979f +#endif + +typedef void (* foc_knob_cb_t)(void *foc_knob_handle, void *user_data); +typedef void *foc_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 */ +} 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 { + 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 + * + * @param config Pointer to knob configuration + * + * @return A handle to the created knob + */ +foc_knob_handle_t foc_knob_create(const foc_knob_config_t *config); + +/** + * @brief Change the mode of the knob. + * + * @param handle A handle to the knob. + * @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. + */ +esp_err_t foc_knob_change_mode(foc_knob_handle_t handle, uint16_t mode); + +/** + * @brief Start knob operation. + * + * This function controls the operation of a knob based on the specified mode, shaft velocity, + * and shaft angle. + * + * @param handle A handle to the knob. + * @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 foc_knob_run(foc_knob_handle_t handle, float shaft_velocity, float shaft_angle); + +/** + * @brief Delete a knob and free associated memory. + * + * This function is used to delete a knob created using the knob_create function. + * It frees the dynamically allocated memory for the knob structure. + * + * @param handle A handle to the knob to be deleted. +* @return ESP_OK on success +*/ +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 +} +#endif 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 new file mode 100644 index 000000000..a88b34e2f --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/include/foc_knob_default.h @@ -0,0 +1,33 @@ +/* + * SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* default knob modes */ +enum { + 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 foc_knob_param_t const *default_foc_knob_param_lst[]; + +#ifdef __cplusplus +} +#endif diff --git a/examples/motor/foc_knob_example/components/foc_knob/license.txt b/examples/motor/foc_knob_example/components/foc_knob/license.txt new file mode 100644 index 000000000..d64569567 --- /dev/null +++ b/examples/motor/foc_knob_example/components/foc_knob/license.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/examples/motor/foc_knob_example/main/CMakeLists.txt b/examples/motor/foc_knob_example/main/CMakeLists.txt new file mode 100644 index 000000000..6c537eb5c --- /dev/null +++ b/examples/motor/foc_knob_example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRCS "main.cpp" + ) diff --git a/examples/motor/foc_knob_example/main/idf_component.yml b/examples/motor/foc_knob_example/main/idf_component.yml new file mode 100644 index 000000000..25cdc1f7c --- /dev/null +++ b/examples/motor/foc_knob_example/main/idf_component.yml @@ -0,0 +1,6 @@ +dependencies: + idf: ">=4.4" + esp_simplefoc: + override_path: "../../../../components/motor/esp_simplefoc" + espressif/button: + version: "~3.1.0" diff --git a/examples/motor/foc_knob_example/main/main.cpp b/examples/motor/foc_knob_example/main/main.cpp new file mode 100644 index 000000000..910cd9592 --- /dev/null +++ b/examples/motor/foc_knob_example/main/main.cpp @@ -0,0 +1,172 @@ +/* + * SPDX-FileCopyrightText: 2016-2023 Espressif Systems (Shanghai) CO LTD + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "esp_log.h" +#include "esp_timer.h" +#include "driver/gpio.h" +#include "foc_knob.h" +#include "foc_knob_default.h" +#include "esp_simplefoc.h" +#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 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(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) +{ + SimpleFOCDebug::enable(); + Serial.begin(115200); + mt6701.init(); + motor.linkSensor(&mt6701); + driver.voltage_power_supply = 5; + driver.voltage_limit = 5; + +#if USING_MCPWM + driver.init(0); +#else + driver.init({LEDC_CHAN_0, LEDC_CHAN_1, LEDC_CHAN_2}); +#endif + + motor.linkDriver(&driver); + motor.foc_modulation = SpaceVectorPWM; + motor.controller = MotionControlType::torque; + + motor.useMonitoring(Serial); + motor.init(); // initialize motor + motor.initFOC(); // align sensor and start FOC + ESP_LOGI(TAG, "Motor Initialize Successfully"); +} + +/*Button press callback*/ +static void button_press_cb(void *arg, void *data) +{ + mode++; + if (mode >= MOTOR_MAX_MODES) { + 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) +{ + button_config_t btn_config = { + .type = BUTTON_TYPE_GPIO, + .long_press_time = 0, + .short_press_time = 0, + .gpio_button_config = { + .gpio_num = SWITCH_BUTTON, + .active_level = 0, + } + }; + + button_handle_t btn = iot_button_create(&btn_config); + iot_button_register_cb(btn, BUTTON_PRESS_DOWN, button_press_cb, NULL); + motor_init(); + + 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, + }; + + foc_knob_handle = foc_knob_create(&cfg); + + 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_knob_example/sdkconfig.defaults b/examples/motor/foc_knob_example/sdkconfig.defaults new file mode 100644 index 000000000..95af99dd7 --- /dev/null +++ b/examples/motor/foc_knob_example/sdkconfig.defaults @@ -0,0 +1,5 @@ +# This file was generated using idf.py save-defconfig. It can be edited manually. +# Espressif IoT Development Framework (ESP-IDF) Project Minimal Configuration +# +CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y +CONFIG_FREERTOS_HZ=1000 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)