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