mirror of
https://github.com/espressif/esp-idf.git
synced 2024-09-20 00:36:01 -04:00
example: update bdc speed control example with new driver API
This commit is contained in:
morris
parent
1571417679
commit
938b3d717f
@ -70,6 +70,22 @@ examples/peripherals/mcpwm:
|
||||
disable:
|
||||
- if: SOC_MCPWM_SUPPORTED != 1
|
||||
|
||||
examples/peripherals/mcpwm/mcpwm_bdc_speed_control:
|
||||
disable:
|
||||
- if: SOC_MCPWM_SUPPORTED != 1
|
||||
disable_test:
|
||||
- if: IDF_TARGET != "esp32s3"
|
||||
temporary: true
|
||||
reason: lack of runners
|
||||
|
||||
examples/peripherals/mcpwm/mcpwm_bldc_hall_control:
|
||||
disable:
|
||||
- if: SOC_MCPWM_SUPPORTED != 1
|
||||
disable_test:
|
||||
- if: IDF_TARGET != "esp32s3"
|
||||
temporary: true
|
||||
reason: lack of runners
|
||||
|
||||
examples/peripherals/pcnt:
|
||||
disable:
|
||||
- if: SOC_PCNT_SUPPORTED != 1
|
||||
|
||||
@ -4,7 +4,11 @@
|
||||
|
||||
(See the README.md file in the upper level 'examples' directory for more information about examples.)
|
||||
|
||||
This example mainly illustrates how to drive a brushed DC motor by generating two specific PWM signals. However the PWM signals from ESP32 can't drive motors directly as the motor usually consumes high current. So an H-bridge like [DRV8848](https://www.ti.com/product/DRV8848) should be used to provide the needed voltage and current for brushed DC motor. To measure the speed of motor, a photoelectric encoder is used to generate the "speed feedback" signals (e.g. a pair of quadrature signal). The example uses a simple PID control approach to keep the motor speed in a constant speed. The example provides a console command line interface for user to update the PID parameters according to actual situation.
|
||||
This example mainly illustrates how to drive a brushed DC motor by generating two specific PWM signals. However the PWM signals from ESP chip can't drive motors directly as the motor usually consumes high current. So an H-bridge like [DRV8848](https://www.ti.com/product/DRV8848) should be used to provide the needed voltage and current for brushed DC motor. To simplify the DC motor control of MCPWM peripheral driver, there's a component called [bdc_motor](components/bdc_motor/README.md) which abstracts the common operations into a generic interface. The most useful operations are: `forward`, `reverse`, `coast` and `brake`.
|
||||
|
||||
To measure the speed of motor, a photoelectric encoder is used to generate the "speed feedback" signals (e.g. a pair of quadrature signal). In the example, we use the PCNT peripheral to decode that quadrature signals. For more information, please refer to [rotary encoder example](../../pcnt/rotary_encoder/README.md) as well.
|
||||
|
||||
The example uses a simple PID algorithm to keep the motor spin in a stable speed. The PID component is fetched from the [IDF Component Registry](https://components.espressif.com/component/espressif/pid_ctrl).
|
||||
|
||||
## How to Use Example
|
||||
|
||||
@ -12,7 +16,7 @@ Before project configuration and build, be sure to set the correct chip target u
|
||||
|
||||
### Hardware Required
|
||||
|
||||
* A development board with any Espressif SoC which features MCPWM and PCNT peripheral (e.g., ESP32-DevKitC, ESP-WROVER-KIT, etc.)
|
||||
* A development board with any Espressif SoC which features MCPWM and PCNT peripheral (e.g., ESP32-DevKitC, ESP32-S3-Motor-Devkit, etc.)
|
||||
* A USB cable for Power supply and programming
|
||||
* A separate 12V power supply for brushed DC motor and H-bridge (the voltage depends on the motor model used in the example)
|
||||
* A motor driving board to transfer pwm signal into driving signal
|
||||
@ -23,16 +27,16 @@ Connection :
|
||||
```
|
||||
Power(12V)
|
||||
|
|
||||
v
|
||||
+----------------+ +--------------------+
|
||||
ESP v
|
||||
+-------------------+ +--------------------+
|
||||
| | | H-Bridge |
|
||||
| GND +<----------->| GND | +--------------+
|
||||
| | | | | |
|
||||
| GENA_GPIO_NUM +----PWM0A--->| IN_A OUT_A +----->| Brushed |
|
||||
| BDC_MCPWM_GPIO_A +----PWM0A--->| IN_A OUT_A +----->| Brushed |
|
||||
| | | | | DC |
|
||||
| GENB_GPIO_NUM +----PWM0B--->| IN_B OUT_B +----->| Motor |
|
||||
| BDC_MCPWM_GPIO_B +----PWM0B--->| IN_B OUT_B +----->| Motor |
|
||||
| | | | | |
|
||||
| ESP | +--------------------+ | |
|
||||
| | +--------------------+ | |
|
||||
| | +------+-------+
|
||||
| | |
|
||||
| | +--------------------+ |
|
||||
@ -40,11 +44,11 @@ Connection :
|
||||
| | | | |
|
||||
| GND +<----------->| |<------------+
|
||||
| | | |
|
||||
|PHASEA_GPIO_NUM |<---PhaseA---+ C1 |
|
||||
|BDC_ENCODER_GPIO_A |<---PhaseA---+ C1 |
|
||||
| | | |
|
||||
|PHASEB_GPIO_NUM |<---PhaseB---+ C2 |
|
||||
|BDC_ENCODER_GPIO_B |<---PhaseB---+ C2 |
|
||||
| | | |
|
||||
+----------------+ +--------------------+
|
||||
+-------------------+ +--------------------+
|
||||
```
|
||||
|
||||
### Build and Flash
|
||||
@ -62,51 +66,30 @@ Run the example, you will see the following output log:
|
||||
|
||||
```
|
||||
I (0) cpu_start: Starting scheduler on APP CPU.
|
||||
configure mcpwm gpio
|
||||
init mcpwm driver
|
||||
init and start rotary encoder
|
||||
init PID control block
|
||||
init motor control timer
|
||||
D (561) gptimer: new group (0) @0x3fce0a24
|
||||
D (561) gptimer: new gptimer (0,0) at 0x3fce0964, resolution=1000000Hz
|
||||
create motor control task
|
||||
start motor control timer
|
||||
D (571) gptimer: install interrupt service for timer (0,0)
|
||||
install console command line
|
||||
|
||||
Type 'help' to get the list of commands.
|
||||
Use UP/DOWN arrows to navigate through command history.
|
||||
Press TAB when typing command name to auto-complete.
|
||||
dc-motor>
|
||||
dc-motor> help
|
||||
help
|
||||
Print the list of registered commands
|
||||
|
||||
pid [-p <kp>] [-i <ki>] [-d <kd>]
|
||||
Set PID parameters
|
||||
-p <kp> Set Kp value of PID
|
||||
-i <ki> Set Ki value of PID
|
||||
-d <kd> Set Kd value of PID
|
||||
I (308) example: Create DC motor
|
||||
I (308) gpio: GPIO[7]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0
|
||||
I (318) gpio: GPIO[15]| InputEn: 0| OutputEn: 1| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0
|
||||
I (328) example: Init pcnt driver to decode rotary signal
|
||||
I (328) gpio: GPIO[36]| InputEn: 1| OutputEn: 0| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0
|
||||
I (338) gpio: GPIO[35]| InputEn: 1| OutputEn: 0| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0
|
||||
I (348) gpio: GPIO[35]| InputEn: 1| OutputEn: 0| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0
|
||||
I (358) gpio: GPIO[36]| InputEn: 1| OutputEn: 0| OpenDrain: 0| Pullup: 1| Pulldown: 0| Intr:0
|
||||
I (368) example: Create PID control block
|
||||
I (378) example: Create a timer to do PID calculation periodically
|
||||
I (378) example: Enable motor
|
||||
I (388) example: Forward motor
|
||||
I (388) example: Start motor speed loop
|
||||
```
|
||||
|
||||
### Set PID parameters
|
||||
### View velocity curve in [Serial Studio](https://github.com/Serial-Studio/Serial-Studio)
|
||||
|
||||
* Command: `pid -p <double> -i <double> -d <double> -t <loc/inc>`
|
||||
* 'p' - proportion value
|
||||
* 'i' - integral value
|
||||
* 'd' - differential value
|
||||
* 't' - PID calculation type (locational or incremental).
|
||||
To help tune the PID parameters (i.e. `Kp`, `Ki` and `Kd` in the example), this example supports to log a short string frame of runtime motor speed. The string frame can be parsed by [Serial Studio](https://github.com/Serial-Studio/Serial-Studio). This example also provides the [communication description file](serial-studio-dashboard.json) out of the box, which can be loaded by Serial Studio and then plot the curves as follows:
|
||||
|
||||
```bash
|
||||
mcpwm-motor> pid -p 0.8 -i 0.02 -d 0.1 -t inc
|
||||
pid: kp = 0.800
|
||||
pid: ki = 0.020
|
||||
pid: kd = 0.100
|
||||
pid: type = increment
|
||||
```
|
||||
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
* Make sure your ESP board and H-bridge module have been connected to the same GND panel.
|
||||
* The PID parameter set in ths example might not work well in all kinds of motors, because it's not adaptive. You need to fine tune the parameters again by yourself.
|
||||
|
||||
For any technical queries, please open an [issue](https://github.com/espressif/esp-idf/issues) on GitHub. We will get back to you soon.
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 122 KiB |
@ -0,0 +1,9 @@
|
||||
set(srcs "src/bdc_motor.c")
|
||||
|
||||
if(CONFIG_SOC_MCPWM_SUPPORTED)
|
||||
list(APPEND srcs "src/bdc_motor_mcpwm_impl.c")
|
||||
endif()
|
||||
|
||||
idf_component_register(SRCS ${srcs}
|
||||
INCLUDE_DIRS "include" "interface"
|
||||
PRIV_REQUIRES "driver")
|
||||
@ -0,0 +1,7 @@
|
||||
# Brushed DC Motor Component
|
||||
|
||||
This directory contains an implementation for Brushed DC Motor by different peripherals. Currently only MCPWM is supported as the BDC motor backend.
|
||||
|
||||
To learn more about how to use this component, please check API Documentation from header file [bdc_motor.h](./include/bdc_motor.h).
|
||||
|
||||
Please note that this component is not considered to be a part of ESP-IDF stable API. It may change and it may be removed in the future releases.
|
||||
@ -0,0 +1,145 @@
|
||||
/*
|
||||
* SPDX-FileCopyrightText: 2022 Espressif Systems (Shanghai) CO LTD
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "esp_err.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/**
|
||||
* @brief Brushed DC Motor handle
|
||||
*/
|
||||
typedef struct bdc_motor_t *bdc_motor_handle_t;
|
||||
|
||||
/**
|
||||
* @brief Enable BDC motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Enable motor successfully
|
||||
* - ESP_ERR_INVALID_ARG: Enable motor failed because of invalid parameters
|
||||
* - ESP_FAIL: Enable motor failed because other error occurred
|
||||
*/
|
||||
esp_err_t bdc_motor_enable(bdc_motor_handle_t motor);
|
||||
|
||||
/**
|
||||
* @brief Disable BDC motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Disable motor successfully
|
||||
* - ESP_ERR_INVALID_ARG: Disable motor failed because of invalid parameters
|
||||
* - ESP_FAIL: Disable motor failed because other error occurred
|
||||
*/
|
||||
esp_err_t bdc_motor_disable(bdc_motor_handle_t motor);
|
||||
|
||||
/**
|
||||
* @brief Set speed for bdc motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
* @param speed: BDC speed
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Set motor speed successfully
|
||||
* - ESP_ERR_INVALID_ARG: Set motor speed failed because of invalid parameters
|
||||
* - ESP_FAIL: Set motor speed failed because other error occurred
|
||||
*/
|
||||
esp_err_t bdc_motor_set_speed(bdc_motor_handle_t motor, uint32_t speed);
|
||||
|
||||
/**
|
||||
* @brief Forward BDC motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Forward motor successfully
|
||||
* - ESP_FAIL: Forward motor failed because some other error occurred
|
||||
*/
|
||||
esp_err_t bdc_motor_forward(bdc_motor_handle_t motor);
|
||||
|
||||
/**
|
||||
* @brief Reverse BDC Motor
|
||||
*
|
||||
* @param strip: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Reverse motor successfully
|
||||
* - ESP_FAIL: Reverse motor failed because some other error occurred
|
||||
*/
|
||||
esp_err_t bdc_motor_reverse(bdc_motor_handle_t motor);
|
||||
|
||||
/**
|
||||
* @brief Stop motor in a coast way (a.k.a Fast Decay)
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Stop motor successfully
|
||||
* - ESP_FAIL: Stop motor failed because some other error occurred
|
||||
*/
|
||||
esp_err_t bdc_motor_coast(bdc_motor_handle_t motor);
|
||||
|
||||
/**
|
||||
* @brief Stop motor in a brake way (a.k.a Slow Decay)
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Stop motor successfully
|
||||
* - ESP_FAIL: Stop motor failed because some other error occurred
|
||||
*/
|
||||
esp_err_t bdc_motor_brake(bdc_motor_handle_t motor);
|
||||
|
||||
/**
|
||||
* @brief Free BDC Motor resources
|
||||
*
|
||||
* @param strip: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Free resources successfully
|
||||
* - ESP_FAIL: Free resources failed because error occurred
|
||||
*/
|
||||
esp_err_t bdc_motor_del(bdc_motor_handle_t motor);
|
||||
|
||||
/**
|
||||
* @brief BDC Motor Configuration
|
||||
*/
|
||||
typedef struct {
|
||||
uint32_t pwma_gpio_num; /*!< BDC Motor PWM A gpio number */
|
||||
uint32_t pwmb_gpio_num; /*!< BDC Motor PWM B gpio number */
|
||||
uint32_t pwm_freq_hz; /*!< PWM frequency, in Hz */
|
||||
} bdc_motor_config_t;
|
||||
|
||||
/**
|
||||
* @brief BDC Motor MCPWM specific configuration
|
||||
*/
|
||||
typedef struct {
|
||||
int group_id; /*!< MCPWM group number */
|
||||
uint32_t resolution_hz; /*!< MCPWM timer resolution */
|
||||
} bdc_motor_mcpwm_config_t;
|
||||
|
||||
/**
|
||||
* @brief Create BDC Motor based on MCPWM peripheral
|
||||
*
|
||||
* @param motor_config: BDC Motor configuration
|
||||
* @param mcpwm_config: MCPWM specific configuration
|
||||
* @param ret_motor Returned BDC Motor handle
|
||||
* @return
|
||||
* - ESP_OK: Create BDC Motor handle successfully
|
||||
* - ESP_ERR_INVALID_ARG: Create BDC Motor handle failed because of invalid argument
|
||||
* - ESP_ERR_NO_MEM: Create BDC Motor handle failed because of out of memory
|
||||
* - ESP_FAIL: Create BDC Motor handle failed because some other error
|
||||
*/
|
||||
esp_err_t bdc_motor_new_mcpwm_device(const bdc_motor_config_t *motor_config, const bdc_motor_mcpwm_config_t *mcpwm_config, bdc_motor_handle_t *ret_motor);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@ -0,0 +1,116 @@
|
||||
/*
|
||||
* SPDX-FileCopyrightText: 2022 Espressif Systems (Shanghai) CO LTD
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "esp_err.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
typedef struct bdc_motor_t bdc_motor_t; /*!< Type of BDC motor */
|
||||
|
||||
/**
|
||||
* @brief BDC motor interface definition
|
||||
*/
|
||||
struct bdc_motor_t {
|
||||
/**
|
||||
* @brief Enable BDC motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Enable motor successfully
|
||||
* - ESP_ERR_INVALID_ARG: Enable motor failed because of invalid parameters
|
||||
* - ESP_FAIL: Enable motor failed because other error occurred
|
||||
*/
|
||||
esp_err_t (*enable)(bdc_motor_t *motor);
|
||||
|
||||
/**
|
||||
* @brief Disable BDC motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Disable motor successfully
|
||||
* - ESP_ERR_INVALID_ARG: Disable motor failed because of invalid parameters
|
||||
* - ESP_FAIL: Disable motor failed because other error occurred
|
||||
*/
|
||||
esp_err_t (*disable)(bdc_motor_t *motor);
|
||||
|
||||
/**
|
||||
* @brief Set speed for bdc motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
* @param speed: BDC speed
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Set motor speed successfully
|
||||
* - ESP_ERR_INVALID_ARG: Set motor speed failed because of invalid parameters
|
||||
* - ESP_FAIL: Set motor speed failed because other error occurred
|
||||
*/
|
||||
esp_err_t (*set_speed)(bdc_motor_t *motor, uint32_t speed);
|
||||
|
||||
/**
|
||||
* @brief Forward BDC motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Forward motor successfully
|
||||
* - ESP_FAIL: Forward motor failed because some other error occurred
|
||||
*/
|
||||
esp_err_t (*forward)(bdc_motor_t *motor);
|
||||
|
||||
/**
|
||||
* @brief Reverse BDC Motor
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Reverse motor successfully
|
||||
* - ESP_FAIL: Reverse motor failed because some other error occurred
|
||||
*/
|
||||
esp_err_t (*reverse)(bdc_motor_t *motor);
|
||||
|
||||
/**
|
||||
* @brief Stop motor in a coast way (a.k.a Fast Decay)
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Stop motor successfully
|
||||
* - ESP_FAIL: Stop motor failed because some other error occurred
|
||||
*/
|
||||
esp_err_t (*coast)(bdc_motor_t *motor);
|
||||
|
||||
/**
|
||||
* @brief Stop motor in a brake way (a.k.a Slow Decay)
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Stop motor successfully
|
||||
* - ESP_FAIL: Stop motor failed because some other error occurred
|
||||
*/
|
||||
esp_err_t (*brake)(bdc_motor_t *motor);
|
||||
|
||||
/**
|
||||
* @brief Free BDC Motor handle resources
|
||||
*
|
||||
* @param motor: BDC Motor handle
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK: Free resources successfully
|
||||
* - ESP_FAIL: Free resources failed because error occurred
|
||||
*/
|
||||
esp_err_t (*del)(bdc_motor_t *motor);
|
||||
};
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@ -0,0 +1,62 @@
|
||||
/*
|
||||
* SPDX-FileCopyrightText: 2022 Espressif Systems (Shanghai) CO LTD
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/cdefs.h>
|
||||
#include "esp_log.h"
|
||||
#include "esp_check.h"
|
||||
#include "bdc_motor.h"
|
||||
#include "bdc_motor_interface.h"
|
||||
|
||||
static const char *TAG = "bdc_motor";
|
||||
|
||||
esp_err_t bdc_motor_enable(bdc_motor_handle_t motor)
|
||||
{
|
||||
ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
|
||||
return motor->enable(motor);
|
||||
}
|
||||
|
||||
esp_err_t bdc_motor_disable(bdc_motor_handle_t motor)
|
||||
{
|
||||
ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
|
||||
return motor->disable(motor);
|
||||
}
|
||||
|
||||
esp_err_t bdc_motor_set_speed(bdc_motor_handle_t motor, uint32_t speed)
|
||||
{
|
||||
ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
|
||||
return motor->set_speed(motor, speed);
|
||||
}
|
||||
|
||||
esp_err_t bdc_motor_forward(bdc_motor_handle_t motor)
|
||||
{
|
||||
ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
|
||||
return motor->forward(motor);
|
||||
}
|
||||
|
||||
esp_err_t bdc_motor_reverse(bdc_motor_handle_t motor)
|
||||
{
|
||||
ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
|
||||
return motor->reverse(motor);
|
||||
}
|
||||
|
||||
esp_err_t bdc_motor_coast(bdc_motor_handle_t motor)
|
||||
{
|
||||
ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
|
||||
return motor->coast(motor);
|
||||
}
|
||||
|
||||
esp_err_t bdc_motor_brake(bdc_motor_handle_t motor)
|
||||
{
|
||||
ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
|
||||
return motor->brake(motor);
|
||||
}
|
||||
|
||||
esp_err_t bdc_motor_del(bdc_motor_handle_t motor)
|
||||
{
|
||||
ESP_RETURN_ON_FALSE(motor, ESP_ERR_INVALID_ARG, TAG, "invalid argument");
|
||||
return motor->del(motor);
|
||||
}
|
||||
@ -0,0 +1,185 @@
|
||||
/*
|
||||
* SPDX-FileCopyrightText: 2022 Espressif Systems (Shanghai) CO LTD
|
||||
*
|
||||
* SPDX-License-Identifier: Apache-2.0
|
||||
*/
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/cdefs.h>
|
||||
#include "esp_log.h"
|
||||
#include "esp_check.h"
|
||||
#include "driver/mcpwm_prelude.h"
|
||||
#include "bdc_motor.h"
|
||||
#include "bdc_motor_interface.h"
|
||||
|
||||
static const char *TAG = "bdc_motor_mcpwm";
|
||||
|
||||
typedef struct {
|
||||
bdc_motor_t base;
|
||||
mcpwm_timer_handle_t timer;
|
||||
mcpwm_oper_handle_t operator;
|
||||
mcpwm_cmpr_handle_t cmpa;
|
||||
mcpwm_cmpr_handle_t cmpb;
|
||||
mcpwm_gen_handle_t gena;
|
||||
mcpwm_gen_handle_t genb;
|
||||
} bdc_motor_mcpwm_obj;
|
||||
|
||||
static esp_err_t bdc_motor_mcpwm_set_speed(bdc_motor_t *motor, uint32_t speed)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = __containerof(motor, bdc_motor_mcpwm_obj, base);
|
||||
ESP_RETURN_ON_ERROR(mcpwm_comparator_set_compare_value(mcpwm_motor->cmpa, speed), TAG, "set compare value failed");
|
||||
ESP_RETURN_ON_ERROR(mcpwm_comparator_set_compare_value(mcpwm_motor->cmpb, speed), TAG, "set compare value failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t bdc_motor_mcpwm_enable(bdc_motor_t *motor)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = __containerof(motor, bdc_motor_mcpwm_obj, base);
|
||||
ESP_RETURN_ON_ERROR(mcpwm_timer_enable(mcpwm_motor->timer), TAG, "enable timer failed");
|
||||
ESP_RETURN_ON_ERROR(mcpwm_timer_start_stop(mcpwm_motor->timer, MCPWM_TIMER_START_NO_STOP), TAG, "start timer failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t bdc_motor_mcpwm_disable(bdc_motor_t *motor)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = __containerof(motor, bdc_motor_mcpwm_obj, base);
|
||||
ESP_RETURN_ON_ERROR(mcpwm_timer_start_stop(mcpwm_motor->timer, MCPWM_TIMER_STOP_EMPTY), TAG, "stop timer failed");
|
||||
ESP_RETURN_ON_ERROR(mcpwm_timer_disable(mcpwm_motor->timer), TAG, "disable timer failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t bdc_motor_mcpwm_forward(bdc_motor_t *motor)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = __containerof(motor, bdc_motor_mcpwm_obj, base);
|
||||
ESP_RETURN_ON_ERROR(mcpwm_generator_set_force_level(mcpwm_motor->gena, -1, true), TAG, "disable force level for gena failed");
|
||||
ESP_RETURN_ON_ERROR(mcpwm_generator_set_force_level(mcpwm_motor->genb, 0, true), TAG, "set force level for genb failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t bdc_motor_mcpwm_reverse(bdc_motor_t *motor)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = __containerof(motor, bdc_motor_mcpwm_obj, base);
|
||||
ESP_RETURN_ON_ERROR(mcpwm_generator_set_force_level(mcpwm_motor->genb, -1, true), TAG, "disable force level for genb failed");
|
||||
ESP_RETURN_ON_ERROR(mcpwm_generator_set_force_level(mcpwm_motor->gena, 0, true), TAG, "set force level for gena failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t bdc_motor_mcpwm_coast(bdc_motor_t *motor)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = __containerof(motor, bdc_motor_mcpwm_obj, base);
|
||||
ESP_RETURN_ON_ERROR(mcpwm_generator_set_force_level(mcpwm_motor->gena, 0, true), TAG, "set force level for gena failed");
|
||||
ESP_RETURN_ON_ERROR(mcpwm_generator_set_force_level(mcpwm_motor->genb, 0, true), TAG, "set force level for genb failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t bdc_motor_mcpwm_brake(bdc_motor_t *motor)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = __containerof(motor, bdc_motor_mcpwm_obj, base);
|
||||
ESP_RETURN_ON_ERROR(mcpwm_generator_set_force_level(mcpwm_motor->gena, 1, true), TAG, "set force level for gena failed");
|
||||
ESP_RETURN_ON_ERROR(mcpwm_generator_set_force_level(mcpwm_motor->genb, 1, true), TAG, "set force level for genb failed");
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
static esp_err_t bdc_motor_mcpwm_del(bdc_motor_t *motor)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = __containerof(motor, bdc_motor_mcpwm_obj, base);
|
||||
mcpwm_del_generator(mcpwm_motor->gena);
|
||||
mcpwm_del_generator(mcpwm_motor->genb);
|
||||
mcpwm_del_comparator(mcpwm_motor->cmpa);
|
||||
mcpwm_del_comparator(mcpwm_motor->cmpb);
|
||||
mcpwm_del_operator(mcpwm_motor->operator);
|
||||
mcpwm_del_timer(mcpwm_motor->timer);
|
||||
free(mcpwm_motor);
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t bdc_motor_new_mcpwm_device(const bdc_motor_config_t *motor_config, const bdc_motor_mcpwm_config_t *mcpwm_config, bdc_motor_handle_t *ret_motor)
|
||||
{
|
||||
bdc_motor_mcpwm_obj *mcpwm_motor = NULL;
|
||||
esp_err_t ret = ESP_OK;
|
||||
ESP_GOTO_ON_FALSE(motor_config && mcpwm_config && ret_motor, ESP_ERR_INVALID_ARG, err, TAG, "invalid argument");
|
||||
mcpwm_motor = calloc(1, sizeof(bdc_motor_mcpwm_obj));
|
||||
ESP_GOTO_ON_FALSE(mcpwm_motor, ESP_ERR_NO_MEM, err, TAG, "no mem for rmt motor");
|
||||
|
||||
// mcpwm timer
|
||||
mcpwm_timer_config_t timer_config = {
|
||||
.group_id = mcpwm_config->group_id,
|
||||
.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
|
||||
.resolution_hz = mcpwm_config->resolution_hz,
|
||||
.period_ticks = mcpwm_config->resolution_hz / motor_config->pwm_freq_hz,
|
||||
.count_mode = MCPWM_TIMER_COUNT_MODE_UP,
|
||||
};
|
||||
ESP_GOTO_ON_ERROR(mcpwm_new_timer(&timer_config, &mcpwm_motor->timer), err, TAG, "create MCPWM timer failed");
|
||||
|
||||
mcpwm_operator_config_t operator_config = {
|
||||
.group_id = mcpwm_config->group_id,
|
||||
};
|
||||
ESP_GOTO_ON_ERROR(mcpwm_new_operator(&operator_config, &mcpwm_motor->operator), err, TAG, "create MCPWM operator failed");
|
||||
|
||||
ESP_GOTO_ON_ERROR(mcpwm_operator_connect_timer(mcpwm_motor->operator, mcpwm_motor->timer), err, TAG, "connect timer and operator failed");
|
||||
|
||||
mcpwm_comparator_config_t comparator_config = {
|
||||
.flags.update_cmp_on_tez = true,
|
||||
};
|
||||
ESP_GOTO_ON_ERROR(mcpwm_new_comparator(mcpwm_motor->operator, &comparator_config, &mcpwm_motor->cmpa), err, TAG, "create comparator failed");
|
||||
ESP_GOTO_ON_ERROR(mcpwm_new_comparator(mcpwm_motor->operator, &comparator_config, &mcpwm_motor->cmpb), err, TAG, "create comparator failed");
|
||||
|
||||
// set the initial compare value for both comparators
|
||||
mcpwm_comparator_set_compare_value(mcpwm_motor->cmpa, 0);
|
||||
mcpwm_comparator_set_compare_value(mcpwm_motor->cmpb, 0);
|
||||
|
||||
mcpwm_generator_config_t generator_config = {
|
||||
.gen_gpio_num = motor_config->pwma_gpio_num,
|
||||
};
|
||||
ESP_GOTO_ON_ERROR(mcpwm_new_generator(mcpwm_motor->operator, &generator_config, &mcpwm_motor->gena), err, TAG, "create generator failed");
|
||||
generator_config.gen_gpio_num = motor_config->pwmb_gpio_num;
|
||||
ESP_GOTO_ON_ERROR(mcpwm_new_generator(mcpwm_motor->operator, &generator_config, &mcpwm_motor->genb), err, TAG, "create generator failed");
|
||||
|
||||
mcpwm_generator_set_actions_on_timer_event(mcpwm_motor->gena,
|
||||
MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH),
|
||||
MCPWM_GEN_TIMER_EVENT_ACTION_END());
|
||||
mcpwm_generator_set_actions_on_compare_event(mcpwm_motor->gena,
|
||||
MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, mcpwm_motor->cmpa, MCPWM_GEN_ACTION_LOW),
|
||||
MCPWM_GEN_COMPARE_EVENT_ACTION_END());
|
||||
mcpwm_generator_set_actions_on_timer_event(mcpwm_motor->genb,
|
||||
MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH),
|
||||
MCPWM_GEN_TIMER_EVENT_ACTION_END());
|
||||
mcpwm_generator_set_actions_on_compare_event(mcpwm_motor->genb,
|
||||
MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, mcpwm_motor->cmpb, MCPWM_GEN_ACTION_LOW),
|
||||
MCPWM_GEN_COMPARE_EVENT_ACTION_END());
|
||||
|
||||
mcpwm_motor->base.enable = bdc_motor_mcpwm_enable;
|
||||
mcpwm_motor->base.disable = bdc_motor_mcpwm_disable;
|
||||
mcpwm_motor->base.forward = bdc_motor_mcpwm_forward;
|
||||
mcpwm_motor->base.reverse = bdc_motor_mcpwm_reverse;
|
||||
mcpwm_motor->base.coast = bdc_motor_mcpwm_coast;
|
||||
mcpwm_motor->base.brake = bdc_motor_mcpwm_brake;
|
||||
mcpwm_motor->base.set_speed = bdc_motor_mcpwm_set_speed;
|
||||
mcpwm_motor->base.del = bdc_motor_mcpwm_del;
|
||||
*ret_motor = &mcpwm_motor->base;
|
||||
return ESP_OK;
|
||||
|
||||
err:
|
||||
if (mcpwm_motor) {
|
||||
if (mcpwm_motor->gena) {
|
||||
mcpwm_del_generator(mcpwm_motor->gena);
|
||||
}
|
||||
if (mcpwm_motor->genb) {
|
||||
mcpwm_del_generator(mcpwm_motor->genb);
|
||||
}
|
||||
if (mcpwm_motor->cmpa) {
|
||||
mcpwm_del_comparator(mcpwm_motor->cmpa);
|
||||
}
|
||||
if (mcpwm_motor->cmpb) {
|
||||
mcpwm_del_comparator(mcpwm_motor->cmpb);
|
||||
}
|
||||
if (mcpwm_motor->operator) {
|
||||
mcpwm_del_operator(mcpwm_motor->operator);
|
||||
}
|
||||
if (mcpwm_motor->timer) {
|
||||
mcpwm_del_timer(mcpwm_motor->timer);
|
||||
}
|
||||
free(mcpwm_motor);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
@ -0,0 +1,12 @@
|
||||
menu "Example Configuration"
|
||||
|
||||
config SERIAL_STUDIO_DEBUG
|
||||
bool "Enable log that can be parsed by Serial Studio"
|
||||
default "n"
|
||||
help
|
||||
Enable this option, the example will print a string at runtime with a specific format,
|
||||
which can be parsed by the Serial Studio tool.
|
||||
With the "serial-studio-dashboard.json" template file provided in this example,
|
||||
user can observe the speed in a curve window in the Serial Studio.
|
||||
|
||||
endmenu
|
||||
@ -5,176 +5,97 @@
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include "sdkconfig.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "freertos/queue.h"
|
||||
#include "driver/gptimer.h"
|
||||
#include "esp_log.h"
|
||||
#include "esp_timer.h"
|
||||
#include "driver/pulse_cnt.h"
|
||||
#include "driver/mcpwm.h"
|
||||
#include "bdc_motor.h"
|
||||
#include "pid_ctrl.h"
|
||||
#include "esp_console.h"
|
||||
#include "argtable3/argtable3.h"
|
||||
|
||||
static const char *TAG = "example";
|
||||
|
||||
// Enable this config, we will print debug formated string, which in return can be captured and parsed by Serial-Studio
|
||||
#define SERIAL_STUDIO_DEBUG 0
|
||||
#define SERIAL_STUDIO_DEBUG CONFIG_SERIAL_STUDIO_DEBUG
|
||||
|
||||
#define BDC_MCPWM_UNIT 0
|
||||
#define BDC_MCPWM_TIMER 0
|
||||
#define BDC_MCPWM_GENA_GPIO_NUM 7
|
||||
#define BDC_MCPWM_GENB_GPIO_NUM 15
|
||||
#define BDC_MCPWM_FREQ_HZ 1500
|
||||
#define BDC_MCPWM_TIMER_RESOLUTION_HZ 10000000 // 10MHz, 1 tick = 0.1us
|
||||
#define BDC_MCPWM_FREQ_HZ 25000 // 25KHz PWM
|
||||
#define BDC_MCPWM_DUTY_TICK_MAX (BDC_MCPWM_TIMER_RESOLUTION_HZ / BDC_MCPWM_FREQ_HZ) // maximum value we can set for the duty cycle, in ticks
|
||||
#define BDC_MCPWM_GPIO_A 7
|
||||
#define BDC_MCPWM_GPIO_B 15
|
||||
|
||||
#define BDC_ENCODER_PCNT_HIGH_LIMIT 100
|
||||
#define BDC_ENCODER_PCNT_LOW_LIMIT -100
|
||||
#define BDC_ENCODER_PHASEA_GPIO_NUM 36
|
||||
#define BDC_ENCODER_PHASEB_GPIO_NUM 35
|
||||
#define BDC_ENCODER_GPIO_A 36
|
||||
#define BDC_ENCODER_GPIO_B 35
|
||||
#define BDC_ENCODER_PCNT_HIGH_LIMIT 1000
|
||||
#define BDC_ENCODER_PCNT_LOW_LIMIT -1000
|
||||
|
||||
#define BDC_PID_CALCULATION_PERIOD_US 10000
|
||||
#define BDC_PID_FEEDBACK_QUEUE_LEN 10
|
||||
|
||||
static pid_ctrl_parameter_t pid_runtime_param = {
|
||||
.kp = 0.6,
|
||||
.ki = 0.3,
|
||||
.kd = 0.12,
|
||||
.cal_type = PID_CAL_TYPE_INCREMENTAL,
|
||||
.max_output = 100,
|
||||
.min_output = -100,
|
||||
.max_integral = 1000,
|
||||
.min_integral = -1000,
|
||||
};
|
||||
static bool pid_need_update = false;
|
||||
static int expect_pulses = 300;
|
||||
static int real_pulses;
|
||||
#define BDC_PID_LOOP_PERIOD_MS 10 // calculate the motor speed every 10ms
|
||||
#define BDC_PID_EXPECT_SPEED 400 // expected motor speed, in the pulses counted by the rotary encoder
|
||||
|
||||
typedef struct {
|
||||
pcnt_unit_handle_t hall_pcnt_encoder;
|
||||
int accumu_count;
|
||||
QueueHandle_t pid_feedback_queue;
|
||||
} motor_control_timer_context_t;
|
||||
|
||||
typedef struct {
|
||||
QueueHandle_t pid_feedback_queue;
|
||||
bdc_motor_handle_t motor;
|
||||
pcnt_unit_handle_t pcnt_encoder;
|
||||
pid_ctrl_block_handle_t pid_ctrl;
|
||||
} motor_control_task_context_t;
|
||||
int accumu_count;
|
||||
int report_pulses;
|
||||
} motor_control_context_t;
|
||||
|
||||
static bool example_pcnt_on_reach(pcnt_unit_handle_t unit, const pcnt_watch_event_data_t *edata, void *user_ctx)
|
||||
{
|
||||
motor_control_timer_context_t *ctx = (motor_control_timer_context_t *)user_ctx;
|
||||
ctx->accumu_count += edata->watch_point_value;
|
||||
int *accumu_count = (int *)user_ctx;
|
||||
*accumu_count += edata->watch_point_value;
|
||||
return false;
|
||||
}
|
||||
|
||||
static void brushed_motor_set_duty(float duty_cycle)
|
||||
{
|
||||
/* motor moves in forward direction, with duty cycle = duty % */
|
||||
if (duty_cycle > 0) {
|
||||
mcpwm_set_signal_low(BDC_MCPWM_UNIT, BDC_MCPWM_TIMER, MCPWM_OPR_A);
|
||||
mcpwm_set_duty(BDC_MCPWM_UNIT, BDC_MCPWM_TIMER, MCPWM_OPR_B, duty_cycle);
|
||||
mcpwm_set_duty_type(BDC_MCPWM_UNIT, BDC_MCPWM_TIMER, MCPWM_OPR_B, MCPWM_DUTY_MODE_0);
|
||||
}
|
||||
/* motor moves in backward direction, with duty cycle = -duty % */
|
||||
else {
|
||||
mcpwm_set_signal_low(BDC_MCPWM_UNIT, BDC_MCPWM_TIMER, MCPWM_OPR_B);
|
||||
mcpwm_set_duty(BDC_MCPWM_UNIT, BDC_MCPWM_TIMER, MCPWM_OPR_A, -duty_cycle);
|
||||
mcpwm_set_duty_type(BDC_MCPWM_UNIT, BDC_MCPWM_TIMER, MCPWM_OPR_A, MCPWM_DUTY_MODE_0);
|
||||
}
|
||||
}
|
||||
|
||||
static bool motor_ctrl_timer_cb(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *arg)
|
||||
static void pid_loop_cb(void *args)
|
||||
{
|
||||
static int last_pulse_count = 0;
|
||||
BaseType_t high_task_awoken = pdFALSE;
|
||||
motor_control_timer_context_t *user_ctx = (motor_control_timer_context_t *)arg;
|
||||
pcnt_unit_handle_t pcnt_unit = user_ctx->hall_pcnt_encoder;
|
||||
motor_control_context_t *ctx = (motor_control_context_t *)args;
|
||||
pcnt_unit_handle_t pcnt_unit = ctx->pcnt_encoder;
|
||||
pid_ctrl_block_handle_t pid_ctrl = ctx->pid_ctrl;
|
||||
bdc_motor_handle_t motor = ctx->motor;
|
||||
|
||||
// get the result from rotary encoder
|
||||
int cur_pulse_count = 0;
|
||||
pcnt_unit_get_count(pcnt_unit, &cur_pulse_count);
|
||||
cur_pulse_count += user_ctx->accumu_count;
|
||||
|
||||
int delta = cur_pulse_count - last_pulse_count;
|
||||
cur_pulse_count += ctx->accumu_count;
|
||||
int real_pulses = cur_pulse_count - last_pulse_count;
|
||||
last_pulse_count = cur_pulse_count;
|
||||
xQueueSendFromISR(user_ctx->pid_feedback_queue, &delta, &high_task_awoken);
|
||||
ctx->report_pulses = real_pulses;
|
||||
|
||||
return high_task_awoken == pdTRUE;
|
||||
}
|
||||
// calculate the speed error
|
||||
float error = BDC_PID_EXPECT_SPEED - real_pulses;
|
||||
float new_speed = 0;
|
||||
|
||||
static void bdc_ctrl_task(void *arg)
|
||||
{
|
||||
float duty_cycle = 0;
|
||||
motor_control_task_context_t *user_ctx = (motor_control_task_context_t *)arg;
|
||||
while (1) {
|
||||
xQueueReceive(user_ctx->pid_feedback_queue, &real_pulses, portMAX_DELAY);
|
||||
float error = expect_pulses - real_pulses;
|
||||
pid_compute(user_ctx->pid_ctrl, error, &duty_cycle);
|
||||
brushed_motor_set_duty(duty_cycle);
|
||||
}
|
||||
}
|
||||
|
||||
static struct {
|
||||
struct arg_dbl *kp;
|
||||
struct arg_dbl *ki;
|
||||
struct arg_dbl *kd;
|
||||
struct arg_end *end;
|
||||
} pid_ctrl_args;
|
||||
|
||||
static int do_pid_ctrl_cmd(int argc, char **argv)
|
||||
{
|
||||
int nerrors = arg_parse(argc, argv, (void **)&pid_ctrl_args);
|
||||
if (nerrors != 0) {
|
||||
arg_print_errors(stderr, pid_ctrl_args.end, argv[0]);
|
||||
return 0;
|
||||
}
|
||||
if (pid_ctrl_args.kp->count) {
|
||||
pid_runtime_param.kp = pid_ctrl_args.kp->dval[0];
|
||||
}
|
||||
if (pid_ctrl_args.ki->count) {
|
||||
pid_runtime_param.ki = pid_ctrl_args.ki->dval[0];
|
||||
}
|
||||
if (pid_ctrl_args.kd->count) {
|
||||
pid_runtime_param.kd = pid_ctrl_args.kd->dval[0];
|
||||
}
|
||||
|
||||
pid_need_update = true;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void register_pid_console_command(void)
|
||||
{
|
||||
pid_ctrl_args.kp = arg_dbl0("p", NULL, "<kp>", "Set Kp value of PID");
|
||||
pid_ctrl_args.ki = arg_dbl0("i", NULL, "<ki>", "Set Ki value of PID");
|
||||
pid_ctrl_args.kd = arg_dbl0("d", NULL, "<kd>", "Set Kd value of PID");
|
||||
pid_ctrl_args.end = arg_end(2);
|
||||
const esp_console_cmd_t pid_ctrl_cmd = {
|
||||
.command = "pid",
|
||||
.help = "Set PID parameters",
|
||||
.hint = NULL,
|
||||
.func = &do_pid_ctrl_cmd,
|
||||
.argtable = &pid_ctrl_args
|
||||
};
|
||||
ESP_ERROR_CHECK(esp_console_cmd_register(&pid_ctrl_cmd));
|
||||
// set the new speed
|
||||
pid_compute(pid_ctrl, error, &new_speed);
|
||||
bdc_motor_set_speed(motor, (uint32_t)new_speed);
|
||||
}
|
||||
|
||||
void app_main(void)
|
||||
{
|
||||
static motor_control_timer_context_t my_timer_ctx = {};
|
||||
|
||||
QueueHandle_t pid_fb_queue = xQueueCreate(BDC_PID_FEEDBACK_QUEUE_LEN, sizeof(int));
|
||||
assert(pid_fb_queue);
|
||||
|
||||
printf("configure mcpwm gpio\r\n");
|
||||
ESP_ERROR_CHECK(mcpwm_gpio_init(BDC_MCPWM_UNIT, MCPWM0A, BDC_MCPWM_GENA_GPIO_NUM));
|
||||
ESP_ERROR_CHECK(mcpwm_gpio_init(BDC_MCPWM_UNIT, MCPWM0B, BDC_MCPWM_GENB_GPIO_NUM));
|
||||
printf("init mcpwm driver\n");
|
||||
mcpwm_config_t pwm_config = {
|
||||
.frequency = BDC_MCPWM_FREQ_HZ,
|
||||
.cmpr_a = 0,
|
||||
.cmpr_b = 0,
|
||||
.counter_mode = MCPWM_UP_COUNTER,
|
||||
.duty_mode = MCPWM_DUTY_MODE_0,
|
||||
static motor_control_context_t motor_ctrl_ctx = {
|
||||
.accumu_count = 0,
|
||||
.pcnt_encoder = NULL,
|
||||
};
|
||||
ESP_ERROR_CHECK(mcpwm_init(BDC_MCPWM_UNIT, BDC_MCPWM_TIMER, &pwm_config));
|
||||
|
||||
printf("init and start rotary encoder\r\n");
|
||||
ESP_LOGI(TAG, "Create DC motor");
|
||||
bdc_motor_config_t motor_config = {
|
||||
.pwm_freq_hz = BDC_MCPWM_FREQ_HZ,
|
||||
.pwma_gpio_num = BDC_MCPWM_GPIO_A,
|
||||
.pwmb_gpio_num = BDC_MCPWM_GPIO_B,
|
||||
};
|
||||
bdc_motor_mcpwm_config_t mcpwm_config = {
|
||||
.group_id = 0,
|
||||
.resolution_hz = BDC_MCPWM_TIMER_RESOLUTION_HZ,
|
||||
};
|
||||
bdc_motor_handle_t motor = NULL;
|
||||
ESP_ERROR_CHECK(bdc_motor_new_mcpwm_device(&motor_config, &mcpwm_config, &motor));
|
||||
motor_ctrl_ctx.motor = motor;
|
||||
|
||||
ESP_LOGI(TAG, "Init pcnt driver to decode rotary signal");
|
||||
pcnt_unit_config_t unit_config = {
|
||||
.high_limit = BDC_ENCODER_PCNT_HIGH_LIMIT,
|
||||
.low_limit = BDC_ENCODER_PCNT_LOW_LIMIT,
|
||||
@ -186,14 +107,14 @@ void app_main(void)
|
||||
};
|
||||
ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(pcnt_unit, &filter_config));
|
||||
pcnt_chan_config_t chan_a_config = {
|
||||
.edge_gpio_num = BDC_ENCODER_PHASEA_GPIO_NUM,
|
||||
.level_gpio_num = BDC_ENCODER_PHASEB_GPIO_NUM,
|
||||
.edge_gpio_num = BDC_ENCODER_GPIO_A,
|
||||
.level_gpio_num = BDC_ENCODER_GPIO_B,
|
||||
};
|
||||
pcnt_channel_handle_t pcnt_chan_a = NULL;
|
||||
ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_a_config, &pcnt_chan_a));
|
||||
pcnt_chan_config_t chan_b_config = {
|
||||
.edge_gpio_num = BDC_ENCODER_PHASEB_GPIO_NUM,
|
||||
.level_gpio_num = BDC_ENCODER_PHASEA_GPIO_NUM,
|
||||
.edge_gpio_num = BDC_ENCODER_GPIO_B,
|
||||
.level_gpio_num = BDC_ENCODER_GPIO_A,
|
||||
};
|
||||
pcnt_channel_handle_t pcnt_chan_b = NULL;
|
||||
ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &chan_b_config, &pcnt_chan_b));
|
||||
@ -204,70 +125,55 @@ void app_main(void)
|
||||
ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, BDC_ENCODER_PCNT_HIGH_LIMIT));
|
||||
ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, BDC_ENCODER_PCNT_LOW_LIMIT));
|
||||
pcnt_event_callbacks_t pcnt_cbs = {
|
||||
.on_reach = example_pcnt_on_reach,
|
||||
.on_reach = example_pcnt_on_reach, // accumulate the overflow in the callback
|
||||
};
|
||||
ESP_ERROR_CHECK(pcnt_unit_register_event_callbacks(pcnt_unit, &pcnt_cbs, &my_timer_ctx));
|
||||
ESP_ERROR_CHECK(pcnt_unit_register_event_callbacks(pcnt_unit, &pcnt_cbs, &motor_ctrl_ctx.accumu_count));
|
||||
ESP_ERROR_CHECK(pcnt_unit_enable(pcnt_unit));
|
||||
ESP_ERROR_CHECK(pcnt_unit_clear_count(pcnt_unit));
|
||||
ESP_ERROR_CHECK(pcnt_unit_start(pcnt_unit));
|
||||
motor_ctrl_ctx.pcnt_encoder = pcnt_unit;
|
||||
|
||||
printf("init PID control block\r\n");
|
||||
pid_ctrl_block_handle_t pid_ctrl;
|
||||
ESP_LOGI(TAG, "Create PID control block");
|
||||
pid_ctrl_parameter_t pid_runtime_param = {
|
||||
.kp = 0.6,
|
||||
.ki = 0.4,
|
||||
.kd = 0.2,
|
||||
.cal_type = PID_CAL_TYPE_INCREMENTAL,
|
||||
.max_output = BDC_MCPWM_DUTY_TICK_MAX - 1,
|
||||
.min_output = 0,
|
||||
.max_integral = 1000,
|
||||
.min_integral = -1000,
|
||||
};
|
||||
pid_ctrl_block_handle_t pid_ctrl = NULL;
|
||||
pid_ctrl_config_t pid_config = {
|
||||
.init_param = pid_runtime_param,
|
||||
};
|
||||
ESP_ERROR_CHECK(pid_new_control_block(&pid_config, &pid_ctrl));
|
||||
motor_ctrl_ctx.pid_ctrl = pid_ctrl;
|
||||
|
||||
printf("init motor control timer\r\n");
|
||||
gptimer_handle_t gptimer;
|
||||
gptimer_config_t timer_config = {
|
||||
.clk_src = GPTIMER_CLK_SRC_DEFAULT,
|
||||
.direction = GPTIMER_COUNT_UP,
|
||||
.resolution_hz = 1000000, // 1MHz, 1 tick = 1us
|
||||
ESP_LOGI(TAG, "Create a timer to do PID calculation periodically");
|
||||
const esp_timer_create_args_t periodic_timer_args = {
|
||||
.callback = pid_loop_cb,
|
||||
.arg = &motor_ctrl_ctx,
|
||||
.name = "pid_loop"
|
||||
};
|
||||
ESP_ERROR_CHECK(gptimer_new_timer(&timer_config, &gptimer));
|
||||
esp_timer_handle_t pid_loop_timer = NULL;
|
||||
ESP_ERROR_CHECK(esp_timer_create(&periodic_timer_args, &pid_loop_timer));
|
||||
|
||||
printf("create motor control task\r\n");
|
||||
static motor_control_task_context_t my_ctrl_task_ctx = {};
|
||||
my_ctrl_task_ctx.pid_feedback_queue = pid_fb_queue;
|
||||
my_ctrl_task_ctx.pid_ctrl = pid_ctrl;
|
||||
xTaskCreate(bdc_ctrl_task, "bdc_ctrl_task", 4096, &my_ctrl_task_ctx, 5, NULL);
|
||||
ESP_LOGI(TAG, "Enable motor");
|
||||
ESP_ERROR_CHECK(bdc_motor_enable(motor));
|
||||
ESP_LOGI(TAG, "Forward motor");
|
||||
ESP_ERROR_CHECK(bdc_motor_forward(motor));
|
||||
|
||||
printf("start motor control timer\r\n");
|
||||
my_timer_ctx.pid_feedback_queue = pid_fb_queue;
|
||||
my_timer_ctx.hall_pcnt_encoder = pcnt_unit;
|
||||
gptimer_event_callbacks_t gptimer_cbs = {
|
||||
.on_alarm = motor_ctrl_timer_cb,
|
||||
};
|
||||
ESP_ERROR_CHECK(gptimer_register_event_callbacks(gptimer, &gptimer_cbs, &my_timer_ctx));
|
||||
gptimer_alarm_config_t alarm_config = {
|
||||
.reload_count = 0,
|
||||
.alarm_count = BDC_PID_CALCULATION_PERIOD_US,
|
||||
.flags.auto_reload_on_alarm = true,
|
||||
};
|
||||
ESP_ERROR_CHECK(gptimer_set_alarm_action(gptimer, &alarm_config));
|
||||
ESP_ERROR_CHECK(gptimer_enable(gptimer));
|
||||
ESP_ERROR_CHECK(gptimer_start(gptimer));
|
||||
|
||||
printf("install console command line\r\n");
|
||||
esp_console_repl_t *repl = NULL;
|
||||
esp_console_repl_config_t repl_config = ESP_CONSOLE_REPL_CONFIG_DEFAULT();
|
||||
repl_config.prompt = "dc-motor>";
|
||||
esp_console_dev_uart_config_t uart_config = ESP_CONSOLE_DEV_UART_CONFIG_DEFAULT();
|
||||
ESP_ERROR_CHECK(esp_console_new_repl_uart(&uart_config, &repl_config, &repl));
|
||||
register_pid_console_command();
|
||||
ESP_ERROR_CHECK(esp_console_start_repl(repl));
|
||||
ESP_LOGI(TAG, "Start motor speed loop");
|
||||
ESP_ERROR_CHECK(esp_timer_start_periodic(pid_loop_timer, BDC_PID_LOOP_PERIOD_MS * 1000));
|
||||
|
||||
while (1) {
|
||||
vTaskDelay(pdMS_TO_TICKS(100));
|
||||
// the following logging format is according to the requirement of serial-studio
|
||||
// also see the parser mapping file `serial-studio-proto-map.json` in the project folder
|
||||
// the following logging format is according to the requirement of serial-studio frame format
|
||||
// also see the dashboard config file `serial-studio-dashboard.json` for more information
|
||||
#if SERIAL_STUDIO_DEBUG
|
||||
printf("/*%d*/\r\n", real_pulses);
|
||||
printf("/*%d*/\r\n", motor_ctrl_ctx.report_pulses);
|
||||
#endif
|
||||
if (pid_need_update) {
|
||||
pid_update_parameters(pid_ctrl, &pid_runtime_param);
|
||||
pid_need_update = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -0,0 +1,17 @@
|
||||
# SPDX-FileCopyrightText: 2021-2022 Espressif Systems (Shanghai) CO LTD
|
||||
# SPDX-License-Identifier: CC0-1.0
|
||||
|
||||
import pytest
|
||||
from pytest_embedded import Dut
|
||||
|
||||
|
||||
@pytest.mark.esp32s3
|
||||
@pytest.mark.generic
|
||||
def test_bdc_speed_control_example(dut: Dut) -> None:
|
||||
dut.expect_exact('example: Create DC motor')
|
||||
dut.expect_exact('example: Init pcnt driver to decode rotary signal')
|
||||
dut.expect_exact('example: Create PID control block')
|
||||
dut.expect_exact('example: Create a timer to do PID calculation periodically')
|
||||
dut.expect_exact('example: Enable motor')
|
||||
dut.expect_exact('example: Forward motor')
|
||||
dut.expect_exact('example: Start motor speed loop')
|
||||
@ -0,0 +1,28 @@
|
||||
{
|
||||
"frameEnd": "*/",
|
||||
"frameStart": "/*",
|
||||
"groups": [
|
||||
{
|
||||
"datasets": [
|
||||
{
|
||||
"alarm": 1000,
|
||||
"fft": false,
|
||||
"fftSamples": 1024,
|
||||
"graph": true,
|
||||
"led": false,
|
||||
"log": false,
|
||||
"max": 500,
|
||||
"min": 0,
|
||||
"title": "current speed",
|
||||
"units": "",
|
||||
"value": "%1",
|
||||
"widget": "gauge"
|
||||
}
|
||||
],
|
||||
"title": "speed",
|
||||
"widget": ""
|
||||
}
|
||||
],
|
||||
"separator": ",",
|
||||
"title": "Brushed DC Motor Speed"
|
||||
}
|
||||
@ -1,22 +0,0 @@
|
||||
{
|
||||
"fe": "*/",
|
||||
"fs": "/*",
|
||||
"g": [
|
||||
{
|
||||
"d": [
|
||||
{
|
||||
"g": true,
|
||||
"max": 100,
|
||||
"min": 0,
|
||||
"t": "pulses within 10ms",
|
||||
"u": "",
|
||||
"v": "%1",
|
||||
"w": ""
|
||||
}
|
||||
],
|
||||
"t": "Encoder Feedback",
|
||||
}
|
||||
],
|
||||
"s": ",",
|
||||
"t": "Brushed DC Motor Speed"
|
||||
}
|
||||
@ -1773,9 +1773,6 @@ examples/peripherals/i2c/i2c_tools/main/cmd_i2ctools.h
|
||||
examples/peripherals/i2c/i2c_tools/main/i2ctools_example_main.c
|
||||
examples/peripherals/ledc/ledc_basic/main/ledc_basic_example_main.c
|
||||
examples/peripherals/ledc/ledc_fade/main/ledc_fade_example_main.c
|
||||
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/cmd_mcpwm_motor.c
|
||||
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.c
|
||||
examples/peripherals/mcpwm/mcpwm_brushed_dc_control/main/mcpwm_brushed_dc_control_example.h
|
||||
examples/peripherals/mcpwm/mcpwm_servo_control/main/mcpwm_servo_control_example_main.c
|
||||
examples/peripherals/sdio/host/main/app_main.c
|
||||
examples/peripherals/sdio/sdio_test.py
|
||||
|
||||
Loading…
Reference in New Issue
Block a user