alex/esp-idf
alex/esp-idf
1
0
Fork 0
mirror of https://github.com/espressif/esp-idf.git synced 2024-10-05 20:47:46 -04:00
Code Issues Actions 2 Packages Projects Releases Wiki Activity

RMT: new example step motor, targeting at esp32s3

Browse Source 
also working on c3 and s2 but may not reliable under loads
This commit is contained in:
SalimTerryLi 2021-09-23 11:12:05 +08:00
parent bd89dcc683
commit 075b091696
No known key found for this signature in database
GPG Key ID: F05CCEF2191AF770
13 changed files with 1003 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/peripherals/rmt/step_motor/CMakeLists.txt Normal file
View File

@ -0,0 +1,6 @@
# The following 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.5)
include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(step_motor)

8
examples/peripherals/rmt/step_motor/Makefile Normal file
View File

@ -0,0 +1,8 @@
#
# This is a project Makefile. It is assumed the directory this Makefile resides in is a
# project subdirectory.
#
PROJECT_NAME := step_motor
include $(IDF_PATH)/make/project.mk

92
examples/peripherals/rmt/step_motor/README.md Normal file
View File

@ -0,0 +1,92 @@
| Supported Targets | ESP32-S2 | ESP32-C3 | ESP32-S3 |
| ----------------- | -------- | -------- | -------- |
# RMT Transmit Loop Example -- Step Motor controller
(See the README.md file in the upper level 'examples' directory for more information about examples.)
RMT peripheral can send customized RMT items in a loop, which means we can use it to generate a configurable length of periodic signal, with accurate number of pulses.
This example will show how to control an A4988 based step motor driver to step accurately with simple APIs, based on the RMT loop feature. The example also implements a [Smoothstep](https://en.wikipedia.org/wiki/Smoothstep) feature which works out of the box.
## How to Use Example
### Hardware Required
* Recommend running this example on development board with SOC chip that support loop auto-stop feature by hardware (e.g. ESP32-S3)
* A USB cable for Power supply and programming
* A 4-wire (A+, A-, B+, B-) step motor
* An A4988 module
Connection :
```
+----------------+ +--------------------+ +--------------+
| | | A4988 | | 4-wire |
| GND +-------------+ GND | | Step |
| | | | | Motor |
| 5V +-------------+ VDD 1B +------+ A2 |
| | | | | |
| GPIO18 +------------>+ DIRECTION 1A +------+ A1 |
| | | | | |
| ESP GPIO17 +------------>+ STEP 2A +------+ B1 |
| | | | | |
| GPIO16 +------------>+ SLEEP 2B +------+ B2 |
| | | | +--------------+
| GPIO15 +------------>+ RESET VMOT +-------------------+
| | | | |
| GPIO7 +------------>+ MS3 GND +----------+ |
| | | | | |
| GPIO6 +------------>+ MS2 | | |
| | | | | |
| GPIO5 +------------>+ MS1 | +---+--------+-----+
| | | | | GND +12V |
| GPIO4 +------------>+ ENABLE | | POWER SUPPLY |
+----------------+ +--------------------+ +------------------+
```
IO mapping on ESP side can be changed in `step_motor_main.c`:
```c
// GPIO configuration
#define STEP_MOTOR_DIRECTION_PIN GPIO_NUM_18
#define STEP_MOTOR_STEP_PIN GPIO_NUM_17
#define STEP_MOTOR_SLEEP_PIN GPIO_NUM_16
#define STEP_MOTOR_RESET_PIN GPIO_NUM_15
#define STEP_MOTOR_MS3_PIN GPIO_NUM_7
#define STEP_MOTOR_MS2_PIN GPIO_NUM_6
#define STEP_MOTOR_MS1_PIN GPIO_NUM_5
#define STEP_MOTOR_ENABLE_PIN GPIO_NUM_4
```
### Build and Flash
Run `idf.py -p PORT flash monitor` to build, flash and monitor the project.
(To exit the serial monitor, type ``Ctrl-]``.)
See the [Getting Started Guide](https://docs.espressif.com/projects/esp-idf/en/latest/get-started/index.html) for full steps to configure and use ESP-IDF to build projects.
## Example Output
```
I (344) step_motor: init
I (344) step_motor: set_step
I (1354) step_motor: step 10 @ 1000/s
I (2364) step_motor: step 100 @ 1000/s
I (3464) step_motor: step 1000 @ 1200/s
I (5294) step_motor: step 5000 @ 1400/s
I (9864) step_motor: smoothstep start 5000 steps @ 500~1400/s
I (14454) step_motor: smoothstep finish
I (15454) step_motor: continuous running for 5s
I (20454) step_motor: stop
I (21504) step_motor: deinit
```
Motor should move as output indicates.
## Troubleshooting
For any technical queries, please open an [issue] (https://github.com/espressif/esp-idf/issues) on GitHub. We will get back to you soon.

10
examples/peripherals/rmt/step_motor/components/step_motor/CMakeLists.txt Normal file
View File

@ -0,0 +1,10 @@
set(component_srcs "src/step_motor.c"
"src/step_motor_rmt.c"
"src/step_motor_driver_io_a4988.c"
)
idf_component_register(SRCS "${component_srcs}"
INCLUDE_DIRS "include"
PRIV_INCLUDE_DIRS ""
PRIV_REQUIRES "driver"
REQUIRES "")

134
examples/peripherals/rmt/step_motor/components/step_motor/include/step_motor.h Normal file
View File

@ -0,0 +1,134 @@
/*
* SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: CC0-1.0
*/
#pragma once
#include <stdbool.h>
#include "driver/rmt.h"
#include "hal/rmt_types.h"
#include "esp_err.h"
#include "step_motor_driver_io.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief Type of step motor interface
*/
typedef struct step_motor_s step_motor_t;
typedef step_motor_t *step_motor_handle_t;
/**
* @brief Declaration of step motor interface
*
*/
struct step_motor_s {
esp_err_t (*init)(step_motor_t *handle);
esp_err_t (*deinit)(step_motor_t *handle);
esp_err_t (*step)(step_motor_t *handle, uint32_t n, uint32_t speed);
esp_err_t (*smooth_step)(step_motor_t *handle, uint32_t n, uint32_t speed_steps, uint32_t speed_min, uint32_t speed_max);
esp_err_t (*set_step)(step_motor_t *handle, uint16_t microstep, bool direction);
// TODO: other API like sleep, enable_output, reset
};
/**
* @brief Initialize step motor driver
*
* @param handle driver handle
* @return
* - ESP_OK: successfully initialized
* - ESP_ERR_INVALID_ARG: wrong parameter
*/
esp_err_t step_motor_init(step_motor_t *handle);
/**
* @brief Deinitialize driver
*
* @param handle driver handle
* @return
* - ESP_OK: Stop playing successfully
*/
esp_err_t step_motor_deinit(step_motor_t *handle);
/**
* @brief Move n small steps.
*
* @note Will block until finish if n is finite steps. But will immediately return if n is UINT32_MAX.
*
* @param handle driver handle
* @param n step count, UINT32_MAX for unlimited, 0 to stop
* @param speed steps per second
* @return
* - ESP_OK: Recycle memory successfully
*/
esp_err_t step_motor_step(step_motor_t *handle, uint32_t n, uint32_t speed);
/**
* @brief Move n small steps. Always blocking and take smooth arguments
*
* ^ speed (steps/s)
* | ********************* <---- speed_max
* | * | | *
* | * | | *
* | * | | *
* | * | | *
* | * speed | n-speed_steps*2 | speed *
* | * steps | | steps * <---- speed_min
* | | |
* +-------------------------------------------------------------------> timestamp (s)
*
* @param handle driver handle
* @param n steps
* @param speed_steps number of sample points during speed smoothing
* @param speed_min minimal speed, steps per seconds
* @param speed_max maximum speed, steps per seconds
* @note may consume lots of ram depending on speed_steps with current implementation (1000 will lead to 8kb of ram usage)
* @return
* - ESP_OK: Recycle memory successfully
*/
esp_err_t step_motor_smooth_step(step_motor_t *handle, uint32_t n, uint32_t speed_steps, uint32_t speed_min, uint32_t speed_max);
/**
* @brief Set microstep resolution
*
* @param handle driver handle
* @param step_config microstep resolution
* @param direction rotating direction
* @return
* - ESP_OK: Recycle memory successfully
*/
esp_err_t step_motor_set_step(step_motor_t *handle, uint16_t microstep, bool direction);
// TODO: move out of this header to rmt one (like step_motor_rmt.h)
/**
* @brief Create step motor instance based on RMT driver
*
* @param[in] io_driver step motor low part driver
* @param[out] ret_handle returned handle of step motor instance
* @return
* - ESP_OK: create step motor instance successfully
* - ESP_ERR_INVALID_ARG: wrong parameter
* - ESP_ERR_NO_MEM: no memory to allocate instance
*/
esp_err_t step_motor_create_rmt(step_motor_driver_io_t *io_driver, const rmt_config_t *rmt_conf, step_motor_handle_t *ret_handle);
/**
* @brief Delete step motor instance that previously created
*
* @param[in] handle step motor instance to be deleted
* @return
* - ESP_OK: create step motor instance successfully
* - ESP_ERR_INVALID_ARG: wrong parameter
*/
esp_err_t step_motor_delete_rmt(step_motor_handle_t handle);
#ifdef __cplusplus
}
#endif

75
examples/peripherals/rmt/step_motor/components/step_motor/include/step_motor_driver_io.h Normal file
View File

@ -0,0 +1,75 @@
/*
* SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: CC0-1.0
*/
#pragma once
#include "esp_err.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef struct step_motor_driver_io_s step_motor_driver_io_t;
typedef step_motor_driver_io_t *step_motor_driver_io_handle_t;
typedef enum {
STEP_MOTOR_DIRECTION_NEGATIVE = 0, STEP_MOTOR_DIRECTION_POSITIVE
} step_direction;
/**
* @brief init low part of driver
* GPIO configuration, Bus initializing...
*/
typedef esp_err_t (*step_motor_driver_io_init)(step_motor_driver_io_t *handle);
/**
* @brief set rotation direction
*/
typedef esp_err_t (*step_motor_driver_io_set_direction)(step_motor_driver_io_t *handle, step_direction direction);
/**
* @brief enable/disable sleep mode if supported
*/
typedef esp_err_t (*step_motor_driver_io_enable_sleep)(step_motor_driver_io_t *handle, bool enabled);
/**
* @brief enable/disable output if supported
*/
typedef esp_err_t (*step_motor_driver_io_enable_output)(step_motor_driver_io_t *handle, bool enabled);
/**
* @brief set microstep configuration if supported.
* param microstep is treated as denominator. a input of 16 means 1/16 step
* should return ESP_ERR_NOT_SUPPORTED if not supported
*/
typedef esp_err_t (*step_motor_driver_io_set_microstep)(step_motor_driver_io_t *handle, uint16_t microstep);
/**
* @brief reset low part of driver
*/
typedef esp_err_t (*step_motor_driver_io_reset)(step_motor_driver_io_t *handle);
/**
* @brief deinit low part of driver
*/
typedef esp_err_t (*step_motor_driver_io_deinit)(step_motor_driver_io_t *handle);
/**
* @brief Driver IC specified control logic
*
* leave callback pointer NULL if action is not supported
*/
struct step_motor_driver_io_s {
step_motor_driver_io_init init; /*!< callback to init low part driver */
step_motor_driver_io_set_direction set_direction; /*!< callback to set rotate direction */
step_motor_driver_io_enable_sleep enable_sleep; /*!< callback to enable sleep mode */
step_motor_driver_io_enable_output enable_output; /*!< callback to enable output */
step_motor_driver_io_set_microstep set_microstep; /*!< callback to set microstep configuration */
bool step_triggered_edge; /*!< true if step is triggered by positive edge, otherwise false */
uint32_t pulse_low_period_us; /*!< minimum low level pulse width on step pin */
uint32_t pulse_high_period_us; /*!< minimum high level pulse width on step pin */
step_motor_driver_io_reset trigger_reset; /*!< callback to trigger a reset on low part driver */
step_motor_driver_io_deinit deinit; /*!< callback to deinit low part driver */
};
#ifdef __cplusplus
}
#endif

