1 /*
2  * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
3  *
4  * SPDX-License-Identifier: CC0-1.0
5  */
6 
7 /*
8  * This file contains an implementation of step motor middleware based on rmt peripheral
9  */
10 
11 #include <math.h>
12 #include "freertos/FreeRTOS.h"
13 #include "freertos/semphr.h"
14 #include "esp_log.h"
15 #include "esp_check.h"
16 #include "driver/rmt.h"
17 #include "step_motor.h"
18 
19 static const char *TAG = "RMT_STEP_MOTOR";
20 
21 typedef enum {
22     STOPPED = 0,
23     SMOOTH_SPEED_UP,
24     SMOOTH_KEEP_SPEED,
25     SMOOTH_SLOW_DOWN,
26     UNLIMITED_LOOP,
27     LIMITED_LOOP,
28 } rmt_step_motor_running_status;
29 
30 typedef struct {
31     step_motor_t base;
32     step_motor_driver_io_t *io_driver;
33     rmt_channel_t rmt_ch;
34     rmt_step_motor_running_status status;
35     rmt_item32_t rmt_items_loop;
36     uint32_t rmt_items_loop_count;
37     rmt_item32_t *rmt_items_speedup;
38     rmt_item32_t *rmt_items_speeddown;
39     uint32_t rmt_items_smoothstep_count;
40     SemaphoreHandle_t notify_semphr;
41 } rmt_step_motor_t;
42 
helper_smootherstep_clamp(float x,float lowerlimit,float upperlimit)43 static inline float helper_smootherstep_clamp(float x, float lowerlimit, float upperlimit)
44 {
45     if (x < lowerlimit) {
46         x = lowerlimit;
47     }
48     if (x > upperlimit) {
49         x = upperlimit;
50     }
51     return x;
52 }
53 
54 // smoothstep formula
55 // see https://en.wikipedia.org/wiki/Smoothstep
helper_smootherstep(float edge0,float edge1,float x)56 static float helper_smootherstep(float edge0, float edge1, float x)
57 {
58     // Scale, and clamp x to 0..1 range
59     x = helper_smootherstep_clamp((x - edge0) / (edge1 - edge0), 0.0, 1.0);
60     // Evaluate polynomial
61     return x * x * x * (x * (x * 6 - 15) + 10) * (edge1 - edge0) + edge0;
62 }
63 
helper_speed_to_duration(uint16_t speed)64 static uint16_t helper_speed_to_duration(uint16_t speed)
65 {
66     return (uint16_t) round(1.0 * 1000 * 1000 / speed);
67 }
68 
helper_fill_rmt_items(rmt_item32_t * items,uint32_t speed,const step_motor_driver_io_t * io_driver)69 static esp_err_t helper_fill_rmt_items(rmt_item32_t *items, uint32_t speed, const step_motor_driver_io_t *io_driver)
70 {
71     items->duration1 = io_driver->step_triggered_edge ? io_driver->pulse_high_period_us : io_driver->pulse_low_period_us;
72     items->level1 = io_driver->step_triggered_edge;
73     items->level0 = !io_driver->step_triggered_edge;
74     uint32_t delay_period = helper_speed_to_duration(speed);
75     if (delay_period <= (io_driver->step_triggered_edge ? io_driver->pulse_low_period_us : io_driver->pulse_high_period_us)) {
76         ESP_LOGW(TAG, "maximum rate reached, driver will generate another possible highest rate instead");
77         items->duration0 = io_driver->step_triggered_edge ? io_driver->pulse_low_period_us : io_driver->pulse_high_period_us;
78     } else {
79         items->duration0 = delay_period;
80     }
81     return ESP_OK;
82 }
83 
rmt_step_motor_init(step_motor_t * motor)84 static esp_err_t rmt_step_motor_init(step_motor_t *motor)
85 {
86     rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
87     step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
88     if (io_driver->init) {
89         return io_driver->init(io_driver);
90     }
91     return ESP_OK;
92 }
93 
rmt_step_motor_deinit(step_motor_t * motor)94 static esp_err_t rmt_step_motor_deinit(step_motor_t *motor)
95 {
96     rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
97     step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
98     if (io_driver->deinit) {
99         return io_driver->deinit(io_driver);
100     }
101     return ESP_OK;
102 }
103 
104 // assume n != 0 and speed is within considerable range
rmt_step_motor_step_impl(step_motor_t * motor,uint32_t n,uint32_t speed)105 static esp_err_t rmt_step_motor_step_impl(step_motor_t *motor, uint32_t n, uint32_t speed)
106 {
107     rmt_step_motor_t *rmt_handle = __containerof(motor, rmt_step_motor_t, base);
108 
109     ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, true));
110     ESP_ERROR_CHECK(rmt_enable_tx_loop_autostop(rmt_handle->rmt_ch, true));
111 
112     rmt_handle->rmt_items_loop_count = n;
113     if ((rmt_handle->rmt_items_loop_count) > 1023) {
114         (rmt_handle->rmt_items_loop_count) -= 1023;
115         ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, 1023));
116     } else {
117         ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, rmt_handle->rmt_items_loop_count));
118         rmt_handle->rmt_items_loop_count = 0;
119     }
120     helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed, rmt_handle->io_driver);
121 
122     rmt_handle->status = LIMITED_LOOP;
123 
124     rmt_write_items(rmt_handle->rmt_ch, &rmt_handle->rmt_items_loop, 1, false);
125     xSemaphoreTake(rmt_handle->notify_semphr, portMAX_DELAY);
126     return ESP_OK;
127 }
128 
rmt_step_motor_step(step_motor_t * handle,uint32_t n,uint32_t speed)129 static esp_err_t rmt_step_motor_step(step_motor_t *handle, uint32_t n, uint32_t speed)
130 {
131     rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
132 
133     ESP_ERROR_CHECK(rmt_tx_stop(rmt_handle->rmt_ch));
134 
135     if (n == UINT32_MAX) {   // forever loop, non-blocking
136         ESP_ERROR_CHECK(rmt_set_tx_loop_count(rmt_handle->rmt_ch, 0));
137         ESP_ERROR_CHECK(rmt_enable_tx_loop_autostop(rmt_handle->rmt_ch, false));
138         ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, true));
139         helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed, rmt_handle->io_driver);
140         rmt_handle->status = UNLIMITED_LOOP;
141         ESP_ERROR_CHECK(rmt_write_items(rmt_handle->rmt_ch, &rmt_handle->rmt_items_loop, 1, false));
142         return ESP_OK;
143     } else if (n == 0) {     // break the forever loop
144         rmt_handle->status = STOPPED;
145         ESP_ERROR_CHECK(rmt_tx_stop(rmt_handle->rmt_ch));
146         ESP_ERROR_CHECK(rmt_set_tx_loop_mode(rmt_handle->rmt_ch, false));
147         return ESP_OK;
148     } else {                 // normally move n steps
149         ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed) > 1, ESP_ERR_INVALID_ARG, TAG,
150                             "speed too fast");
151         return rmt_step_motor_step_impl(handle, n, speed);
152     }
153 }
154 
rmt_step_motor_smoothstep(step_motor_t * handle,uint32_t n,uint32_t speed_steps,uint32_t speed_min,uint32_t speed_max)155 static esp_err_t rmt_step_motor_smoothstep(step_motor_t *handle, uint32_t n, uint32_t speed_steps, uint32_t speed_min,
156         uint32_t speed_max)
157 {
158     esp_err_t ret = ESP_OK;
159     ESP_RETURN_ON_FALSE(speed_min <= speed_max, ESP_ERR_INVALID_ARG, TAG, "max speed lower than min speed");
160     ESP_RETURN_ON_FALSE(n > speed_steps * 2, ESP_ERR_INVALID_ARG, TAG, "too few steps. consider lower speed_steps");
161     ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed_min) < 1 << 15, ESP_ERR_INVALID_ARG, TAG, "min speed too low");
162     ESP_RETURN_ON_FALSE(helper_speed_to_duration(speed_max) > 1, ESP_ERR_INVALID_ARG, TAG, "max speed too high");
163 
164     rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
165     rmt_handle->rmt_items_speedup = malloc(sizeof(rmt_item32_t) * speed_steps);
166     ESP_RETURN_ON_FALSE(rmt_handle->rmt_items_speedup != NULL, ESP_ERR_NO_MEM, TAG,
167                         "failed to allocate rmt_items_speedup");
168     rmt_handle->rmt_items_speeddown = malloc(sizeof(rmt_item32_t) * speed_steps);
169     ESP_GOTO_ON_FALSE(rmt_handle->rmt_items_speeddown != NULL, ESP_ERR_NO_MEM, err_free_speedup, TAG,
170                       "failed to allocate rmt_items_speeddown");
171     ESP_GOTO_ON_ERROR(rmt_tx_stop(rmt_handle->rmt_ch), err_free_speeddown, TAG, "failed to stop rmt tx");
172 
173     // prepare speed tables
174     for (int i = 0; i < speed_steps; ++i) {
175         helper_fill_rmt_items(&rmt_handle->rmt_items_speedup[i],
176                               (uint16_t)helper_smootherstep(
177                                   (float)speed_min,
178                                   (float)speed_max,
179                                   (float)speed_min + ( (float)i / (float)speed_steps) * (float)(speed_max - speed_min))
180                               , rmt_handle->io_driver
181                              );
182     }
183     for (int i = 0; i < speed_steps; ++i) {
184         helper_fill_rmt_items(&rmt_handle->rmt_items_speeddown[i],
185                               speed_max + speed_min - (uint16_t)helper_smootherstep(
186                                   (float)speed_min,
187                                   (float)speed_max,
188                                   (float)speed_min + ((float) i / (float)speed_steps) * (float)(speed_max - speed_min)
189                               )
190                               , rmt_handle->io_driver
191                              );
192     }
193     rmt_handle->rmt_items_smoothstep_count = speed_steps;
194     // prepare continuous phase rmt payload
195     helper_fill_rmt_items(&rmt_handle->rmt_items_loop, speed_max, rmt_handle->io_driver);
196     rmt_handle->rmt_items_loop_count = n - speed_steps * 2;
197     // set status to be checked inside ISR
198     rmt_handle->status = SMOOTH_SPEED_UP;
199     // start transmitting
200     ESP_ERROR_CHECK(rmt_write_items(rmt_handle->rmt_ch, rmt_handle->rmt_items_speedup, speed_steps, false));
201 
202     // waiting for transfer done
203     xSemaphoreTake(rmt_handle->notify_semphr, portMAX_DELAY);
204 
205 err_free_speeddown:
206     free(rmt_handle->rmt_items_speeddown);
207 err_free_speedup:
208     free(rmt_handle->rmt_items_speedup);
209     return ret;
210 }
211 
rmt_step_motor_set_step(step_motor_t * handle,uint16_t microstep,bool direction)212 static esp_err_t rmt_step_motor_set_step(step_motor_t *handle, uint16_t microstep, bool direction)
213 {
214     rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
215     step_motor_driver_io_t *io_driver = rmt_handle->io_driver;
216     if (io_driver->set_direction) {
217         ESP_ERROR_CHECK(io_driver->set_direction(io_driver, direction));
218     }
219     if (io_driver->set_microstep) {
220         ESP_ERROR_CHECK(io_driver->set_microstep(io_driver, microstep));
221     }
222     // at least 200ns delay as described in datasheet
223     esp_rom_delay_us(1);
224     return ESP_OK;
225 }
226 
rmt_tx_loop_intr(rmt_channel_t channel,void * args)227 static IRAM_ATTR void rmt_tx_loop_intr(rmt_channel_t channel, void *args)
228 {
229     rmt_step_motor_t *rmt_step_motor = (rmt_step_motor_t *) args;
230 
231     // smoothstep speedup stage finished
232     if (rmt_step_motor->status == SMOOTH_SPEED_UP) {
233         rmt_step_motor->status = SMOOTH_KEEP_SPEED;
234         rmt_set_tx_loop_mode(rmt_step_motor->rmt_ch, true);
235         rmt_enable_tx_loop_autostop(rmt_step_motor->rmt_ch, true);
236         rmt_set_tx_intr_en(rmt_step_motor->rmt_ch, 0);
237         // continue and configure loop count
238     }
239 
240     if (rmt_step_motor->status == SMOOTH_KEEP_SPEED || rmt_step_motor->status == LIMITED_LOOP) {
241         // loop count not 0, continuing looping
242         if ((rmt_step_motor->rmt_items_loop_count) != 0) {
243             if ((rmt_step_motor->rmt_items_loop_count) > 1023) {
244                 (rmt_step_motor->rmt_items_loop_count) -= 1023;
245                 rmt_set_tx_loop_count(rmt_step_motor->rmt_ch, 1023);
246             } else {
247                 rmt_set_tx_loop_count(rmt_step_motor->rmt_ch, rmt_step_motor->rmt_items_loop_count);
248                 rmt_step_motor->rmt_items_loop_count = 0;
249             }
250             rmt_write_items(rmt_step_motor->rmt_ch, &rmt_step_motor->rmt_items_loop, 1, false);
251             return;
252         }
253     }
254 
255     // smoothstep keep speed stage finished
256     if (rmt_step_motor->status == SMOOTH_KEEP_SPEED) {
257         rmt_step_motor->status = SMOOTH_SLOW_DOWN;
258         rmt_set_tx_loop_mode(rmt_step_motor->rmt_ch, false);
259         rmt_enable_tx_loop_autostop(rmt_step_motor->rmt_ch, false);
260         rmt_set_tx_intr_en(rmt_step_motor->rmt_ch, 1);
261         rmt_write_items(rmt_step_motor->rmt_ch, rmt_step_motor->rmt_items_speeddown, rmt_step_motor->rmt_items_smoothstep_count, false);
262         return;
263     }
264 
265     if (rmt_step_motor->status == LIMITED_LOOP || rmt_step_motor->status == SMOOTH_SLOW_DOWN) {
266         rmt_step_motor->status = STOPPED;
267         BaseType_t xHigherPriorityTaskWoken = pdFALSE;
268         xSemaphoreGiveFromISR(rmt_step_motor->notify_semphr, &xHigherPriorityTaskWoken);
269         if (xHigherPriorityTaskWoken == pdTRUE) {
270             portYIELD_FROM_ISR();
271         }
272     }
273 }
274 
step_motor_create_rmt(step_motor_driver_io_t * io_driver,const rmt_config_t * rmt_conf,step_motor_handle_t * ret_handle)275 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)
276 {
277     esp_err_t ret = ESP_OK;
278     rmt_step_motor_t *rmt_step_motor = NULL;
279 
280     ESP_RETURN_ON_ERROR(rmt_config(rmt_conf), TAG, "Failed to configure RMT");
281     ESP_RETURN_ON_ERROR(rmt_driver_install(rmt_conf->channel, 0, 0), TAG, "Failed to install RMT driver");
282 
283     ESP_GOTO_ON_FALSE(io_driver, ESP_ERR_INVALID_ARG, err, TAG, "configuration can't be null");
284     ESP_GOTO_ON_FALSE(ret_handle, ESP_ERR_INVALID_ARG, err, TAG, "can't assign handle to null");
285 
286     rmt_step_motor = calloc(1, sizeof(rmt_step_motor_t));
287     ESP_GOTO_ON_FALSE(rmt_step_motor, ESP_ERR_NO_MEM, err, TAG, "allocate context memory failed");
288     rmt_step_motor->rmt_ch = rmt_conf->channel;
289 
290     rmt_step_motor->notify_semphr = xSemaphoreCreateBinary();
291     ESP_GOTO_ON_FALSE(rmt_step_motor, ESP_ERR_NO_MEM, err, TAG, "allocate semaphore memory failed");
292 
293     rmt_step_motor->io_driver = io_driver;
294 
295     // register tx end callback function, which got invoked when tx loop comes to the end
296     rmt_register_tx_end_callback(rmt_tx_loop_intr, rmt_step_motor);
297 
298     rmt_step_motor->base.init = rmt_step_motor_init;
299     rmt_step_motor->base.deinit = rmt_step_motor_deinit;
300     rmt_step_motor->base.step = rmt_step_motor_step;
301     rmt_step_motor->base.set_step = rmt_step_motor_set_step;
302     rmt_step_motor->base.smooth_step = rmt_step_motor_smoothstep;
303 
304     *ret_handle = &(rmt_step_motor->base);
305     return ESP_OK;
306 
307 err:
308     if (rmt_step_motor) {
309         if (rmt_step_motor->notify_semphr) {
310             vSemaphoreDelete(rmt_step_motor->notify_semphr);
311         }
312         free(rmt_step_motor);
313     }
314     return ret;
315 }
316 
step_motor_delete_rmt(step_motor_handle_t handle)317 esp_err_t step_motor_delete_rmt(step_motor_handle_t handle)
318 {
319     ESP_RETURN_ON_FALSE(handle, ESP_ERR_INVALID_STATE, TAG, "empty handle");
320     rmt_step_motor_t *rmt_handle = __containerof(handle, rmt_step_motor_t, base);
321     ESP_RETURN_ON_ERROR(rmt_driver_uninstall(rmt_handle->rmt_ch), TAG, "Failed to uninstall RMT driver");
322     vSemaphoreDelete(rmt_handle->notify_semphr);
323     free(rmt_handle);
324     return ESP_OK;
325 }
326