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_drv_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_DRV_MICROSTEP_PARAM_IDX 2
32 
33 #define STEPPER_DIRECTION_MAP_ENTRY(_name, _dir)                                                   \
34 	{                                                                                          \
35 		.name = _name,                                                                     \
36 		.direction = _dir,                                                                 \
37 	}
38 
39 #define STEPPER_MICROSTEP_MAP(_name, _microstep)                                                   \
40 	{                                                                                          \
41 		.name = _name,                                                                     \
42 		.microstep = _microstep,                                                           \
43 	}
44 
print_stepper_drv_event_cb(const struct device * dev,const enum stepper_drv_event event,void * user_data)45 static void print_stepper_drv_event_cb(const struct device *dev, const enum stepper_drv_event event,
46 				       void *user_data)
47 {
48 	const struct shell *sh = user_data;
49 
50 	if (!sh) {
51 		return;
52 	}
53 
54 	switch (event) {
55 	case STEPPER_DRV_EVENT_STALL_DETECTED:
56 		shell_info(sh, "%s: Stall detected.", dev->name);
57 		break;
58 	case STEPPER_DRV_EVENT_FAULT_DETECTED:
59 		shell_info(sh, "%s: Fault detected.", dev->name);
60 		break;
61 	default:
62 		shell_info(sh, "%s: Unknown event.", dev->name);
63 		break;
64 	}
65 }
66 
print_callback(const struct device * dev,const enum stepper_event event,void * user_data)67 static void print_callback(const struct device *dev, const enum stepper_event event,
68 			   void *user_data)
69 {
70 	const struct shell *sh = user_data;
71 
72 	if (!sh) {
73 		return;
74 	}
75 
76 	switch (event) {
77 	case STEPPER_EVENT_STEPS_COMPLETED:
78 		shell_info(sh, "%s: Steps completed.", dev->name);
79 		break;
80 	case STEPPER_EVENT_LEFT_END_STOP_DETECTED:
81 		shell_info(sh, "%s: Left limit switch pressed.", dev->name);
82 		break;
83 	case STEPPER_EVENT_RIGHT_END_STOP_DETECTED:
84 		shell_info(sh, "%s: Right limit switch pressed.", dev->name);
85 		break;
86 	case STEPPER_EVENT_STOPPED:
87 		shell_info(sh, "%s: Stepper stopped.", dev->name);
88 		break;
89 	default:
90 		shell_info(sh, "%s: Unknown signal received.", dev->name);
91 		break;
92 	}
93 }
94 
device_is_stepper_drv(const struct device * dev)95 static bool device_is_stepper_drv(const struct device *dev)
96 {
97 	return DEVICE_API_IS(stepper_drv, dev);
98 }
99 
device_is_stepper_controller(const struct device * dev)100 static bool device_is_stepper_controller(const struct device *dev)
101 {
102 	return DEVICE_API_IS(stepper, dev);
103 }
104 
105 static const struct stepper_direction_map stepper_direction_map[] = {
106 	STEPPER_DIRECTION_MAP_ENTRY("positive", STEPPER_DIRECTION_POSITIVE),
107 	STEPPER_DIRECTION_MAP_ENTRY("negative", STEPPER_DIRECTION_NEGATIVE),
108 };
109 
110 static const struct stepper_microstep_map stepper_microstep_map[] = {
111 	STEPPER_MICROSTEP_MAP("1", STEPPER_DRV_MICRO_STEP_1),
112 	STEPPER_MICROSTEP_MAP("2", STEPPER_DRV_MICRO_STEP_2),
113 	STEPPER_MICROSTEP_MAP("4", STEPPER_DRV_MICRO_STEP_4),
114 	STEPPER_MICROSTEP_MAP("8", STEPPER_DRV_MICRO_STEP_8),
115 	STEPPER_MICROSTEP_MAP("16", STEPPER_DRV_MICRO_STEP_16),
116 	STEPPER_MICROSTEP_MAP("32", STEPPER_DRV_MICRO_STEP_32),
117 	STEPPER_MICROSTEP_MAP("64", STEPPER_DRV_MICRO_STEP_64),
118 	STEPPER_MICROSTEP_MAP("128", STEPPER_DRV_MICRO_STEP_128),
119 	STEPPER_MICROSTEP_MAP("256", STEPPER_DRV_MICRO_STEP_256),
120 };
121 
cmd_stepper_direction(size_t idx,struct shell_static_entry * entry)122 static void cmd_stepper_direction(size_t idx, struct shell_static_entry *entry)
123 {
124 	if (idx < ARRAY_SIZE(stepper_direction_map)) {
125 		entry->syntax = stepper_direction_map[idx].name;
126 	} else {
127 		entry->syntax = NULL;
128 	}
129 	entry->handler = NULL;
130 	entry->help = "Stepper direction";
131 	entry->subcmd = NULL;
132 }
133 
134 SHELL_DYNAMIC_CMD_CREATE(dsub_stepper_direction, cmd_stepper_direction);
135 
cmd_stepper_microstep(size_t idx,struct shell_static_entry * entry)136 static void cmd_stepper_microstep(size_t idx, struct shell_static_entry *entry)
137 {
138 	if (idx < ARRAY_SIZE(stepper_microstep_map)) {
139 		entry->syntax = stepper_microstep_map[idx].name;
140 	} else {
141 		entry->syntax = NULL;
142 	}
143 	entry->handler = NULL;
144 	entry->help = "Stepper microstep resolution";
145 	entry->subcmd = NULL;
146 }
147 
148 SHELL_DYNAMIC_CMD_CREATE(dsub_stepper_microstep, cmd_stepper_microstep);
149 
cmd_pos_stepper_motor_name(size_t idx,struct shell_static_entry * entry)150 static void cmd_pos_stepper_motor_name(size_t idx, struct shell_static_entry *entry)
151 {
152 	const struct device *dev = shell_device_filter(idx, device_is_stepper_drv);
153 
154 	entry->syntax = (dev != NULL) ? dev->name : NULL;
155 	entry->handler = NULL;
156 	entry->help = "List Devices";
157 	entry->subcmd = NULL;
158 }
159 
160 SHELL_DYNAMIC_CMD_CREATE(dsub_pos_stepper_motor_name, cmd_pos_stepper_motor_name);
161 
cmd_pos_stepper_controller_name(size_t idx,struct shell_static_entry * entry)162 static void cmd_pos_stepper_controller_name(size_t idx, struct shell_static_entry *entry)
163 {
164 	const struct device *dev = shell_device_filter(idx, device_is_stepper_controller);
165 
166 	entry->syntax = (dev != NULL) ? dev->name : NULL;
167 	entry->handler = NULL;
168 	entry->help = "List Devices";
169 	entry->subcmd = NULL;
170 }
171 
172 SHELL_DYNAMIC_CMD_CREATE(dsub_pos_stepper_controller_name, cmd_pos_stepper_controller_name);
173 
cmd_pos_stepper_controller_name_dir(size_t idx,struct shell_static_entry * entry)174 static void cmd_pos_stepper_controller_name_dir(size_t idx, struct shell_static_entry *entry)
175 {
176 	const struct device *dev = shell_device_filter(idx, device_is_stepper_controller);
177 
178 	if (dev != NULL) {
179 		entry->syntax = dev->name;
180 	} else {
181 		entry->syntax = NULL;
182 	}
183 	entry->handler = NULL;
184 	entry->help = "List Devices";
185 	entry->subcmd = &dsub_stepper_direction;
186 }
187 
188 SHELL_DYNAMIC_CMD_CREATE(dsub_pos_stepper_controller_name_dir, cmd_pos_stepper_controller_name_dir);
189 
cmd_pos_stepper_motor_name_microstep(size_t idx,struct shell_static_entry * entry)190 static void cmd_pos_stepper_motor_name_microstep(size_t idx, struct shell_static_entry *entry)
191 {
192 	const struct device *dev = shell_device_filter(idx, device_is_stepper_drv);
193 
194 	if (dev != NULL) {
195 		entry->syntax = dev->name;
196 	} else {
197 		entry->syntax = NULL;
198 	}
199 	entry->handler = NULL;
200 	entry->help = "List Devices";
201 	entry->subcmd = &dsub_stepper_microstep;
202 }
203 
204 SHELL_DYNAMIC_CMD_CREATE(dsub_pos_stepper_motor_name_microstep,
205 			 cmd_pos_stepper_motor_name_microstep);
206 
parse_device_arg(const struct shell * sh,char ** argv,const struct device ** dev)207 static int parse_device_arg(const struct shell *sh, char **argv, const struct device **dev)
208 {
209 	*dev = shell_device_get_binding(argv[ARG_IDX_DEV]);
210 	if (!*dev) {
211 		shell_error(sh, "Stepper device %s not found", argv[ARG_IDX_DEV]);
212 		return -ENODEV;
213 	}
214 	return 0;
215 }
216 
cmd_stepper_enable(const struct shell * sh,size_t argc,char ** argv)217 static int cmd_stepper_enable(const struct shell *sh, size_t argc, char **argv)
218 {
219 	const struct device *dev;
220 	int err;
221 
222 	err = parse_device_arg(sh, argv, &dev);
223 	if (err < 0) {
224 		return err;
225 	}
226 
227 	err = stepper_drv_enable(dev);
228 	if (err) {
229 		shell_error(sh, "Error: %d", err);
230 	}
231 
232 	err = stepper_drv_set_event_cb(dev, print_stepper_drv_event_cb, (void *)sh);
233 	if (err) {
234 		shell_error(sh, "Failed to set stepper driver event callback: %d", err);
235 	}
236 	return err;
237 }
238 
cmd_stepper_disable(const struct shell * sh,size_t argc,char ** argv)239 static int cmd_stepper_disable(const struct shell *sh, size_t argc, char **argv)
240 {
241 	const struct device *dev;
242 	int err;
243 
244 	err = parse_device_arg(sh, argv, &dev);
245 	if (err < 0) {
246 		return err;
247 	}
248 
249 	err = stepper_drv_disable(dev);
250 	if (err) {
251 		shell_error(sh, "Error: %d", err);
252 	}
253 
254 	return err;
255 }
256 
cmd_stepper_stop(const struct shell * sh,size_t argc,char ** argv)257 static int cmd_stepper_stop(const struct shell *sh, size_t argc, char **argv)
258 {
259 	const struct device *dev;
260 	int err = 0;
261 
262 	err = parse_device_arg(sh, argv, &dev);
263 	if (err < 0) {
264 		return err;
265 	}
266 
267 	err = stepper_stop(dev);
268 	if (err) {
269 		shell_error(sh, "Error: %d", err);
270 		return err;
271 	}
272 
273 	err = stepper_set_event_callback(dev, print_callback, (void *)sh);
274 	if (err != 0) {
275 		shell_error(sh, "Failed to set callback: %d", err);
276 	}
277 
278 	return err;
279 }
280 
cmd_stepper_move_by(const struct shell * sh,size_t argc,char ** argv)281 static int cmd_stepper_move_by(const struct shell *sh, size_t argc, char **argv)
282 {
283 	const struct device *dev;
284 	int err = 0;
285 
286 	int32_t micro_steps = shell_strtol(argv[ARG_IDX_PARAM], 10, &err);
287 
288 	if (err < 0) {
289 		return err;
290 	}
291 
292 	err = parse_device_arg(sh, argv, &dev);
293 	if (err < 0) {
294 		return err;
295 	}
296 
297 	err = stepper_set_event_callback(dev, print_callback, (void *)sh);
298 	if (err != 0) {
299 		shell_error(sh, "Failed to set callback: %d", err);
300 	}
301 
302 	err = stepper_move_by(dev, micro_steps);
303 	if (err) {
304 		shell_error(sh, "Error: %d", err);
305 	}
306 
307 	return err;
308 }
309 
cmd_stepper_set_microstep_interval(const struct shell * sh,size_t argc,char ** argv)310 static int cmd_stepper_set_microstep_interval(const struct shell *sh, size_t argc, char **argv)
311 {
312 	const struct device *dev;
313 	int err = 0;
314 
315 	uint64_t step_interval = shell_strtoull(argv[ARG_IDX_PARAM], 10, &err);
316 
317 	if (err < 0) {
318 		return err;
319 	}
320 
321 	err = parse_device_arg(sh, argv, &dev);
322 	if (err < 0) {
323 		return err;
324 	}
325 
326 	err = stepper_set_microstep_interval(dev, step_interval);
327 	if (err) {
328 		shell_error(sh, "Error: %d", err);
329 	}
330 
331 	return err;
332 }
333 
cmd_stepper_set_micro_step_res(const struct shell * sh,size_t argc,char ** argv)334 static int cmd_stepper_set_micro_step_res(const struct shell *sh, size_t argc, char **argv)
335 {
336 	const struct device *dev;
337 	enum stepper_drv_micro_step_resolution resolution;
338 	int err = -EINVAL;
339 
340 	for (int i = 0; i < ARRAY_SIZE(stepper_microstep_map); i++) {
341 		if (strcmp(argv[STEPPER_DRV_MICROSTEP_PARAM_IDX], stepper_microstep_map[i].name) ==
342 		    0) {
343 			resolution = stepper_microstep_map[i].microstep;
344 			err = 0;
345 			break;
346 		}
347 	}
348 	if (err != 0) {
349 		shell_error(sh, "Invalid microstep value %s",
350 			    argv[STEPPER_DRV_MICROSTEP_PARAM_IDX]);
351 		return err;
352 	}
353 
354 	err = parse_device_arg(sh, argv, &dev);
355 	if (err < 0) {
356 		return err;
357 	}
358 
359 	err = stepper_drv_set_micro_step_res(dev, resolution);
360 	if (err) {
361 		shell_error(sh, "Error: %d", err);
362 	}
363 
364 	return err;
365 }
366 
cmd_stepper_get_micro_step_res(const struct shell * sh,size_t argc,char ** argv)367 static int cmd_stepper_get_micro_step_res(const struct shell *sh, size_t argc, char **argv)
368 {
369 	const struct device *dev;
370 	int err;
371 	enum stepper_drv_micro_step_resolution micro_step_res;
372 
373 	err = parse_device_arg(sh, argv, &dev);
374 	if (err < 0) {
375 		return err;
376 	}
377 
378 	err = stepper_drv_get_micro_step_res(dev, &micro_step_res);
379 	if (err < 0) {
380 		shell_warn(sh, "Failed to get micro-step resolution: %d", err);
381 	} else {
382 		shell_print(sh, "Micro-step Resolution: %d", micro_step_res);
383 	}
384 
385 	return err;
386 }
387 
cmd_stepper_set_reference_position(const struct shell * sh,size_t argc,char ** argv)388 static int cmd_stepper_set_reference_position(const struct shell *sh, size_t argc, char **argv)
389 {
390 	const struct device *dev;
391 	int err = 0;
392 
393 	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_reference_position(dev, position);
405 	if (err) {
406 		shell_error(sh, "Error: %d", err);
407 	}
408 
409 	return err;
410 }
411 
cmd_stepper_get_actual_position(const struct shell * sh,size_t argc,char ** argv)412 static int cmd_stepper_get_actual_position(const struct shell *sh, size_t argc, char **argv)
413 {
414 	const struct device *dev;
415 	int err;
416 
417 	int32_t actual_position;
418 
419 	err = parse_device_arg(sh, argv, &dev);
420 	if (err < 0) {
421 		return err;
422 	}
423 
424 	err = stepper_get_actual_position(dev, &actual_position);
425 	if (err < 0) {
426 		shell_warn(sh, "Failed to get actual position: %d", err);
427 	} else {
428 		shell_print(sh, "Actual Position: %d", actual_position);
429 	}
430 
431 	return err;
432 }
433 
cmd_stepper_move_to(const struct shell * sh,size_t argc,char ** argv)434 static int cmd_stepper_move_to(const struct shell *sh, size_t argc, char **argv)
435 {
436 	const struct device *dev;
437 	int err = 0;
438 
439 	const int32_t position = shell_strtol(argv[ARG_IDX_PARAM], 10, &err);
440 
441 	if (err < 0) {
442 		return err;
443 	}
444 
445 	err = parse_device_arg(sh, argv, &dev);
446 	if (err < 0) {
447 		return err;
448 	}
449 
450 	err = stepper_set_event_callback(dev, print_callback, (void *)sh);
451 	if (err != 0) {
452 		shell_error(sh, "Failed to set callback: %d", err);
453 	}
454 
455 	err = stepper_move_to(dev, position);
456 	if (err) {
457 		shell_error(sh, "Error: %d", err);
458 	}
459 
460 	return err;
461 }
462 
cmd_stepper_run(const struct shell * sh,size_t argc,char ** argv)463 static int cmd_stepper_run(const struct shell *sh, size_t argc, char **argv)
464 {
465 	const struct device *dev;
466 	int err;
467 
468 	enum stepper_direction direction = STEPPER_DIRECTION_POSITIVE;
469 
470 	err = -EINVAL;
471 	for (int i = 0; i < ARRAY_SIZE(stepper_direction_map); i++) {
472 		if (strcmp(argv[ARG_IDX_PARAM], stepper_direction_map[i].name) == 0) {
473 			direction = stepper_direction_map[i].direction;
474 			err = 0;
475 			break;
476 		}
477 	}
478 	if (err != 0) {
479 		shell_error(sh, "Invalid direction %s", argv[ARG_IDX_PARAM]);
480 		return err;
481 	}
482 
483 	err = parse_device_arg(sh, argv, &dev);
484 	if (err < 0) {
485 		return err;
486 	}
487 
488 	err = stepper_set_event_callback(dev, print_callback, (void *)sh);
489 	if (err != 0) {
490 		shell_error(sh, "Failed to set callback: %d", err);
491 	}
492 
493 	err = stepper_run(dev, direction);
494 	if (err) {
495 		shell_error(sh, "Error: %d", err);
496 		return err;
497 	}
498 
499 	return 0;
500 }
501 
cmd_stepper_control_info(const struct shell * sh,size_t argc,char ** argv)502 static int cmd_stepper_control_info(const struct shell *sh, size_t argc, char **argv)
503 {
504 	const struct device *dev;
505 	int err;
506 	bool is_moving;
507 	int32_t actual_position;
508 
509 	err = parse_device_arg(sh, argv, &dev);
510 	if (err < 0) {
511 		return err;
512 	}
513 
514 	shell_print(sh, "Stepper Info:");
515 	shell_print(sh, "Device: %s", dev->name);
516 
517 	err = stepper_get_actual_position(dev, &actual_position);
518 	if (err < 0) {
519 		shell_warn(sh, "Failed to get actual position: %d", err);
520 	} else {
521 		shell_print(sh, "Actual Position: %d", actual_position);
522 	}
523 
524 	err = stepper_is_moving(dev, &is_moving);
525 	if (err < 0) {
526 		shell_warn(sh, "Failed to check if the motor is moving: %d", err);
527 	} else {
528 		shell_print(sh, "Is Moving: %s", is_moving ? "Yes" : "No");
529 	}
530 
531 	return 0;
532 }
533 
cmd_stepper_info(const struct shell * sh,size_t argc,char ** argv)534 static int cmd_stepper_info(const struct shell *sh, size_t argc, char **argv)
535 {
536 	const struct device *dev;
537 	int err;
538 	enum stepper_drv_micro_step_resolution micro_step_res;
539 
540 	err = parse_device_arg(sh, argv, &dev);
541 	if (err < 0) {
542 		return err;
543 	}
544 
545 	shell_print(sh, "Stepper Info:");
546 	shell_print(sh, "Device: %s", dev->name);
547 
548 	err = stepper_drv_get_micro_step_res(dev, &micro_step_res);
549 	if (err < 0) {
550 		shell_warn(sh, "Failed to get micro-step resolution: %d", err);
551 	} else {
552 		shell_print(sh, "Micro-step Resolution: %d", micro_step_res);
553 	}
554 
555 	return 0;
556 }
557 
558 SHELL_STATIC_SUBCMD_SET_CREATE(
559 	stepper_cmds,
560 	SHELL_CMD_ARG(enable, &dsub_pos_stepper_motor_name, "<device>", cmd_stepper_enable, 2, 0),
561 	SHELL_CMD_ARG(disable, &dsub_pos_stepper_motor_name, "<device>", cmd_stepper_disable, 2, 0),
562 	SHELL_CMD_ARG(set_micro_step_res, &dsub_pos_stepper_motor_name_microstep,
563 		      "<device> <resolution>", cmd_stepper_set_micro_step_res, 3, 0),
564 	SHELL_CMD_ARG(get_micro_step_res, &dsub_pos_stepper_motor_name, "<device>",
565 		      cmd_stepper_get_micro_step_res, 2, 0),
566 	SHELL_CMD_ARG(set_reference_position, &dsub_pos_stepper_controller_name,
567 		      "<device> <position>", cmd_stepper_set_reference_position, 3, 0),
568 	SHELL_CMD_ARG(get_actual_position, &dsub_pos_stepper_controller_name, "<device>",
569 		      cmd_stepper_get_actual_position, 2, 0),
570 	SHELL_CMD_ARG(set_microstep_interval, &dsub_pos_stepper_controller_name,
571 		      "<device> <microstep_interval_ns>", cmd_stepper_set_microstep_interval, 3, 0),
572 	SHELL_CMD_ARG(move_by, &dsub_pos_stepper_controller_name, "<device> <microsteps>",
573 		      cmd_stepper_move_by, 3, 0),
574 	SHELL_CMD_ARG(move_to, &dsub_pos_stepper_controller_name, "<device> <microsteps>",
575 		      cmd_stepper_move_to, 3, 0),
576 	SHELL_CMD_ARG(run, &dsub_pos_stepper_controller_name_dir, "<device> <direction>",
577 		      cmd_stepper_run, 3, 0),
578 	SHELL_CMD_ARG(stop, &dsub_pos_stepper_controller_name, "<device>", cmd_stepper_stop, 2, 0),
579 	SHELL_CMD_ARG(control_info, &dsub_pos_stepper_controller_name, "<device>",
580 		      cmd_stepper_control_info, 2, 0),
581 	SHELL_CMD_ARG(info, &dsub_pos_stepper_motor_name, "<device>", cmd_stepper_info, 2, 0),
582 	SHELL_SUBCMD_SET_END);
583 
584 SHELL_CMD_REGISTER(stepper, &stepper_cmds, "Stepper motor commands", NULL);
585