49
examples/peripherals/rmt/step_motor/components/step_motor/include/step_motor_driver_io_a4988.h Normal file
View File

@ -0,0 +1,49 @@
/*
* SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: CC0-1.0
*/
#pragma once
#include "esp_err.h"
#include "step_motor_driver_io.h"
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief A4988 configuration
*/
typedef struct step_motor_io_a4988_conf_s {
gpio_num_t direction_pin;
gpio_num_t sleep_pin;
gpio_num_t reset_pin;
gpio_num_t ms3_pin;
gpio_num_t ms2_pin;
gpio_num_t ms1_pin;
gpio_num_t enable_pin;
} step_motor_io_a4988_conf_t;
/**
* @brief A4988 low part driver handle
*/
typedef struct step_motor_driver_io_a4988_s {
step_motor_driver_io_t base;
step_motor_io_a4988_conf_t conf;
} step_motor_driver_io_a4988_t;
/**
* @brief create an A4988 driver handle
*/
esp_err_t step_motor_new_a4988_io_driver(const step_motor_io_a4988_conf_t *conf, step_motor_driver_io_handle_t *handle);
/**
* @brief delete an A4988 driver handle
*/
esp_err_t step_motor_delete_a4988_io_driver(step_motor_driver_io_handle_t handle);
#ifdef __cplusplus
}
#endif

