2 * Copyright Droids Corporation (2008)
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 * Revision : $Id: commands_cs.c,v 1.2 2009-04-07 20:03:48 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
37 #include <control_system_manager.h>
38 #include <blocking_detection_manager.h>
42 #include <parse_string.h>
43 #include <parse_num.h>
50 const prog_char *name;
54 prog_char csb_left_arm_str[] = "left_arm";
55 prog_char csb_right_arm_str[] = "right_arm";
56 struct csb_list csb_list[] = {
57 { .name = csb_left_arm_str, .csb = &mechboard.left_arm },
58 { .name = csb_right_arm_str, .csb = &mechboard.right_arm },
61 struct cmd_cs_result {
62 fixed_string_t cmdname;
63 fixed_string_t csname;
66 /* token to be used for all cs-related commands */
67 prog_char str_csb_name[] = "left_arm#right_arm";
68 parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
70 struct cs_block *cs_from_name(const char *name)
74 for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
75 if (!strcmp_P(name, csb_list[i].name))
76 return csb_list[i].csb;
81 /**********************************************************/
82 /* Gains for control system */
84 /* this structure is filled when cmd_gain is parsed successfully */
85 struct cmd_gain_result {
86 struct cmd_cs_result cs;
92 /* function called when cmd_gain is parsed successfully */
93 static void cmd_gain_parsed(void * parsed_result, void *show)
95 struct cmd_gain_result *res = parsed_result;
98 csb = cs_from_name(res->cs.csname);
100 printf_P(PSTR("null csb\r\n"));
105 pid_set_gains(&csb->pid, res->p, res->i, res->d);
107 printf_P(PSTR("%s %s %d %d %d\r\n"),
110 pid_get_gain_P(&csb->pid),
111 pid_get_gain_I(&csb->pid),
112 pid_get_gain_D(&csb->pid));
115 prog_char str_gain_arg0[] = "gain";
116 parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
117 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
118 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
119 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
121 prog_char help_gain[] = "Set gain values for PID";
122 parse_pgm_inst_t cmd_gain = {
123 .f = cmd_gain_parsed, /* function to call */
124 .data = NULL, /* 2nd arg of func */
125 .help_str = help_gain,
126 .tokens = { /* token list, NULL terminated */
127 (prog_void *)&cmd_gain_arg0,
128 (prog_void *)&cmd_csb_name_tok,
129 (prog_void *)&cmd_gain_p,
130 (prog_void *)&cmd_gain_i,
131 (prog_void *)&cmd_gain_d,
137 /* this structure is filled when cmd_gain is parsed successfully */
138 struct cmd_gain_show_result {
139 struct cmd_cs_result cs;
143 prog_char str_gain_show_arg[] = "show";
144 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
146 prog_char help_gain_show[] = "Show gain values for PID";
147 parse_pgm_inst_t cmd_gain_show = {
148 .f = cmd_gain_parsed, /* function to call */
149 .data = (void *)1, /* 2nd arg of func */
150 .help_str = help_gain_show,
151 .tokens = { /* token list, NULL terminated */
152 (prog_void *)&cmd_gain_arg0,
153 (prog_void *)&cmd_csb_name_tok,
154 (prog_void *)&cmd_gain_show_arg,
159 /**********************************************************/
160 /* Speeds for control system */
162 /* this structure is filled when cmd_speed is parsed successfully */
163 struct cmd_speed_result {
164 struct cmd_cs_result cs;
168 /* function called when cmd_speed is parsed successfully */
169 static void cmd_speed_parsed(void *parsed_result, __attribute__((unused)) void *show)
171 struct cmd_speed_result * res = parsed_result;
173 struct cs_block *csb;
175 csb = cs_from_name(res->cs.csname);
177 printf_P(PSTR("null csb\r\n"));
183 ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
185 printf_P(PSTR("%s %lu\r\n"),
189 printf_P(PSTR("TODO\r\n"));
193 prog_char str_speed_arg0[] = "speed";
194 parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
195 parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
197 prog_char help_speed[] = "Set speed values for ramp filter";
198 parse_pgm_inst_t cmd_speed = {
199 .f = cmd_speed_parsed, /* function to call */
200 .data = NULL, /* 2nd arg of func */
201 .help_str = help_speed,
202 .tokens = { /* token list, NULL terminated */
203 (prog_void *)&cmd_speed_arg0,
204 (prog_void *)&cmd_csb_name_tok,
205 (prog_void *)&cmd_speed_s,
211 struct cmd_speed_show_result {
212 struct cmd_cs_result cs;
216 prog_char str_speed_show_arg[] = "show";
217 parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
219 prog_char help_speed_show[] = "Show speed values for ramp filter";
220 parse_pgm_inst_t cmd_speed_show = {
221 .f = cmd_speed_parsed, /* function to call */
222 .data = (void *)1, /* 2nd arg of func */
223 .help_str = help_speed_show,
224 .tokens = { /* token list, NULL terminated */
225 (prog_void *)&cmd_speed_arg0,
226 (prog_void *)&cmd_csb_name_tok,
227 (prog_void *)&cmd_speed_show_arg,
232 /**********************************************************/
233 /* Derivate_Filters for control system */
235 /* this structure is filled when cmd_derivate_filter is parsed successfully */
236 struct cmd_derivate_filter_result {
237 struct cmd_cs_result cs;
241 /* function called when cmd_derivate_filter is parsed successfully */
242 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
244 struct cmd_derivate_filter_result * res = parsed_result;
245 struct cs_block *csb;
247 csb = cs_from_name(res->cs.csname);
249 printf_P(PSTR("null csb\r\n"));
254 pid_set_derivate_filter(&csb->pid, res->size);
256 printf_P(PSTR("%s %s %u\r\n"),
259 pid_get_derivate_filter(&csb->pid));
262 prog_char str_derivate_filter_arg0[] = "derivate_filter";
263 parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
264 parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
266 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
267 parse_pgm_inst_t cmd_derivate_filter = {
268 .f = cmd_derivate_filter_parsed, /* function to call */
269 .data = (void *)1, /* 2nd arg of func */
270 .help_str = help_derivate_filter,
271 .tokens = { /* token list, NULL terminated */
272 (prog_void *)&cmd_derivate_filter_arg0,
273 (prog_void *)&cmd_csb_name_tok,
274 (prog_void *)&cmd_derivate_filter_size,
281 struct cmd_derivate_filter_show_result {
282 struct cmd_cs_result cs;
286 prog_char str_derivate_filter_show_arg[] = "show";
287 parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg);
289 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
290 parse_pgm_inst_t cmd_derivate_filter_show = {
291 .f = cmd_derivate_filter_parsed, /* function to call */
292 .data = NULL, /* 2nd arg of func */
293 .help_str = help_derivate_filter_show,
294 .tokens = { /* token list, NULL terminated */
295 (prog_void *)&cmd_derivate_filter_arg0,
296 (prog_void *)&cmd_csb_name_tok,
297 (prog_void *)&cmd_derivate_filter_show_arg,
303 /**********************************************************/
304 /* Consign for control system */
306 /* this structure is filled when cmd_consign is parsed successfully */
307 struct cmd_consign_result {
308 struct cmd_cs_result cs;
312 /* function called when cmd_consign is parsed successfully */
313 static void cmd_consign_parsed(void * parsed_result, __attribute__((unused)) void *data)
315 struct cmd_consign_result * res = parsed_result;
316 struct cs_block *csb;
318 csb = cs_from_name(res->cs.csname);
320 printf_P(PSTR("null csb\r\n"));
324 cs_set_consign(&csb->cs, res->p);
327 prog_char str_consign_arg0[] = "consign";
328 parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
329 parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
331 prog_char help_consign[] = "Set consign value";
332 parse_pgm_inst_t cmd_consign = {
333 .f = cmd_consign_parsed, /* function to call */
334 .data = NULL, /* 2nd arg of func */
335 .help_str = help_consign,
336 .tokens = { /* token list, NULL terminated */
337 (prog_void *)&cmd_consign_arg0,
338 (prog_void *)&cmd_csb_name_tok,
339 (prog_void *)&cmd_consign_p,
345 /**********************************************************/
346 /* Maximums for control system */
348 /* this structure is filled when cmd_maximum is parsed successfully */
349 struct cmd_maximum_result {
350 struct cmd_cs_result cs;
356 /* function called when cmd_maximum is parsed successfully */
357 static void cmd_maximum_parsed(void *parsed_result, void *show)
359 struct cmd_maximum_result * res = parsed_result;
361 struct cs_block *csb;
363 csb = cs_from_name(res->cs.csname);
365 printf_P(PSTR("null csb\r\n"));
370 pid_set_maximums(&csb->pid, res->in, res->i, res->out);
372 printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
374 pid_get_max_in(&csb->pid),
375 pid_get_max_I(&csb->pid),
376 pid_get_max_out(&csb->pid));
379 prog_char str_maximum_arg0[] = "maximum";
380 parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
381 parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
382 parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
383 parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
385 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
386 parse_pgm_inst_t cmd_maximum = {
387 .f = cmd_maximum_parsed, /* function to call */
388 .data = NULL, /* 2nd arg of func */
389 .help_str = help_maximum,
390 .tokens = { /* token list, NULL terminated */
391 (prog_void *)&cmd_maximum_arg0,
392 (prog_void *)&cmd_csb_name_tok,
393 (prog_void *)&cmd_maximum_in,
394 (prog_void *)&cmd_maximum_i,
395 (prog_void *)&cmd_maximum_out,
402 /* this structure is filled when cmd_maximum is parsed successfully */
403 struct cmd_maximum_show_result {
404 struct cmd_cs_result cs;
407 prog_char str_maximum_show_arg[] = "show";
408 parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
410 prog_char help_maximum_show[] = "Show maximum values for PID";
411 parse_pgm_inst_t cmd_maximum_show = {
412 .f = cmd_maximum_parsed, /* function to call */
413 .data = (void *)1, /* 2nd arg of func */
414 .help_str = help_maximum_show,
415 .tokens = { /* token list, NULL terminated */
416 (prog_void *)&cmd_maximum_arg0,
417 (prog_void *)&cmd_csb_name_tok,
418 (prog_void *)&cmd_maximum_show_arg,
423 /**********************************************************/
424 /* Quadramp for control system */
426 /* this structure is filled when cmd_quadramp is parsed successfully */
427 struct cmd_quadramp_result {
428 struct cmd_cs_result cs;
435 /* function called when cmd_quadramp is parsed successfully */
436 static void cmd_quadramp_parsed(void *parsed_result, void *show)
438 struct cmd_quadramp_result * res = parsed_result;
440 struct cs_block *csb;
442 csb = cs_from_name(res->cs.csname);
444 printf_P(PSTR("null csb\r\n"));
449 quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
450 quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
453 printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
455 csb->qr.var_2nd_ord_pos,
456 csb->qr.var_2nd_ord_neg,
457 csb->qr.var_1st_ord_pos,
458 csb->qr.var_1st_ord_neg);
461 prog_char str_quadramp_arg0[] = "quadramp";
462 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
463 parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
464 parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
465 parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
466 parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
468 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
469 parse_pgm_inst_t cmd_quadramp = {
470 .f = cmd_quadramp_parsed, /* function to call */
471 .data = NULL, /* 2nd arg of func */
472 .help_str = help_quadramp,
473 .tokens = { /* token list, NULL terminated */
474 (prog_void *)&cmd_quadramp_arg0,
475 (prog_void *)&cmd_csb_name_tok,
476 (prog_void *)&cmd_quadramp_ap,
477 (prog_void *)&cmd_quadramp_an,
478 (prog_void *)&cmd_quadramp_sp,
479 (prog_void *)&cmd_quadramp_sn,
487 struct cmd_quadramp_show_result {
488 struct cmd_cs_result cs;
492 prog_char str_quadramp_show_arg[] = "show";
493 parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
495 prog_char help_quadramp_show[] = "Get quadramp values for control system";
496 parse_pgm_inst_t cmd_quadramp_show = {
497 .f = cmd_quadramp_parsed, /* function to call */
498 .data = (void *)1, /* 2nd arg of func */
499 .help_str = help_quadramp_show,
500 .tokens = { /* token list, NULL terminated */
501 (prog_void *)&cmd_quadramp_arg0,
502 (prog_void *)&cmd_csb_name_tok,
503 (prog_void *)&cmd_quadramp_show_arg,
510 /**********************************************************/
511 /* cs_status show for control system */
513 /* this structure is filled when cmd_cs_status is parsed successfully */
514 struct cmd_cs_status_result {
515 struct cmd_cs_result cs;
519 /* function called when cmd_cs_status is parsed successfully */
520 static void cmd_cs_status_parsed(void *parsed_result, __attribute__((unused)) void *data)
522 struct cmd_cs_status_result *res = parsed_result;
523 struct cs_block *csb;
525 uint8_t print_pid = 0, print_cs = 0;
527 csb = cs_from_name(res->cs.csname);
529 printf_P(PSTR("null csb\r\n"));
532 if (strcmp_P(res->arg, PSTR("on")) == 0) {
534 printf_P(PSTR("%s is on\r\n"), res->cs.csname);
537 else if (strcmp_P(res->arg, PSTR("off")) == 0) {
539 printf_P(PSTR("%s is off\r\n"), res->cs.csname);
542 else if (strcmp_P(res->arg, PSTR("show")) == 0) {
545 else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
549 else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
552 else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
557 printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
560 dump_cs(res->cs.csname, &csb->cs);
562 dump_pid(res->cs.csname, &csb->pid);
564 } while(loop && !cmdline_keypressed());
567 prog_char str_cs_status_arg0[] = "cs_status";
568 parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
569 prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
570 parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
572 prog_char help_cs_status[] = "Show cs status";
573 parse_pgm_inst_t cmd_cs_status = {
574 .f = cmd_cs_status_parsed, /* function to call */
575 .data = NULL, /* 2nd arg of func */
576 .help_str = help_cs_status,
577 .tokens = { /* token list, NULL terminated */
578 (prog_void *)&cmd_cs_status_arg0,
579 (prog_void *)&cmd_csb_name_tok,
580 (prog_void *)&cmd_cs_status_arg,
586 /**********************************************************/
587 /* Blocking_I for control system */
589 /* this structure is filled when cmd_blocking_i is parsed successfully */
590 struct cmd_blocking_i_result {
591 struct cmd_cs_result cs;
598 /* function called when cmd_blocking_i is parsed successfully */
599 static void cmd_blocking_i_parsed(void *parsed_result, void *show)
601 struct cmd_blocking_i_result * res = parsed_result;
603 struct cs_block *csb;
605 csb = cs_from_name(res->cs.csname);
607 printf_P(PSTR("null csb\r\n"));
612 bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
615 printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
624 prog_char str_blocking_i_arg0[] = "blocking";
625 parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
626 parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
627 parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
628 parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
629 parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
631 prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
632 parse_pgm_inst_t cmd_blocking_i = {
633 .f = cmd_blocking_i_parsed, /* function to call */
634 .data = NULL, /* 2nd arg of func */
635 .help_str = help_blocking_i,
636 .tokens = { /* token list, NULL terminated */
637 (prog_void *)&cmd_blocking_i_arg0,
638 (prog_void *)&cmd_csb_name_tok,
639 (prog_void *)&cmd_blocking_i_k1,
640 (prog_void *)&cmd_blocking_i_k2,
641 (prog_void *)&cmd_blocking_i_i,
642 (prog_void *)&cmd_blocking_i_cpt,
649 struct cmd_blocking_i_show_result {
650 struct cmd_cs_result cs;
654 prog_char str_blocking_i_show_arg[] = "show";
655 parse_pgm_token_string_t cmd_blocking_i_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_show_result, show, str_blocking_i_show_arg);
657 prog_char help_blocking_i_show[] = "Show blocking detection values";
658 parse_pgm_inst_t cmd_blocking_i_show = {
659 .f = cmd_blocking_i_parsed, /* function to call */
660 .data = (void *)1, /* 2nd arg of func */
661 .help_str = help_blocking_i_show,
662 .tokens = { /* token list, NULL terminated */
663 (prog_void *)&cmd_blocking_i_arg0,
664 (prog_void *)&cmd_csb_name_tok,
665 (prog_void *)&cmd_blocking_i_show_arg,