1 /*
2 * Copyright (c) 2024, Fabian Blatz <fabianblatz@gmail.com>
3 * Copyright (c) 2024, Jilay Sandeep Pandya
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/shell/shell.h>
9 #include <zephyr/device.h>
10 #include <zephyr/drivers/stepper.h>
11
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(stepper_shell, CONFIG_STEPPER_LOG_LEVEL);
14
15 enum {
16 ARG_IDX_DEV = 1,
17 ARG_IDX_PARAM = 2,
18 ARG_IDX_VALUE = 3,
19 };
20
21 struct stepper_microstep_map {
22 const char *name;
23 enum stepper_micro_step_resolution microstep;
24 };
25
26 struct stepper_direction_map {
27 const char *name;
28 enum stepper_direction direction;
29 };
30
31 #define STEPPER_DIRECTION_MAP_ENTRY(_name, _dir) \
32 { \
33 .name = _name, \
34 .direction = _dir, \
35 }
36
37 #define STEPPER_MICROSTEP_MAP(_name, _microstep) \
38 { \
39 .name = _name, \
40 .microstep = _microstep, \
41 }
42
print_callback(const struct device * dev,const enum stepper_event event,void * user_data)43 static void print_callback(const struct device *dev, const enum stepper_event event,
44 void *user_data)
45 {
46 const struct shell *sh = user_data;
47 if (!sh) {
48 return;
49 }
50
51 switch (event) {
52 case STEPPER_EVENT_STEPS_COMPLETED:
53 shell_info(sh, "%s: Steps completed.", dev->name);
54 break;
55 case STEPPER_EVENT_STALL_DETECTED:
56 shell_info(sh, "%s: Stall detected.", dev->name);
57 break;
58 case STEPPER_EVENT_LEFT_END_STOP_DETECTED:
59 shell_info(sh, "%s: Left limit switch pressed.", dev->name);
60 break;
61 case STEPPER_EVENT_RIGHT_END_STOP_DETECTED:
62 shell_info(sh, "%s: Right limit switch pressed.", dev->name);
63 break;
64 case STEPPER_EVENT_STOPPED:
65 shell_info(sh, "%s: Stepper stopped.", dev->name);
66 break;
67 default:
68 shell_info(sh, "%s: Unknown signal received.", dev->name);
69 break;
70 }
71 }
72
device_is_stepper(const struct device * dev)73 static bool device_is_stepper(const struct device *dev)
74 {
75 return DEVICE_API_IS(stepper, dev);
76 }
77
78 static const struct stepper_direction_map stepper_direction_map[] = {
79 STEPPER_DIRECTION_MAP_ENTRY("positive", STEPPER_DIRECTION_POSITIVE),
80 STEPPER_DIRECTION_MAP_ENTRY("negative", STEPPER_DIRECTION_NEGATIVE),
81 };
82
83 static const struct stepper_microstep_map stepper_microstep_map[] = {
84 STEPPER_MICROSTEP_MAP("1", STEPPER_MICRO_STEP_1),
85 STEPPER_MICROSTEP_MAP("2", STEPPER_MICRO_STEP_2),
86 STEPPER_MICROSTEP_MAP("4", STEPPER_MICRO_STEP_4),
87 STEPPER_MICROSTEP_MAP("8", STEPPER_MICRO_STEP_8),
88 STEPPER_MICROSTEP_MAP("16", STEPPER_MICRO_STEP_16),
89 STEPPER_MICROSTEP_MAP("32", STEPPER_MICRO_STEP_32),
90 STEPPER_MICROSTEP_MAP("64", STEPPER_MICRO_STEP_64),
91 STEPPER_MICROSTEP_MAP("128", STEPPER_MICRO_STEP_128),
92 STEPPER_MICROSTEP_MAP("256", STEPPER_MICRO_STEP_256),
93 };
94
cmd_stepper_direction(size_t idx,struct shell_static_entry * entry)95 static void cmd_stepper_direction(size_t idx, struct shell_static_entry *entry)
96 {
97 if (idx < ARRAY_SIZE(stepper_direction_map)) {
98 entry->syntax = stepper_direction_map[idx].name;
99 } else {
100 entry->syntax = NULL;
101 }
102 entry->handler = NULL;
103 entry->help = "Stepper direction";
104 entry->subcmd = NULL;
105 }
106
107 SHELL_DYNAMIC_CMD_CREATE(dsub_stepper_direction, cmd_stepper_direction);
108
cmd_stepper_microstep(size_t idx,struct shell_static_entry * entry)109 static void cmd_stepper_microstep(size_t idx, struct shell_static_entry *entry)
110 {
111 if (idx < ARRAY_SIZE(stepper_microstep_map)) {
112 entry->syntax = stepper_microstep_map[idx].name;
113 } else {
114 entry->syntax = NULL;
115 }
116 entry->handler = NULL;
117 entry->help = "Stepper microstep resolution";
118 entry->subcmd = NULL;
119 }
120
121 SHELL_DYNAMIC_CMD_CREATE(dsub_stepper_microstep, cmd_stepper_microstep);
122
cmd_pos_stepper_motor_name(size_t idx,struct shell_static_entry * entry)123 static void cmd_pos_stepper_motor_name(size_t idx, struct shell_static_entry *entry)
124 {
125 const struct device *dev = shell_device_filter(idx, device_is_stepper);
126
127 entry->syntax = (dev != NULL) ? dev->name : NULL;
128 entry->handler = NULL;
129 entry->help = "List Devices";
130 entry->subcmd = NULL;
131 }
132
133 SHELL_DYNAMIC_CMD_CREATE(dsub_pos_stepper_motor_name, cmd_pos_stepper_motor_name);
134
cmd_pos_stepper_motor_name_dir(size_t idx,struct shell_static_entry * entry)135 static void cmd_pos_stepper_motor_name_dir(size_t idx, struct shell_static_entry *entry)
136 {
137 const struct device *dev = shell_device_filter(idx, device_is_stepper);
138
139 if (dev != NULL) {
140 entry->syntax = dev->name;
141 } else {
142 entry->syntax = NULL;
143 }
144 entry->handler = NULL;
145 entry->help = "List Devices";
146 entry->subcmd = &dsub_stepper_direction;
147 }
148
149 SHELL_DYNAMIC_CMD_CREATE(dsub_pos_stepper_motor_name_dir, cmd_pos_stepper_motor_name_dir);
150
cmd_pos_stepper_motor_name_microstep(size_t idx,struct shell_static_entry * entry)151 static void cmd_pos_stepper_motor_name_microstep(size_t idx, struct shell_static_entry *entry)
152 {
153 const struct device *dev = shell_device_filter(idx, device_is_stepper);
154
155 if (dev != NULL) {
156 entry->syntax = dev->name;
157 } else {
158 entry->syntax = NULL;
159 }
160 entry->handler = NULL;
161 entry->help = "List Devices";
162 entry->subcmd = &dsub_stepper_microstep;
163 }
164
165 SHELL_DYNAMIC_CMD_CREATE(dsub_pos_stepper_motor_name_microstep,
166 cmd_pos_stepper_motor_name_microstep);
167
parse_device_arg(const struct shell * sh,char ** argv,const struct device ** dev)168 static int parse_device_arg(const struct shell *sh, char **argv, const struct device **dev)
169 {
170 *dev = shell_device_get_binding(argv[ARG_IDX_DEV]);
171 if (!*dev) {
172 shell_error(sh, "Stepper device %s not found", argv[ARG_IDX_DEV]);
173 return -ENODEV;
174 }
175 return 0;
176 }
177
cmd_stepper_enable(const struct shell * sh,size_t argc,char ** argv)178 static int cmd_stepper_enable(const struct shell *sh, size_t argc, char **argv)
179 {
180 const struct device *dev;
181 int err = 0;
182 bool enable = shell_strtobool(argv[ARG_IDX_PARAM], 10, &err);
183
184 if (err < 0) {
185 return err;
186 }
187
188 err = parse_device_arg(sh, argv, &dev);
189 if (err < 0) {
190 return err;
191 }
192
193 err = stepper_enable(dev, enable);
194 if (err) {
195 shell_error(sh, "Error: %d", err);
196 }
197
198 return err;
199 }
200
cmd_stepper_stop(const struct shell * sh,size_t argc,char ** argv)201 static int cmd_stepper_stop(const struct shell *sh, size_t argc, char **argv)
202 {
203 const struct device *dev;
204 int err = 0;
205
206 err = parse_device_arg(sh, argv, &dev);
207 if (err < 0) {
208 return err;
209 }
210
211 err = stepper_stop(dev);
212 if (err) {
213 shell_error(sh, "Error: %d", err);
214 return err;
215 }
216
217 err = stepper_set_event_callback(dev, print_callback, (void *)sh);
218 if (err != 0) {
219 shell_error(sh, "Failed to set callback: %d", err);
220 }
221
222 return err;
223 }
224
cmd_stepper_move_by(const struct shell * sh,size_t argc,char ** argv)225 static int cmd_stepper_move_by(const struct shell *sh, size_t argc, char **argv)
226 {
227 const struct device *dev;
228 int err = 0;
229
230 int32_t micro_steps = shell_strtol(argv[ARG_IDX_PARAM], 10, &err);
231
232 if (err < 0) {
233 return err;
234 }
235
236 err = parse_device_arg(sh, argv, &dev);
237 if (err < 0) {
238 return err;
239 }
240
241 err = stepper_set_event_callback(dev, print_callback, (void *)sh);
242 if (err != 0) {
243 shell_error(sh, "Failed to set callback: %d", err);
244 }
245
246 err = stepper_move_by(dev, micro_steps);
247 if (err) {
248 shell_error(sh, "Error: %d", err);
249 }
250
251 return err;
252 }
253
cmd_stepper_set_microstep_interval(const struct shell * sh,size_t argc,char ** argv)254 static int cmd_stepper_set_microstep_interval(const struct shell *sh, size_t argc, char **argv)
255 {
256 const struct device *dev;
257 int err = 0;
258 uint64_t step_interval = shell_strtoull(argv[ARG_IDX_PARAM], 10, &err);
259
260 if (err < 0) {
261 return err;
262 }
263
264 err = parse_device_arg(sh, argv, &dev);
265 if (err < 0) {
266 return err;
267 }
268
269 err = stepper_set_microstep_interval(dev, step_interval);
270 if (err) {
271 shell_error(sh, "Error: %d", err);
272 }
273
274 return err;
275 }
276
cmd_stepper_set_micro_step_res(const struct shell * sh,size_t argc,char ** argv)277 static int cmd_stepper_set_micro_step_res(const struct shell *sh, size_t argc, char **argv)
278 {
279 const struct device *dev;
280 enum stepper_micro_step_resolution resolution;
281 int err = -EINVAL;
282
283 for (int i = 0; i < ARRAY_SIZE(stepper_microstep_map); i++) {
284 if (strcmp(argv[ARG_IDX_PARAM], stepper_microstep_map[i].name) == 0) {
285 resolution = stepper_microstep_map[i].microstep;
286 err = 0;
287 break;
288 }
289 }
290 if (err != 0) {
291 shell_error(sh, "Invalid microstep value %s", argv[ARG_IDX_PARAM]);
292 return err;
293 }
294
295 err = parse_device_arg(sh, argv, &dev);
296 if (err < 0) {
297 return err;
298 }
299
300 err = stepper_set_micro_step_res(dev, resolution);
301 if (err) {
302 shell_error(sh, "Error: %d", err);
303 }
304
305 return err;
306 }
307
cmd_stepper_get_micro_step_res(const struct shell * sh,size_t argc,char ** argv)308 static int cmd_stepper_get_micro_step_res(const struct shell *sh, size_t argc, char **argv)
309 {
310 const struct device *dev;
311 int err;
312 enum stepper_micro_step_resolution micro_step_res;
313
314 err = parse_device_arg(sh, argv, &dev);
315 if (err < 0) {
316 return err;
317 }
318
319 err = stepper_get_micro_step_res(dev, µ_step_res);
320 if (err < 0) {
321 shell_warn(sh, "Failed to get micro-step resolution: %d", err);
322 } else {
323 shell_print(sh, "Micro-step Resolution: %d", micro_step_res);
324 }
325
326 return err;
327 }
328
cmd_stepper_set_reference_position(const struct shell * sh,size_t argc,char ** argv)329 static int cmd_stepper_set_reference_position(const struct shell *sh, size_t argc, char **argv)
330 {
331 const struct device *dev;
332 int err = 0;
333 int32_t position = shell_strtol(argv[ARG_IDX_PARAM], 10, &err);
334
335 if (err < 0) {
336 return err;
337 }
338
339 err = parse_device_arg(sh, argv, &dev);
340 if (err < 0) {
341 return err;
342 }
343
344 err = stepper_set_reference_position(dev, position);
345 if (err) {
346 shell_error(sh, "Error: %d", err);
347 }
348
349 return err;
350 }
351
cmd_stepper_get_actual_position(const struct shell * sh,size_t argc,char ** argv)352 static int cmd_stepper_get_actual_position(const struct shell *sh, size_t argc, char **argv)
353 {
354 const struct device *dev;
355 int err;
356 int32_t actual_position;
357
358 err = parse_device_arg(sh, argv, &dev);
359 if (err < 0) {
360 return err;
361 }
362
363 err = stepper_get_actual_position(dev, &actual_position);
364 if (err < 0) {
365 shell_warn(sh, "Failed to get actual position: %d", err);
366 } else {
367 shell_print(sh, "Actual Position: %d", actual_position);
368 }
369
370 return err;
371 }
372
cmd_stepper_move_to(const struct shell * sh,size_t argc,char ** argv)373 static int cmd_stepper_move_to(const struct shell *sh, size_t argc, char **argv)
374 {
375 const struct device *dev;
376 int err = 0;
377 const int32_t position = shell_strtol(argv[ARG_IDX_PARAM], 10, &err);
378
379 if (err < 0) {
380 return err;
381 }
382
383 err = parse_device_arg(sh, argv, &dev);
384 if (err < 0) {
385 return err;
386 }
387
388 err = stepper_set_event_callback(dev, print_callback, (void *)sh);
389 if (err != 0) {
390 shell_error(sh, "Failed to set callback: %d", err);
391 }
392
393 err = stepper_move_to(dev, position);
394 if (err) {
395 shell_error(sh, "Error: %d", err);
396 }
397
398 return err;
399 }
400
cmd_stepper_run(const struct shell * sh,size_t argc,char ** argv)401 static int cmd_stepper_run(const struct shell *sh, size_t argc, char **argv)
402 {
403 const struct device *dev;
404 int err = -EINVAL;
405 enum stepper_direction direction = STEPPER_DIRECTION_POSITIVE;
406
407 for (int i = 0; i < ARRAY_SIZE(stepper_direction_map); i++) {
408 if (strcmp(argv[ARG_IDX_PARAM], stepper_direction_map[i].name) == 0) {
409 direction = stepper_direction_map[i].direction;
410 err = 0;
411 break;
412 }
413 }
414 if (err != 0) {
415 shell_error(sh, "Invalid direction %s", argv[ARG_IDX_PARAM]);
416 return err;
417 }
418
419 err = parse_device_arg(sh, argv, &dev);
420 if (err < 0) {
421 return err;
422 }
423
424 err = stepper_set_event_callback(dev, print_callback, (void *)sh);
425 if (err != 0) {
426 shell_error(sh, "Failed to set callback: %d", err);
427 }
428
429 err = stepper_run(dev, direction);
430 if (err) {
431 shell_error(sh, "Error: %d", err);
432 return err;
433 }
434
435 return 0;
436 }
437
cmd_stepper_info(const struct shell * sh,size_t argc,char ** argv)438 static int cmd_stepper_info(const struct shell *sh, size_t argc, char **argv)
439 {
440 const struct device *dev;
441 int err;
442 bool is_moving;
443 int32_t actual_position;
444 enum stepper_micro_step_resolution micro_step_res;
445
446 err = parse_device_arg(sh, argv, &dev);
447 if (err < 0) {
448 return err;
449 }
450
451 shell_print(sh, "Stepper Info:");
452 shell_print(sh, "Device: %s", dev->name);
453
454 err = stepper_get_actual_position(dev, &actual_position);
455 if (err < 0) {
456 shell_warn(sh, "Failed to get actual position: %d", err);
457 } else {
458 shell_print(sh, "Actual Position: %d", actual_position);
459 }
460
461 err = stepper_get_micro_step_res(dev, µ_step_res);
462 if (err < 0) {
463 shell_warn(sh, "Failed to get micro-step resolution: %d", err);
464 } else {
465 shell_print(sh, "Micro-step Resolution: %d", micro_step_res);
466 }
467
468 err = stepper_is_moving(dev, &is_moving);
469 if (err < 0) {
470 shell_warn(sh, "Failed to check if the motor is moving: %d", err);
471 } else {
472 shell_print(sh, "Is Moving: %s", is_moving ? "Yes" : "No");
473 }
474
475 return 0;
476 }
477
478 SHELL_STATIC_SUBCMD_SET_CREATE(
479 stepper_cmds,
480 SHELL_CMD_ARG(enable, &dsub_pos_stepper_motor_name, "<device> <on/off>", cmd_stepper_enable,
481 3, 0),
482 SHELL_CMD_ARG(set_micro_step_res, &dsub_pos_stepper_motor_name_microstep,
483 "<device> <resolution>", cmd_stepper_set_micro_step_res, 3, 0),
484 SHELL_CMD_ARG(get_micro_step_res, &dsub_pos_stepper_motor_name, "<device>",
485 cmd_stepper_get_micro_step_res, 2, 0),
486 SHELL_CMD_ARG(set_reference_position, &dsub_pos_stepper_motor_name, "<device> <position>",
487 cmd_stepper_set_reference_position, 3, 0),
488 SHELL_CMD_ARG(get_actual_position, &dsub_pos_stepper_motor_name, "<device>",
489 cmd_stepper_get_actual_position, 2, 0),
490 SHELL_CMD_ARG(set_microstep_interval, &dsub_pos_stepper_motor_name,
491 "<device> <microstep_interval_ns>", cmd_stepper_set_microstep_interval, 3, 0),
492 SHELL_CMD_ARG(move_by, &dsub_pos_stepper_motor_name, "<device> <microsteps>",
493 cmd_stepper_move_by, 3, 0),
494 SHELL_CMD_ARG(move_to, &dsub_pos_stepper_motor_name, "<device> <microsteps>",
495 cmd_stepper_move_to, 3, 0),
496 SHELL_CMD_ARG(run, &dsub_pos_stepper_motor_name_dir, "<device> <direction>",
497 cmd_stepper_run, 3, 0),
498 SHELL_CMD_ARG(stop, &dsub_pos_stepper_motor_name, "<device>", cmd_stepper_stop, 2, 0),
499 SHELL_CMD_ARG(info, &dsub_pos_stepper_motor_name, "<device>", cmd_stepper_info, 2, 0),
500 SHELL_SUBCMD_SET_END);
501
502 SHELL_CMD_REGISTER(stepper, &stepper_cmds, "Stepper motor commands", NULL);
503