32
examples/peripherals/rmt/step_motor/components/step_motor/src/step_motor.c Normal file
View File

@ -0,0 +1,32 @@
/*
* SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: CC0-1.0
*/
#include "step_motor.h"
esp_err_t step_motor_init(step_motor_t *handle)
{
return handle->init(handle);
}
esp_err_t step_motor_deinit(step_motor_t *handle)
{
return handle->deinit(handle);
}
esp_err_t step_motor_step(step_motor_t *handle, uint32_t n, uint32_t speed)
{
return handle->step(handle, n, speed);
}
esp_err_t step_motor_smooth_step(step_motor_t *handle, uint32_t n, uint32_t speed_steps, uint32_t speed_min, uint32_t speed_max)
{
return handle->smooth_step(handle, n, speed_steps, speed_min, speed_max);
}
esp_err_t step_motor_set_step(step_motor_t *handle, uint16_t microstep, bool direction)
{
return handle->set_step(handle, microstep, direction);
}

174
examples/peripherals/rmt/step_motor/components/step_motor/src/step_motor_driver_io_a4988.c Normal file
View File

@ -0,0 +1,174 @@
/*
* SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: CC0-1.0
*/
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <string.h>
#include "hal/gpio_types.h"
#include "driver/gpio.h"
#include "esp_check.h"
#include "step_motor_driver_io_a4988.h"
static const char *TAG = "A4988_IO";
#define A4988_RESPONSE_DELAY_MS 10
static esp_err_t a4988_init(step_motor_driver_io_t *handle)
{
step_motor_driver_io_a4988_t *a4988_motor = __containerof(handle, step_motor_driver_io_a4988_t, base);
gpio_config_t io_conf;
io_conf.intr_type = GPIO_INTR_DISABLE;
io_conf.mode = GPIO_MODE_OUTPUT;
//bit mask of the pins that you want to set,e.g.GPIO18/19
io_conf.pin_bit_mask = BIT64(a4988_motor->conf.direction_pin) |
BIT64(a4988_motor->conf.sleep_pin) |
BIT64(a4988_motor->conf.reset_pin) |
BIT64(a4988_motor->conf.ms3_pin) |
BIT64(a4988_motor->conf.ms2_pin) |
BIT64(a4988_motor->conf.ms1_pin) |
BIT64(a4988_motor->conf.enable_pin);
io_conf.pull_down_en = 0;
io_conf.pull_up_en = 0;
ESP_ERROR_CHECK(gpio_config(&io_conf));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.direction_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.sleep_pin, 0)); // default sleep
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.reset_pin, 0)); // keep reset
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms3_pin, 0)); // 1/1 phase
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms2_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms1_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.enable_pin, 1)); // disable by default
vTaskDelay(pdMS_TO_TICKS(A4988_RESPONSE_DELAY_MS));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.reset_pin, 1));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.sleep_pin, 1));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.enable_pin, 0));
vTaskDelay(pdMS_TO_TICKS(A4988_RESPONSE_DELAY_MS));
return ESP_OK;
}
static esp_err_t a4988_set_direction(step_motor_driver_io_t *handle, step_direction direction)
{
step_motor_driver_io_a4988_t *a4988_motor = __containerof(handle, step_motor_driver_io_a4988_t, base);
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.direction_pin, direction));
return ESP_OK;
}
static esp_err_t a4988_enable_sleep(step_motor_driver_io_t *handle, bool enabled)
{
step_motor_driver_io_a4988_t *a4988_motor = __containerof(handle, step_motor_driver_io_a4988_t, base);
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.sleep_pin, enabled));
return ESP_OK;
}
static esp_err_t a4988_enable_output(step_motor_driver_io_t *handle, bool enabled)
{
step_motor_driver_io_a4988_t *a4988_motor = __containerof(handle, step_motor_driver_io_a4988_t, base);
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.enable_pin, enabled));
return ESP_OK;
}
static esp_err_t a4988_set_microstep(step_motor_driver_io_t *handle, uint16_t microstep)
{
step_motor_driver_io_a4988_t *a4988_motor = __containerof(handle, step_motor_driver_io_a4988_t, base);
switch (microstep) {
case 1:
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms3_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms2_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms1_pin, 0));
break;
case 2:
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms3_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms2_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms1_pin, 1));
break;
case 4:
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms3_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms2_pin, 1));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms1_pin, 0));
break;
case 8:
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms3_pin, 0));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms2_pin, 1));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms1_pin, 1));
break;
case 16:
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms3_pin, 1));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms2_pin, 1));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.ms1_pin, 1));
break;
default:
return ESP_ERR_NOT_SUPPORTED;
}
return ESP_OK;
}
static esp_err_t a4988_reset(step_motor_driver_io_t *handle)
{
step_motor_driver_io_a4988_t *a4988_motor = __containerof(handle, step_motor_driver_io_a4988_t, base);
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.reset_pin, 0));
vTaskDelay(pdMS_TO_TICKS(A4988_RESPONSE_DELAY_MS));
ESP_ERROR_CHECK(gpio_set_level(a4988_motor->conf.reset_pin, 1));
return ESP_OK;
}
static esp_err_t a4988_deinit(step_motor_driver_io_t *handle)
{
step_motor_driver_io_a4988_t *a4988_motor = __containerof(handle, step_motor_driver_io_a4988_t, base);
gpio_config_t io_conf;
io_conf.intr_type = GPIO_INTR_DISABLE;
io_conf.mode = GPIO_MODE_INPUT;
io_conf.pin_bit_mask = BIT64(a4988_motor->conf.direction_pin) |
BIT64(a4988_motor->conf.sleep_pin) |
BIT64(a4988_motor->conf.reset_pin) |
BIT64(a4988_motor->conf.ms3_pin) |
BIT64(a4988_motor->conf.ms2_pin) |
BIT64(a4988_motor->conf.ms1_pin) |
BIT64(a4988_motor->conf.enable_pin);
io_conf.pull_down_en = 0;
io_conf.pull_up_en = 0;
ESP_ERROR_CHECK(gpio_config(&io_conf));
return ESP_OK;
}
esp_err_t step_motor_new_a4988_io_driver(const step_motor_io_a4988_conf_t *conf, step_motor_driver_io_handle_t *handle)
{
esp_err_t ret = ESP_OK;
step_motor_driver_io_a4988_t *a4988 = NULL;
ESP_GOTO_ON_FALSE(conf, ESP_ERR_INVALID_ARG, err, TAG, "configuration can't be null");
ESP_GOTO_ON_FALSE(handle, ESP_ERR_INVALID_ARG, err, TAG, "can't assign handle to null");
a4988 = calloc(1, sizeof(step_motor_driver_io_a4988_t));
ESP_GOTO_ON_FALSE(a4988, ESP_ERR_NO_MEM, err, TAG, "allocate context memory failed");
memcpy(&a4988->conf, conf, sizeof(step_motor_io_a4988_conf_t));
a4988->base.init = a4988_init;
a4988->base.deinit = a4988_deinit;
a4988->base.set_direction = a4988_set_direction;
a4988->base.set_microstep = a4988_set_microstep;
a4988->base.enable_sleep = a4988_enable_sleep;
a4988->base.enable_output = a4988_enable_output;
a4988->base.trigger_reset = a4988_reset;
a4988->base.step_triggered_edge = 1;
a4988->base.pulse_high_period_us = 1;
a4988->base.pulse_low_period_us = 1;
*handle = &(a4988->base);
return ESP_OK;
err:
if (a4988) {
free(a4988);
}
return ret;
}
esp_err_t step_motor_delete_a4988_io_driver(step_motor_driver_io_handle_t handle)
{
ESP_RETURN_ON_FALSE(handle, ESP_ERR_INVALID_STATE, TAG, "empty handle");
step_motor_driver_io_a4988_t *a4988 = __containerof(handle, step_motor_driver_io_a4988_t, base);
free(a4988);
return ESP_OK;
}

