1 /*
2 * SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG
3 * SPDX-FileCopyrightText: Copyright (c) 2024 Jilay Sandeep Pandya
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT zephyr_gpio_stepper
8
9 #include <zephyr/drivers/gpio.h>
10 #include <zephyr/kernel.h>
11 #include <zephyr/sys_clock.h>
12 #include <zephyr/drivers/stepper.h>
13 #include <zephyr/sys/__assert.h>
14
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_REGISTER(gpio_stepper_motor_controller, CONFIG_STEPPER_LOG_LEVEL);
17
18 #define MAX_MICRO_STEP_RES STEPPER_MICRO_STEP_2
19 #define NUM_CONTROL_PINS 4
20
21 static const uint8_t
22 half_step_lookup_table[NUM_CONTROL_PINS * MAX_MICRO_STEP_RES][NUM_CONTROL_PINS] = {
23 {1u, 1u, 0u, 0u}, {0u, 1u, 0u, 0u}, {0u, 1u, 1u, 0u}, {0u, 0u, 1u, 0u},
24 {0u, 0u, 1u, 1u}, {0u, 0u, 0u, 1u}, {1u, 0u, 0u, 1u}, {1u, 0u, 0u, 0u}};
25
26 struct gpio_stepper_config {
27 const struct gpio_dt_spec *control_pins;
28 bool invert_direction;
29 };
30
31 struct gpio_stepper_data {
32 const struct device *dev;
33 struct k_spinlock lock;
34 enum stepper_direction direction;
35 enum stepper_run_mode run_mode;
36 uint8_t step_gap;
37 uint8_t coil_charge;
38 struct k_work_delayable stepper_dwork;
39 int32_t actual_position;
40 uint32_t delay_in_us;
41 int32_t step_count;
42 bool is_enabled;
43 stepper_event_callback_t callback;
44 void *event_cb_user_data;
45 };
46
stepper_motor_set_coil_charge(const struct device * dev)47 static int stepper_motor_set_coil_charge(const struct device *dev)
48 {
49 struct gpio_stepper_data *data = dev->data;
50 const struct gpio_stepper_config *config = dev->config;
51
52 for (int i = 0; i < NUM_CONTROL_PINS; i++) {
53 (void)gpio_pin_set_dt(&config->control_pins[i],
54 half_step_lookup_table[data->coil_charge][i]);
55 }
56 return 0;
57 }
58
increment_coil_charge(const struct device * dev)59 static void increment_coil_charge(const struct device *dev)
60 {
61 struct gpio_stepper_data *data = dev->data;
62
63 if (data->coil_charge == NUM_CONTROL_PINS * MAX_MICRO_STEP_RES - data->step_gap) {
64 data->coil_charge = 0;
65 } else {
66 data->coil_charge = data->coil_charge + data->step_gap;
67 }
68 }
69
decrement_coil_charge(const struct device * dev)70 static void decrement_coil_charge(const struct device *dev)
71 {
72 struct gpio_stepper_data *data = dev->data;
73
74 if (data->coil_charge == 0) {
75 data->coil_charge = NUM_CONTROL_PINS * MAX_MICRO_STEP_RES - data->step_gap;
76 } else {
77 data->coil_charge = data->coil_charge - data->step_gap;
78 }
79 }
80
power_down_coils(const struct device * dev)81 static int power_down_coils(const struct device *dev)
82 {
83 const struct gpio_stepper_config *config = dev->config;
84
85 for (int i = 0; i < NUM_CONTROL_PINS; i++) {
86 const int err = gpio_pin_set_dt(&config->control_pins[i], 0u);
87
88 if (err != 0) {
89 LOG_ERR("Failed to power down coil %d", i);
90 return err;
91 }
92 }
93 return 0;
94 }
95
update_coil_charge(const struct device * dev)96 static void update_coil_charge(const struct device *dev)
97 {
98 const struct gpio_stepper_config *config = dev->config;
99 struct gpio_stepper_data *data = dev->data;
100
101 if (data->direction == STEPPER_DIRECTION_POSITIVE) {
102 config->invert_direction ? decrement_coil_charge(dev) : increment_coil_charge(dev);
103 data->actual_position++;
104 } else if (data->direction == STEPPER_DIRECTION_NEGATIVE) {
105 config->invert_direction ? increment_coil_charge(dev) : decrement_coil_charge(dev);
106 data->actual_position--;
107 }
108 }
109
update_remaining_steps(struct gpio_stepper_data * data)110 static void update_remaining_steps(struct gpio_stepper_data *data)
111 {
112 if (data->step_count > 0) {
113 data->step_count--;
114 (void)k_work_reschedule(&data->stepper_dwork, K_USEC(data->delay_in_us));
115 } else if (data->step_count < 0) {
116 data->step_count++;
117 (void)k_work_reschedule(&data->stepper_dwork, K_USEC(data->delay_in_us));
118 } else {
119 if (!data->callback) {
120 LOG_WRN_ONCE("No callback set");
121 return;
122 }
123 data->callback(data->dev, STEPPER_EVENT_STEPS_COMPLETED, data->event_cb_user_data);
124 }
125 }
126
update_direction_from_step_count(const struct device * dev)127 static void update_direction_from_step_count(const struct device *dev)
128 {
129 struct gpio_stepper_data *data = dev->data;
130
131 if (data->step_count > 0) {
132 data->direction = STEPPER_DIRECTION_POSITIVE;
133 } else if (data->step_count < 0) {
134 data->direction = STEPPER_DIRECTION_NEGATIVE;
135 } else {
136 LOG_ERR("Step count is zero");
137 }
138 }
139
position_mode_task(const struct device * dev)140 static void position_mode_task(const struct device *dev)
141 {
142 struct gpio_stepper_data *data = dev->data;
143
144 if (data->step_count) {
145 (void)stepper_motor_set_coil_charge(dev);
146 update_coil_charge(dev);
147 }
148 update_remaining_steps(dev->data);
149 }
150
velocity_mode_task(const struct device * dev)151 static void velocity_mode_task(const struct device *dev)
152 {
153 struct gpio_stepper_data *data = dev->data;
154
155 (void)stepper_motor_set_coil_charge(dev);
156 update_coil_charge(dev);
157 (void)k_work_reschedule(&data->stepper_dwork, K_USEC(data->delay_in_us));
158 }
159
stepper_work_step_handler(struct k_work * work)160 static void stepper_work_step_handler(struct k_work *work)
161 {
162 struct k_work_delayable *dwork = k_work_delayable_from_work(work);
163 struct gpio_stepper_data *data =
164 CONTAINER_OF(dwork, struct gpio_stepper_data, stepper_dwork);
165
166 K_SPINLOCK(&data->lock) {
167 switch (data->run_mode) {
168 case STEPPER_RUN_MODE_POSITION:
169 position_mode_task(data->dev);
170 break;
171 case STEPPER_RUN_MODE_VELOCITY:
172 velocity_mode_task(data->dev);
173 break;
174 default:
175 LOG_WRN("Unsupported run mode %d", data->run_mode);
176 break;
177 }
178 }
179 }
180
gpio_stepper_move_by(const struct device * dev,int32_t micro_steps)181 static int gpio_stepper_move_by(const struct device *dev, int32_t micro_steps)
182 {
183 struct gpio_stepper_data *data = dev->data;
184
185 if (!data->is_enabled) {
186 LOG_ERR("Stepper motor is not enabled");
187 return -ECANCELED;
188 }
189
190 if (data->delay_in_us == 0) {
191 LOG_ERR("Velocity not set or invalid velocity set");
192 return -EINVAL;
193 }
194 K_SPINLOCK(&data->lock) {
195 data->run_mode = STEPPER_RUN_MODE_POSITION;
196 data->step_count = micro_steps;
197 update_direction_from_step_count(dev);
198 (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
199 }
200 return 0;
201 }
202
gpio_stepper_set_reference_position(const struct device * dev,int32_t position)203 static int gpio_stepper_set_reference_position(const struct device *dev, int32_t position)
204 {
205 struct gpio_stepper_data *data = dev->data;
206
207 K_SPINLOCK(&data->lock) {
208 data->actual_position = position;
209 }
210 return 0;
211 }
212
gpio_stepper_get_actual_position(const struct device * dev,int32_t * position)213 static int gpio_stepper_get_actual_position(const struct device *dev, int32_t *position)
214 {
215 struct gpio_stepper_data *data = dev->data;
216
217 K_SPINLOCK(&data->lock) {
218 *position = data->actual_position;
219 }
220 return 0;
221 }
222
gpio_stepper_move_to(const struct device * dev,int32_t micro_steps)223 static int gpio_stepper_move_to(const struct device *dev, int32_t micro_steps)
224 {
225 struct gpio_stepper_data *data = dev->data;
226
227 if (!data->is_enabled) {
228 LOG_ERR("Stepper motor is not enabled");
229 return -ECANCELED;
230 }
231
232 if (data->delay_in_us == 0) {
233 LOG_ERR("Velocity not set or invalid velocity set");
234 return -EINVAL;
235 }
236 K_SPINLOCK(&data->lock) {
237 data->run_mode = STEPPER_RUN_MODE_POSITION;
238 data->step_count = micro_steps - data->actual_position;
239 update_direction_from_step_count(dev);
240 (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
241 }
242 return 0;
243 }
244
gpio_stepper_is_moving(const struct device * dev,bool * is_moving)245 static int gpio_stepper_is_moving(const struct device *dev, bool *is_moving)
246 {
247 struct gpio_stepper_data *data = dev->data;
248
249 *is_moving = k_work_delayable_is_pending(&data->stepper_dwork);
250 LOG_DBG("Motor is %s moving", *is_moving ? "" : "not");
251 return 0;
252 }
253
gpio_stepper_set_max_velocity(const struct device * dev,uint32_t velocity)254 static int gpio_stepper_set_max_velocity(const struct device *dev, uint32_t velocity)
255 {
256 struct gpio_stepper_data *data = dev->data;
257
258 if (velocity == 0) {
259 LOG_ERR("Velocity cannot be zero");
260 return -EINVAL;
261 }
262
263 if (velocity > USEC_PER_SEC) {
264 LOG_ERR("Velocity cannot be greater than %d micro_steps_per_second", USEC_PER_SEC);
265 return -EINVAL;
266 }
267
268 K_SPINLOCK(&data->lock) {
269 data->delay_in_us = USEC_PER_SEC / velocity;
270 }
271 LOG_DBG("Setting Motor Speed to %d", velocity);
272 return 0;
273 }
274
gpio_stepper_run(const struct device * dev,const enum stepper_direction direction,const uint32_t velocity)275 static int gpio_stepper_run(const struct device *dev, const enum stepper_direction direction,
276 const uint32_t velocity)
277 {
278 struct gpio_stepper_data *data = dev->data;
279
280 if (!data->is_enabled) {
281 LOG_ERR("Stepper motor is not enabled");
282 return -ECANCELED;
283 }
284
285 K_SPINLOCK(&data->lock) {
286 data->run_mode = STEPPER_RUN_MODE_VELOCITY;
287 data->direction = direction;
288 if (velocity != 0) {
289 data->delay_in_us = USEC_PER_SEC / velocity;
290 (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
291 } else {
292 (void)k_work_cancel_delayable(&data->stepper_dwork);
293 }
294 }
295 return 0;
296 }
297
gpio_stepper_set_micro_step_res(const struct device * dev,enum stepper_micro_step_resolution micro_step_res)298 static int gpio_stepper_set_micro_step_res(const struct device *dev,
299 enum stepper_micro_step_resolution micro_step_res)
300 {
301 struct gpio_stepper_data *data = dev->data;
302
303 K_SPINLOCK(&data->lock) {
304 switch (micro_step_res) {
305 case STEPPER_MICRO_STEP_1:
306 case STEPPER_MICRO_STEP_2:
307 data->step_gap = MAX_MICRO_STEP_RES >> (micro_step_res - 1);
308 break;
309 default:
310 LOG_ERR("Unsupported micro step resolution %d", micro_step_res);
311 return -ENOTSUP;
312 }
313 }
314 return 0;
315 }
316
gpio_stepper_get_micro_step_res(const struct device * dev,enum stepper_micro_step_resolution * micro_step_res)317 static int gpio_stepper_get_micro_step_res(const struct device *dev,
318 enum stepper_micro_step_resolution *micro_step_res)
319 {
320 struct gpio_stepper_data *data = dev->data;
321 *micro_step_res = MAX_MICRO_STEP_RES >> (data->step_gap - 1);
322 return 0;
323 }
324
gpio_stepper_set_event_callback(const struct device * dev,stepper_event_callback_t callback,void * user_data)325 static int gpio_stepper_set_event_callback(const struct device *dev,
326 stepper_event_callback_t callback, void *user_data)
327 {
328 struct gpio_stepper_data *data = dev->data;
329
330 K_SPINLOCK(&data->lock) {
331 data->callback = callback;
332 }
333 data->event_cb_user_data = user_data;
334 return 0;
335 }
336
gpio_stepper_enable(const struct device * dev,bool enable)337 static int gpio_stepper_enable(const struct device *dev, bool enable)
338 {
339 struct gpio_stepper_data *data = dev->data;
340
341 K_SPINLOCK(&data->lock) {
342
343 data->is_enabled = enable;
344
345 if (enable) {
346 (void)k_work_reschedule(&data->stepper_dwork, K_NO_WAIT);
347 } else {
348 (void)k_work_cancel_delayable(&data->stepper_dwork);
349 const int err = power_down_coils(dev);
350
351 if (err != 0) {
352 return -EIO;
353 }
354 }
355 }
356 return 0;
357 }
358
gpio_stepper_init(const struct device * dev)359 static int gpio_stepper_init(const struct device *dev)
360 {
361 struct gpio_stepper_data *data = dev->data;
362 const struct gpio_stepper_config *config = dev->config;
363
364 data->dev = dev;
365 LOG_DBG("Initializing %s gpio_stepper with %d pin", dev->name, NUM_CONTROL_PINS);
366 for (uint8_t n_pin = 0; n_pin < NUM_CONTROL_PINS; n_pin++) {
367 (void)gpio_pin_configure_dt(&config->control_pins[n_pin], GPIO_OUTPUT_INACTIVE);
368 }
369 k_work_init_delayable(&data->stepper_dwork, stepper_work_step_handler);
370 return 0;
371 }
372
373 static DEVICE_API(stepper, gpio_stepper_api) = {
374 .enable = gpio_stepper_enable,
375 .move_by = gpio_stepper_move_by,
376 .is_moving = gpio_stepper_is_moving,
377 .set_reference_position = gpio_stepper_set_reference_position,
378 .get_actual_position = gpio_stepper_get_actual_position,
379 .move_to = gpio_stepper_move_to,
380 .set_max_velocity = gpio_stepper_set_max_velocity,
381 .run = gpio_stepper_run,
382 .set_micro_step_res = gpio_stepper_set_micro_step_res,
383 .get_micro_step_res = gpio_stepper_get_micro_step_res,
384 .set_event_callback = gpio_stepper_set_event_callback,
385 };
386
387 #define GPIO_STEPPER_DEFINE(inst) \
388 static const struct gpio_dt_spec gpio_stepper_motor_control_pins_##inst[] = { \
389 DT_INST_FOREACH_PROP_ELEM_SEP(inst, gpios, GPIO_DT_SPEC_GET_BY_IDX, (,)), \
390 }; \
391 BUILD_ASSERT(ARRAY_SIZE(gpio_stepper_motor_control_pins_##inst) == 4, \
392 "gpio_stepper_controller driver currently supports only 4 wire configuration"); \
393 static const struct gpio_stepper_config gpio_stepper_config_##inst = { \
394 .invert_direction = DT_INST_PROP(inst, invert_direction), \
395 .control_pins = gpio_stepper_motor_control_pins_##inst}; \
396 static struct gpio_stepper_data gpio_stepper_data_##inst = { \
397 .step_gap = MAX_MICRO_STEP_RES >> (DT_INST_PROP(inst, micro_step_res) - 1), \
398 }; \
399 BUILD_ASSERT(DT_INST_PROP(inst, micro_step_res) <= STEPPER_MICRO_STEP_2, \
400 "gpio_stepper_controller driver supports up to 2 micro steps"); \
401 DEVICE_DT_INST_DEFINE(inst, gpio_stepper_init, NULL, &gpio_stepper_data_##inst, \
402 &gpio_stepper_config_##inst, POST_KERNEL, \
403 CONFIG_STEPPER_INIT_PRIORITY, &gpio_stepper_api);
404
405 DT_INST_FOREACH_STATUS_OKAY(GPIO_STEPPER_DEFINE)
406
