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