325
examples/peripherals/rmt/step_motor/components/step_motor/src/step_motor_rmt.c Normal file
View File

@ -0,0 +1,325 @@
/*
* SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: CC0-1.0
*/
/*
* This file contains an implementation of step motor middleware based on rmt peripheral
*/
#include <math.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "esp_log.h"
#include "esp_check.h"
#include "driver/rmt.h"
#include "step_motor.h"
static const char *TAG = "RMT_STEP_MOTOR";
typedef enum {
STOPPED = 0,
SMOOTH_SPEED_UP,
SMOOTH_KEEP_SPEED,
SMOOTH_SLOW_DOWN,
UNLIMITED_LOOP,
LIMITED_LOOP,
} rmt_step_motor_running_status;
typedef struct {
step_motor_t base;
step_motor_driver_io_t *io_driver;
rmt_channel_t rmt_ch;
rmt_step_motor_running_status status;
rmt_item32_t rmt_items_loop;
uint32_t rmt_items_loop_count;
rmt_item32_t *rmt_items_speedup;
rmt_item32_t *rmt_items_speeddown;
uint32_t rmt_items_smoothstep_count;
SemaphoreHandle_t notify_semphr;
} rmt_step_motor_t;
static inline float helper_smootherstep_clamp(float x, float lowerlimit, float upperlimit)
{
if (x < lowerlimit) {
x = lowerlimit;
}
if (x > upperlimit) {
x = upperlimit;
}
return x;
}
// smoothstep formula
// see https://en.wikipedia.org/wiki/Smoothstep
static float helper_smootherstep(float edge0, float edge1, float x)
{
// Scale, and clamp x to 0..1 range
x = helper_smootherstep_clamp((x - edge0) / (edge1 - edge0), 0.0, 1.0);
// Evaluate polynomial
return x * x * x * (x * (x * 6 - 15) + 10) * (edge1 - edge0) + edge0;
}
static uint16_t helper_speed_to_duration(uint16_t speed)
{
return (uint16_t) round(1.0 * 1000 * 1000 / speed);
}
static esp_err_t helper_fill_rmt_items(rmt_item32_t *items, uint32_t speed, const step_motor_driver_io_t *io_driver)
{
items->duration1 = io_driver->step_triggered_edge ? io_driver->pulse_high_period_us : io_driver->pulse_low_period_us;
items->level1 = io_driver->step_triggered_edge;
items->level0 = !io_driver->step_triggered_edge;
uint32_t delay_period = helper_speed_to_duration(speed);
if (delay_period <= (io_driver->step_triggered_edge ? io_driver->pulse_low_period_us : io_driver->pulse_high_period_us)) {
ESP_LOGW(TAG, "maximum rate reached, driver will generate another possible highest rate instead");
items->duration0 = io_driver->step_triggered_edge ? io_driver->pulse_low_period_us : io_driver->pulse_high_period_us;
} else {
items->duration0 = delay_period;
}
return ESP_OK;
}
static esp_err_t rmt_step_motor_init(step_motor_t *motor)
{
rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
if (io_driver->init) {
return io_driver->init(io_driver);
}
return ESP_OK;
}
static esp_err_t rmt_step_motor_deinit(step_motor_t *motor)
{
rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
if (io_driver->deinit) {
return io_driver->deinit(io_driver);
}
return ESP_OK;
}
// assume n != 0 and speed is within considerable range
static esp_err_t rmt_step_motor_step_impl(step_motor_t *motor, uint32_t n, uint32_t speed)
{
rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, true));
ESP_ERROR_CHECK(rmt_enable_tx_loop_autostop(rmt_handle->rmt_ch, true));
rmt_handle->rmt_items_loop_count = n;
if ((rmt_handle->rmt_items_loop_count) > 1023) {
(rmt_handle->rmt_items_loop_count) -= 1023;
ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, 1023));
} else {
ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, rmt_handle->rmt_items_loop_count));
rmt_handle->rmt_items_loop_count = 0;
}
helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed, rmt_handle->io_driver);
rmt_handle->status = LIMITED_LOOP;
rmt_write_items(rmt_handle->rmt_ch, &rmt_handle->rmt_items_loop, 1, false);
xSemaphoreTake(rmt_handle->notify_semphr, portMAX_DELAY);
return ESP_OK;
}
static esp_err_t rmt_step_motor_step(step_motor_t *handle, uint32_t n, uint32_t speed)
{
rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
ESP_ERROR_CHECK(rmt_tx_stop(rmt_handle->rmt_ch));
if (n == UINT32_MAX) { // forever loop, non-blocking
ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, 0));
ESP_ERROR_CHECK(rmt_enable_tx_loop_autostop(rmt_handle->rmt_ch, false));
ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, true));
helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed, rmt_handle->io_driver);
rmt_handle->status = UNLIMITED_LOOP;
ESP_ERROR_CHECK(rmt_write_items(rmt_handle->rmt_ch, &rmt_handle->rmt_items_loop, 1, false));
return ESP_OK;
} else if (n == 0) { // break the forever loop
rmt_handle->status = STOPPED;
ESP_ERROR_CHECK(rmt_tx_stop(rmt_handle->rmt_ch));
ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, false));
return ESP_OK;
} else { // normally move n steps
ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed) > 1, ESP_ERR_INVALID_ARG, TAG,
"speed too fast");
return rmt_step_motor_step_impl(handle, n, speed);
}
}
static esp_err_t rmt_step_motor_smoothstep(step_motor_t *handle, uint32_t n, uint32_t speed_steps, uint32_t speed_min,
uint32_t speed_max)
{
esp_err_t ret = ESP_OK;
ESP_RETURN_ON_FALSE(speed_min <= speed_max, ESP_ERR_INVALID_ARG, TAG, "max speed lower than min speed");
ESP_RETURN_ON_FALSE(n > speed_steps * 2, ESP_ERR_INVALID_ARG, TAG, "too few steps. consider lower speed_steps");
ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed_min) < 1 << 15, ESP_ERR_INVALID_ARG, TAG, "min speed too low");
ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed_max) > 1, ESP_ERR_INVALID_ARG, TAG, "max speed too high");
rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
rmt_handle->rmt_items_speedup = malloc(sizeof(rmt_item32_t) * speed_steps);
ESP_RETURN_ON_FALSE(rmt_handle->rmt_items_speedup != NULL, ESP_ERR_NO_MEM, TAG,
"failed to allocate rmt_items_speedup");
rmt_handle->rmt_items_speeddown = malloc(sizeof(rmt_item32_t) * speed_steps);
ESP_GOTO_ON_FALSE(rmt_handle->rmt_items_speeddown != NULL, ESP_ERR_NO_MEM, err_free_speedup, TAG,
"failed to allocate rmt_items_speeddown");
ESP_GOTO_ON_ERROR(rmt_tx_stop(rmt_handle->rmt_ch), err_free_speeddown, TAG, "failed to stop rmt tx");
// prepare speed tables
for (int i = 0; i < speed_steps; ++i) {
helper_fill_rmt_items(&rmt_handle->rmt_items_speedup[i],
(uint16_t)helper_smootherstep(
(float)speed_min,
(float)speed_max,
(float)speed_min + ( (float)i / (float)speed_steps) * (float)(speed_max - speed_min))
, rmt_handle->io_driver
);
}
for (int i = 0; i < speed_steps; ++i) {
helper_fill_rmt_items(&rmt_handle->rmt_items_speeddown[i],
speed_max + speed_min - (uint16_t)helper_smootherstep(
(float)speed_min,
(float)speed_max,
(float)speed_min + ((float) i / (float)speed_steps) * (float)(speed_max - speed_min)
)
, rmt_handle->io_driver
);
}
rmt_handle->rmt_items_smoothstep_count = speed_steps;
// prepare continuous phase rmt payload
helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed_max, rmt_handle->io_driver);
rmt_handle->rmt_items_loop_count = n - speed_steps * 2;
// set status to be checked inside ISR
rmt_handle->status = SMOOTH_SPEED_UP;
// start transmitting
ESP_ERROR_CHECK(rmt_write_items(rmt_handle->rmt_ch, rmt_handle->rmt_items_speedup, speed_steps, false));
// waiting for transfer done
xSemaphoreTake(rmt_handle->notify_semphr, portMAX_DELAY);
err_free_speeddown:
free(rmt_handle->rmt_items_speeddown);
err_free_speedup:
free(rmt_handle->rmt_items_speedup);
return ret;
}
static esp_err_t rmt_step_motor_set_step(step_motor_t *handle, uint16_t microstep, bool direction)
{
rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
if (io_driver->set_direction) {
ESP_ERROR_CHECK(io_driver->set_direction(io_driver, direction));
}
if (io_driver->set_microstep) {
ESP_ERROR_CHECK(io_driver->set_microstep(io_driver, microstep));
}
// at least 200ns delay as described in datasheet
esp_rom_delay_us(1);
return ESP_OK;
}
static IRAM_ATTR void rmt_tx_loop_intr(rmt_channel_t channel, void *args)
{
rmt_step_motor_t *rmt_step_motor = (rmt_step_motor_t *) args;
// smoothstep speedup stage finished
if (rmt_step_motor->status == SMOOTH_SPEED_UP) {
rmt_step_motor->status = SMOOTH_KEEP_SPEED;
rmt_set_tx_loop_mode(rmt_step_motor->rmt_ch, true);
rmt_enable_tx_loop_autostop(rmt_step_motor->rmt_ch, true);
rmt_set_tx_intr_en(rmt_step_motor->rmt_ch, 0);
// continue and configure loop count
}
if (rmt_step_motor->status == SMOOTH_KEEP_SPEED || rmt_step_motor->status == LIMITED_LOOP) {
// loop count not 0, continuing looping
if ((rmt_step_motor->rmt_items_loop_count) != 0) {
if ((rmt_step_motor->rmt_items_loop_count) > 1023) {
(rmt_step_motor->rmt_items_loop_count) -= 1023;
rmt_set_tx_loop_count(rmt_step_motor->rmt_ch, 1023);
} else {
rmt_set_tx_loop_count(rmt_step_motor->rmt_ch, rmt_step_motor->rmt_items_loop_count);
rmt_step_motor->rmt_items_loop_count = 0;
}
rmt_write_items(rmt_step_motor->rmt_ch, &rmt_step_motor->rmt_items_loop, 1, false);
return;
}
}
// smoothstep keep speed stage finished
if (rmt_step_motor->status == SMOOTH_KEEP_SPEED) {
rmt_step_motor->status = SMOOTH_SLOW_DOWN;
rmt_set_tx_loop_mode(rmt_step_motor->rmt_ch, false);
rmt_enable_tx_loop_autostop(rmt_step_motor->rmt_ch, false);
rmt_set_tx_intr_en(rmt_step_motor->rmt_ch, 1);
rmt_write_items(rmt_step_motor->rmt_ch, rmt_step_motor->rmt_items_speeddown, rmt_step_motor->rmt_items_smoothstep_count, false);
return;
}
if (rmt_step_motor->status == LIMITED_LOOP || rmt_step_motor->status == SMOOTH_SLOW_DOWN) {
rmt_step_motor->status = STOPPED;
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xSemaphoreGiveFromISR(rmt_step_motor->notify_semphr, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken == pdTRUE) {
portYIELD_FROM_ISR();
}
}
}
esp_err_t step_motor_create_rmt(step_motor_driver_io_t *io_driver, const rmt_config_t *rmt_conf, step_motor_handle_t *ret_handle)
{
esp_err_t ret = ESP_OK;
rmt_step_motor_t *rmt_step_motor = NULL;
ESP_RETURN_ON_ERROR(rmt_config(rmt_conf), TAG, "Failed to configure RMT");
ESP_RETURN_ON_ERROR(rmt_driver_install(rmt_conf->channel, 0, 0), TAG, "Failed to install RMT driver");
ESP_GOTO_ON_FALSE(io_driver, ESP_ERR_INVALID_ARG, err, TAG, "configuration can't be null");
ESP_GOTO_ON_FALSE(ret_handle, ESP_ERR_INVALID_ARG, err, TAG, "can't assign handle to null");
rmt_step_motor = calloc(1, sizeof(rmt_step_motor_t));
ESP_GOTO_ON_FALSE(rmt_step_motor, ESP_ERR_NO_MEM, err, TAG, "allocate context memory failed");
rmt_step_motor->rmt_ch = rmt_conf->channel;
rmt_step_motor->notify_semphr = xSemaphoreCreateBinary();
ESP_GOTO_ON_FALSE(rmt_step_motor, ESP_ERR_NO_MEM, err, TAG, "allocate semaphore memory failed");
rmt_step_motor->io_driver = io_driver;
// register tx end callback function, which got invoked when tx loop comes to the end
rmt_register_tx_end_callback(rmt_tx_loop_intr, rmt_step_motor);
rmt_step_motor->base.init = rmt_step_motor_init;
rmt_step_motor->base.deinit = rmt_step_motor_deinit;
rmt_step_motor->base.step = rmt_step_motor_step;
rmt_step_motor->base.set_step = rmt_step_motor_set_step;
rmt_step_motor->base.smooth_step = rmt_step_motor_smoothstep;
*ret_handle = &(rmt_step_motor->base);
return ESP_OK;
err:
if (rmt_step_motor) {
if (rmt_step_motor->notify_semphr) {
vSemaphoreDelete(rmt_step_motor->notify_semphr);
}
free(rmt_step_motor);
}
return ret;
}
esp_err_t step_motor_delete_rmt(step_motor_handle_t handle)
{
ESP_RETURN_ON_FALSE(handle, ESP_ERR_INVALID_STATE, TAG, "empty handle");
rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
ESP_RETURN_ON_ERROR(rmt_driver_uninstall(rmt_handle->rmt_ch), TAG, "Failed to uninstall RMT driver");
vSemaphoreDelete(rmt_handle->notify_semphr);
free(rmt_handle);
return ESP_OK;
}

