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.1 2009-03-29 18:44:54 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
33 #include <clock_time.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_roller_str[] = "roller";
55 prog_char csb_forktrans_str[] = "forktrans";
56 prog_char csb_forkrot_str[] = "forkrot";
57 prog_char csb_beacon_str[] = "beacon";
58 struct csb_list csb_list[] = {
59 { .name = csb_roller_str, .csb = &ballboard.roller },
60 { .name = csb_forktrans_str, .csb = &ballboard.forktrans },
61 { .name = csb_forkrot_str, .csb = &ballboard.forkrot },
62 { .name = csb_beacon_str, .csb = &ballboard.beacon },
65 struct cmd_cs_result {
66 fixed_string_t cmdname;
67 fixed_string_t csname;
70 /* token to be used for all cs-related commands */
71 prog_char str_csb_name[] = "roller#forktrans#forkrot#beacon";
72 parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
74 struct cs_block *cs_from_name(const char *name)
78 for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
79 if (!strcmp_P(name, csb_list[i].name))
80 return csb_list[i].csb;
85 /**********************************************************/
86 /* Gains for control system */
88 /* this structure is filled when cmd_gain is parsed successfully */
89 struct cmd_gain_result {
90 struct cmd_cs_result cs;
96 /* function called when cmd_gain is parsed successfully */
97 static void cmd_gain_parsed(void * parsed_result, void *show)
99 struct cmd_gain_result *res = parsed_result;
100 struct cs_block *csb;
102 csb = cs_from_name(res->cs.csname);
104 printf_P(PSTR("null csb\r\n"));
109 pid_set_gains(&csb->pid, res->p, res->i, res->d);
111 printf_P(PSTR("%s %s %d %d %d\r\n"),
114 pid_get_gain_P(&csb->pid),
115 pid_get_gain_I(&csb->pid),
116 pid_get_gain_D(&csb->pid));
119 prog_char str_gain_arg0[] = "gain";
120 parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
121 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
122 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
123 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
125 prog_char help_gain[] = "Set gain values for PID";
126 parse_pgm_inst_t cmd_gain = {
127 .f = cmd_gain_parsed, /* function to call */
128 .data = NULL, /* 2nd arg of func */
129 .help_str = help_gain,
130 .tokens = { /* token list, NULL terminated */
131 (prog_void *)&cmd_gain_arg0,
132 (prog_void *)&cmd_csb_name_tok,
133 (prog_void *)&cmd_gain_p,
134 (prog_void *)&cmd_gain_i,
135 (prog_void *)&cmd_gain_d,
141 /* this structure is filled when cmd_gain is parsed successfully */
142 struct cmd_gain_show_result {
143 struct cmd_cs_result cs;
147 prog_char str_gain_show_arg[] = "show";
148 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
150 prog_char help_gain_show[] = "Show gain values for PID";
151 parse_pgm_inst_t cmd_gain_show = {
152 .f = cmd_gain_parsed, /* function to call */
153 .data = (void *)1, /* 2nd arg of func */
154 .help_str = help_gain_show,
155 .tokens = { /* token list, NULL terminated */
156 (prog_void *)&cmd_gain_arg0,
157 (prog_void *)&cmd_csb_name_tok,
158 (prog_void *)&cmd_gain_show_arg,
163 /**********************************************************/
164 /* Speeds for control system */
166 /* this structure is filled when cmd_speed is parsed successfully */
167 struct cmd_speed_result {
168 struct cmd_cs_result cs;
172 /* function called when cmd_speed is parsed successfully */
173 static void cmd_speed_parsed(void *parsed_result, void *show)
175 struct cmd_speed_result * res = parsed_result;
177 struct cs_block *csb;
179 csb = cs_from_name(res->cs.csname);
181 printf_P(PSTR("null csb\r\n"));
187 ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
189 printf_P(PSTR("%s %lu\r\n"),
193 printf_P(PSTR("TODO\r\n"));
197 prog_char str_speed_arg0[] = "speed";
198 parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
199 parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
201 prog_char help_speed[] = "Set speed values for ramp filter";
202 parse_pgm_inst_t cmd_speed = {
203 .f = cmd_speed_parsed, /* function to call */
204 .data = NULL, /* 2nd arg of func */
205 .help_str = help_speed,
206 .tokens = { /* token list, NULL terminated */
207 (prog_void *)&cmd_speed_arg0,
208 (prog_void *)&cmd_csb_name_tok,
209 (prog_void *)&cmd_speed_s,
215 struct cmd_speed_show_result {
216 struct cmd_cs_result cs;
220 prog_char str_speed_show_arg[] = "show";
221 parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
223 prog_char help_speed_show[] = "Show speed values for ramp filter";
224 parse_pgm_inst_t cmd_speed_show = {
225 .f = cmd_speed_parsed, /* function to call */
226 .data = (void *)1, /* 2nd arg of func */
227 .help_str = help_speed_show,
228 .tokens = { /* token list, NULL terminated */
229 (prog_void *)&cmd_speed_arg0,
230 (prog_void *)&cmd_csb_name_tok,
231 (prog_void *)&cmd_speed_show_arg,
236 /**********************************************************/
237 /* Derivate_Filters for control system */
239 /* this structure is filled when cmd_derivate_filter is parsed successfully */
240 struct cmd_derivate_filter_result {
241 struct cmd_cs_result cs;
245 /* function called when cmd_derivate_filter is parsed successfully */
246 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
248 struct cmd_derivate_filter_result * res = parsed_result;
249 struct cs_block *csb;
251 csb = cs_from_name(res->cs.csname);
253 printf_P(PSTR("null csb\r\n"));
258 pid_set_derivate_filter(&csb->pid, res->size);
260 printf_P(PSTR("%s %s %u\r\n"),
263 pid_get_derivate_filter(&csb->pid));
266 prog_char str_derivate_filter_arg0[] = "derivate_filter";
267 parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
268 parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
270 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
271 parse_pgm_inst_t cmd_derivate_filter = {
272 .f = cmd_derivate_filter_parsed, /* function to call */
273 .data = (void *)1, /* 2nd arg of func */
274 .help_str = help_derivate_filter,
275 .tokens = { /* token list, NULL terminated */
276 (prog_void *)&cmd_derivate_filter_arg0,
277 (prog_void *)&cmd_csb_name_tok,
278 (prog_void *)&cmd_derivate_filter_size,
285 struct cmd_derivate_filter_show_result {
286 struct cmd_cs_result cs;
290 prog_char str_derivate_filter_show_arg[] = "show";
291 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);
293 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
294 parse_pgm_inst_t cmd_derivate_filter_show = {
295 .f = cmd_derivate_filter_parsed, /* function to call */
296 .data = NULL, /* 2nd arg of func */
297 .help_str = help_derivate_filter_show,
298 .tokens = { /* token list, NULL terminated */
299 (prog_void *)&cmd_derivate_filter_arg0,
300 (prog_void *)&cmd_csb_name_tok,
301 (prog_void *)&cmd_derivate_filter_show_arg,
307 /**********************************************************/
308 /* Consign for control system */
310 /* this structure is filled when cmd_consign is parsed successfully */
311 struct cmd_consign_result {
312 struct cmd_cs_result cs;
316 /* function called when cmd_consign is parsed successfully */
317 static void cmd_consign_parsed(void * parsed_result, void *data)
319 struct cmd_consign_result * res = parsed_result;
320 struct cs_block *csb;
322 csb = cs_from_name(res->cs.csname);
324 printf_P(PSTR("null csb\r\n"));
328 cs_set_consign(&csb->cs, res->p);
331 prog_char str_consign_arg0[] = "consign";
332 parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
333 parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
335 prog_char help_consign[] = "Set consign value";
336 parse_pgm_inst_t cmd_consign = {
337 .f = cmd_consign_parsed, /* function to call */
338 .data = NULL, /* 2nd arg of func */
339 .help_str = help_consign,
340 .tokens = { /* token list, NULL terminated */
341 (prog_void *)&cmd_consign_arg0,
342 (prog_void *)&cmd_csb_name_tok,
343 (prog_void *)&cmd_consign_p,
349 /**********************************************************/
350 /* Maximums for control system */
352 /* this structure is filled when cmd_maximum is parsed successfully */
353 struct cmd_maximum_result {
354 struct cmd_cs_result cs;
360 /* function called when cmd_maximum is parsed successfully */
361 static void cmd_maximum_parsed(void *parsed_result, void *show)
363 struct cmd_maximum_result * res = parsed_result;
365 struct cs_block *csb;
367 csb = cs_from_name(res->cs.csname);
369 printf_P(PSTR("null csb\r\n"));
374 pid_set_maximums(&csb->pid, res->in, res->i, res->out);
376 printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
378 pid_get_max_in(&csb->pid),
379 pid_get_max_I(&csb->pid),
380 pid_get_max_out(&csb->pid));
383 prog_char str_maximum_arg0[] = "maximum";
384 parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
385 parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
386 parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
387 parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
389 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
390 parse_pgm_inst_t cmd_maximum = {
391 .f = cmd_maximum_parsed, /* function to call */
392 .data = NULL, /* 2nd arg of func */
393 .help_str = help_maximum,
394 .tokens = { /* token list, NULL terminated */
395 (prog_void *)&cmd_maximum_arg0,
396 (prog_void *)&cmd_csb_name_tok,
397 (prog_void *)&cmd_maximum_in,
398 (prog_void *)&cmd_maximum_i,
399 (prog_void *)&cmd_maximum_out,
406 /* this structure is filled when cmd_maximum is parsed successfully */
407 struct cmd_maximum_show_result {
408 struct cmd_cs_result cs;
411 prog_char str_maximum_show_arg[] = "show";
412 parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
414 prog_char help_maximum_show[] = "Show maximum values for PID";
415 parse_pgm_inst_t cmd_maximum_show = {
416 .f = cmd_maximum_parsed, /* function to call */
417 .data = (void *)1, /* 2nd arg of func */
418 .help_str = help_maximum_show,
419 .tokens = { /* token list, NULL terminated */
420 (prog_void *)&cmd_maximum_arg0,
421 (prog_void *)&cmd_csb_name_tok,
422 (prog_void *)&cmd_maximum_show_arg,
427 /**********************************************************/
428 /* Quadramp for control system */
430 /* this structure is filled when cmd_quadramp is parsed successfully */
431 struct cmd_quadramp_result {
432 struct cmd_cs_result cs;
439 /* function called when cmd_quadramp is parsed successfully */
440 static void cmd_quadramp_parsed(void *parsed_result, void *show)
442 struct cmd_quadramp_result * res = parsed_result;
444 struct cs_block *csb;
446 csb = cs_from_name(res->cs.csname);
448 printf_P(PSTR("null csb\r\n"));
453 quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
454 quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
457 printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"),
459 csb->qr.var_2nd_ord_pos,
460 csb->qr.var_2nd_ord_neg,
461 csb->qr.var_1st_ord_pos,
462 csb->qr.var_1st_ord_neg);
465 prog_char str_quadramp_arg0[] = "quadramp";
466 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
467 parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT);
468 parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT);
469 parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT);
470 parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT);
472 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
473 parse_pgm_inst_t cmd_quadramp = {
474 .f = cmd_quadramp_parsed, /* function to call */
475 .data = NULL, /* 2nd arg of func */
476 .help_str = help_quadramp,
477 .tokens = { /* token list, NULL terminated */
478 (prog_void *)&cmd_quadramp_arg0,
479 (prog_void *)&cmd_csb_name_tok,
480 (prog_void *)&cmd_quadramp_ap,
481 (prog_void *)&cmd_quadramp_an,
482 (prog_void *)&cmd_quadramp_sp,
483 (prog_void *)&cmd_quadramp_sn,
491 struct cmd_quadramp_show_result {
492 struct cmd_cs_result cs;
496 prog_char str_quadramp_show_arg[] = "show";
497 parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
499 prog_char help_quadramp_show[] = "Get quadramp values for control system";
500 parse_pgm_inst_t cmd_quadramp_show = {
501 .f = cmd_quadramp_parsed, /* function to call */
502 .data = (void *)1, /* 2nd arg of func */
503 .help_str = help_quadramp_show,
504 .tokens = { /* token list, NULL terminated */
505 (prog_void *)&cmd_quadramp_arg0,
506 (prog_void *)&cmd_csb_name_tok,
507 (prog_void *)&cmd_quadramp_show_arg,
514 /**********************************************************/
515 /* cs_status show for control system */
517 /* this structure is filled when cmd_cs_status is parsed successfully */
518 struct cmd_cs_status_result {
519 struct cmd_cs_result cs;
523 /* function called when cmd_cs_status is parsed successfully */
524 static void cmd_cs_status_parsed(void *parsed_result, void *data)
526 struct cmd_cs_status_result *res = parsed_result;
527 struct cs_block *csb;
529 uint8_t print_pid = 0, print_cs = 0;
531 csb = cs_from_name(res->cs.csname);
533 printf_P(PSTR("null csb\r\n"));
536 if (strcmp_P(res->arg, PSTR("on")) == 0) {
538 printf_P(PSTR("%s is on\r\n"), res->cs.csname);
541 else if (strcmp_P(res->arg, PSTR("off")) == 0) {
543 printf_P(PSTR("%s is off\r\n"), res->cs.csname);
546 else if (strcmp_P(res->arg, PSTR("show")) == 0) {
549 else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
553 else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
556 else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
561 printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
564 dump_cs(res->cs.csname, &csb->cs);
566 dump_pid(res->cs.csname, &csb->pid);
568 } while(loop && !cmdline_keypressed());
571 prog_char str_cs_status_arg0[] = "cs_status";
572 parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
573 prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
574 parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
576 prog_char help_cs_status[] = "Show cs status";
577 parse_pgm_inst_t cmd_cs_status = {
578 .f = cmd_cs_status_parsed, /* function to call */
579 .data = NULL, /* 2nd arg of func */
580 .help_str = help_cs_status,
581 .tokens = { /* token list, NULL terminated */
582 (prog_void *)&cmd_cs_status_arg0,
583 (prog_void *)&cmd_csb_name_tok,
584 (prog_void *)&cmd_cs_status_arg,
590 /**********************************************************/
591 /* Blocking_I for control system */
593 /* this structure is filled when cmd_blocking_i is parsed successfully */
594 struct cmd_blocking_i_result {
595 struct cmd_cs_result cs;
602 /* function called when cmd_blocking_i is parsed successfully */
603 static void cmd_blocking_i_parsed(void *parsed_result, void *show)
605 struct cmd_blocking_i_result * res = parsed_result;
607 struct cs_block *csb;
609 csb = cs_from_name(res->cs.csname);
611 printf_P(PSTR("null csb\r\n"));
616 bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
619 printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
628 prog_char str_blocking_i_arg0[] = "blocking";
629 parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
630 parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
631 parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
632 parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
633 parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
635 prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
636 parse_pgm_inst_t cmd_blocking_i = {
637 .f = cmd_blocking_i_parsed, /* function to call */
638 .data = NULL, /* 2nd arg of func */
639 .help_str = help_blocking_i,
640 .tokens = { /* token list, NULL terminated */
641 (prog_void *)&cmd_blocking_i_arg0,
642 (prog_void *)&cmd_csb_name_tok,
643 (prog_void *)&cmd_blocking_i_k1,
644 (prog_void *)&cmd_blocking_i_k2,
645 (prog_void *)&cmd_blocking_i_i,
646 (prog_void *)&cmd_blocking_i_cpt,
653 struct cmd_blocking_i_show_result {
654 struct cmd_cs_result cs;
658 prog_char str_blocking_i_show_arg[] = "show";
659 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);
661 prog_char help_blocking_i_show[] = "Show blocking detection values";
662 parse_pgm_inst_t cmd_blocking_i_show = {
663 .f = cmd_blocking_i_parsed, /* function to call */
664 .data = (void *)1, /* 2nd arg of func */
665 .help_str = help_blocking_i_show,
666 .tokens = { /* token list, NULL terminated */
667 (prog_void *)&cmd_blocking_i_arg0,
668 (prog_void *)&cmd_csb_name_tok,
669 (prog_void *)&cmd_blocking_i_show_arg,