2
examples/peripherals/rmt/step_motor/main/CMakeLists.txt Normal file
View File

@ -0,0 +1,2 @@
idf_component_register(SRCS "step_motor_main.c"
INCLUDE_DIRS ".")

4
examples/peripherals/rmt/step_motor/main/component.mk Normal file
View File

@ -0,0 +1,4 @@
#
# Main Makefile. This is basically the same as a component makefile.
#
# (Uses default behaviour of compiling all source files in directory, adding 'include' to include path.)

92
examples/peripherals/rmt/step_motor/main/step_motor_main.c Normal file
View File

@ -0,0 +1,92 @@
/* RMT example -- step motor */
/*
* SPDX-FileCopyrightText: 2021 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: CC0-1.0
*/
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "driver/rmt.h"
#include "step_motor.h"
#include "step_motor_driver_io_a4988.h"
// GPIO configuration
#define STEP_MOTOR_DIRECTION_PIN GPIO_NUM_18
#define STEP_MOTOR_STEP_PIN GPIO_NUM_17
#define STEP_MOTOR_SLEEP_PIN GPIO_NUM_16
#define STEP_MOTOR_RESET_PIN GPIO_NUM_15
#define STEP_MOTOR_MS3_PIN GPIO_NUM_7
#define STEP_MOTOR_MS2_PIN GPIO_NUM_6
#define STEP_MOTOR_MS1_PIN GPIO_NUM_5
#define STEP_MOTOR_ENABLE_PIN GPIO_NUM_4
#define RMT_TX_CHANNEL RMT_CHANNEL_0
static const char *TAG = "step_motor";
void app_main(void)
{
// Apply default RMT configuration
rmt_config_t dev_config = RMT_DEFAULT_CONFIG_TX(STEP_MOTOR_STEP_PIN, RMT_TX_CHANNEL);
step_motor_io_a4988_conf_t a4988_conf = {
.direction_pin = STEP_MOTOR_DIRECTION_PIN,
.sleep_pin = STEP_MOTOR_SLEEP_PIN,
.reset_pin = STEP_MOTOR_RESET_PIN,
.ms3_pin = STEP_MOTOR_MS3_PIN,
.ms2_pin = STEP_MOTOR_MS2_PIN,
.ms1_pin = STEP_MOTOR_MS1_PIN,
.enable_pin = STEP_MOTOR_ENABLE_PIN,
};
// Install low part driver
step_motor_driver_io_t *a4988_io;
ESP_ERROR_CHECK(step_motor_new_a4988_io_driver(&a4988_conf, &a4988_io));
// Install rmt driver
step_motor_t *motor = NULL;
ESP_ERROR_CHECK(step_motor_create_rmt(a4988_io, &dev_config, &motor));
step_motor_init(motor);
ESP_LOGI(TAG, "init");
ESP_LOGI(TAG, "set_step");
// configure Microstep to Full Step
step_motor_set_step(motor, 1, STEP_MOTOR_DIRECTION_POSITIVE);
vTaskDelay(pdMS_TO_TICKS(1000));
ESP_LOGI(TAG, "step 10 @ 1000/s");
step_motor_step(motor, 10, 1000);
vTaskDelay(pdMS_TO_TICKS(1000));
ESP_LOGI(TAG, "step 100 @ 1000/s");
step_motor_step(motor, 100, 1000);
vTaskDelay(pdMS_TO_TICKS(1000));
ESP_LOGI(TAG, "step 1000 @ 1200/s");
step_motor_step(motor, 1000, 1200);
vTaskDelay(pdMS_TO_TICKS(1000));
ESP_LOGI(TAG, "step 5000 @ 1400/s");
step_motor_step(motor, 5000, 1400);
vTaskDelay(pdMS_TO_TICKS(1000));
ESP_LOGI(TAG, "smoothstep start 5000 steps @ 500~1400/s");
step_motor_smooth_step(motor, 5000, 1000, 500, 1400);
ESP_LOGI(TAG, "smoothstep finish");
vTaskDelay(pdMS_TO_TICKS(1000));
ESP_LOGI(TAG, "continuous running for 5s");
step_motor_step(motor, UINT32_MAX, 1000);
vTaskDelay(pdMS_TO_TICKS(5000));
ESP_LOGI(TAG, "stop");
step_motor_step(motor, 0, 1000);
vTaskDelay(pdMS_TO_TICKS(1000));
step_motor_deinit(motor);
ESP_LOGI(TAG, "deinit");
ESP_ERROR_CHECK(step_motor_delete_rmt(motor));
ESP_ERROR_CHECK(step_motor_delete_a4988_io_driver(a4988_io));
}