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.4 2009-05-02 10:08:09 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 <trajectory_manager.h>
39 #include <vect_base.h>
42 #include <obstacle_avoidance.h>
43 #include <blocking_detection_manager.h>
44 #include <robot_system.h>
45 #include <position_manager.h>
49 #include <parse_string.h>
50 #include <parse_num.h>
57 const prog_char *name;
61 prog_char csb_angle_str[] = "angle";
62 prog_char csb_distance_str[] = "distance";
63 struct csb_list csb_list[] = {
64 { .name = csb_angle_str, .csb = &mainboard.angle },
65 { .name = csb_distance_str, .csb = &mainboard.distance },
68 struct cmd_cs_result {
69 fixed_string_t cmdname;
70 fixed_string_t csname;
73 /* token to be used for all cs-related commands */
74 prog_char str_csb_name[] = "angle#distance";
75 parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
77 struct cs_block *cs_from_name(const char *name)
81 for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
82 if (!strcmp_P(name, csb_list[i].name))
83 return csb_list[i].csb;
88 /**********************************************************/
89 /* Gains for control system */
91 /* this structure is filled when cmd_gain is parsed successfully */
92 struct cmd_gain_result {
93 struct cmd_cs_result cs;
99 /* function called when cmd_gain is parsed successfully */
100 static void cmd_gain_parsed(void * parsed_result, void *show)
102 struct cmd_gain_result *res = parsed_result;
103 struct cs_block *csb;
105 csb = cs_from_name(res->cs.csname);
107 printf_P(PSTR("null csb\r\n"));
112 pid_set_gains(&csb->pid, res->p, res->i, res->d);
114 printf_P(PSTR("%s %s %d %d %d\r\n"),
117 pid_get_gain_P(&csb->pid),
118 pid_get_gain_I(&csb->pid),
119 pid_get_gain_D(&csb->pid));
122 prog_char str_gain_arg0[] = "gain";
123 parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
124 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
125 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
126 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
128 prog_char help_gain[] = "Set gain values for PID";
129 parse_pgm_inst_t cmd_gain = {
130 .f = cmd_gain_parsed, /* function to call */
131 .data = NULL, /* 2nd arg of func */
132 .help_str = help_gain,
133 .tokens = { /* token list, NULL terminated */
134 (prog_void *)&cmd_gain_arg0,
135 (prog_void *)&cmd_csb_name_tok,
136 (prog_void *)&cmd_gain_p,
137 (prog_void *)&cmd_gain_i,
138 (prog_void *)&cmd_gain_d,
144 /* this structure is filled when cmd_gain is parsed successfully */
145 struct cmd_gain_show_result {
146 struct cmd_cs_result cs;
150 prog_char str_gain_show_arg[] = "show";
151 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
153 prog_char help_gain_show[] = "Show gain values for PID";
154 parse_pgm_inst_t cmd_gain_show = {
155 .f = cmd_gain_parsed, /* function to call */
156 .data = (void *)1, /* 2nd arg of func */
157 .help_str = help_gain_show,
158 .tokens = { /* token list, NULL terminated */
159 (prog_void *)&cmd_gain_arg0,
160 (prog_void *)&cmd_csb_name_tok,
161 (prog_void *)&cmd_gain_show_arg,
166 /**********************************************************/
167 /* Speeds for control system */
169 /* this structure is filled when cmd_speed is parsed successfully */
170 struct cmd_speed_result {
171 struct cmd_cs_result cs;
175 /* function called when cmd_speed is parsed successfully */
176 static void cmd_speed_parsed(void *parsed_result, void *show)
178 struct cmd_speed_result * res = parsed_result;
180 struct cs_block *csb;
182 csb = cs_from_name(res->cs.csname);
184 printf_P(PSTR("null csb\r\n"));
190 ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
192 printf_P(PSTR("%s %lu\r\n"),
196 printf_P(PSTR("TODO\r\n"));
200 prog_char str_speed_arg0[] = "speed";
201 parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
202 parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
204 prog_char help_speed[] = "Set speed values for ramp filter";
205 parse_pgm_inst_t cmd_speed = {
206 .f = cmd_speed_parsed, /* function to call */
207 .data = NULL, /* 2nd arg of func */
208 .help_str = help_speed,
209 .tokens = { /* token list, NULL terminated */
210 (prog_void *)&cmd_speed_arg0,
211 (prog_void *)&cmd_csb_name_tok,
212 (prog_void *)&cmd_speed_s,
218 struct cmd_speed_show_result {
219 struct cmd_cs_result cs;
223 prog_char str_speed_show_arg[] = "show";
224 parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
226 prog_char help_speed_show[] = "Show speed values for ramp filter";
227 parse_pgm_inst_t cmd_speed_show = {
228 .f = cmd_speed_parsed, /* function to call */
229 .data = (void *)1, /* 2nd arg of func */
230 .help_str = help_speed_show,
231 .tokens = { /* token list, NULL terminated */
232 (prog_void *)&cmd_speed_arg0,
233 (prog_void *)&cmd_csb_name_tok,
234 (prog_void *)&cmd_speed_show_arg,
239 /**********************************************************/
240 /* Derivate_Filters for control system */
242 /* this structure is filled when cmd_derivate_filter is parsed successfully */
243 struct cmd_derivate_filter_result {
244 struct cmd_cs_result cs;
248 /* function called when cmd_derivate_filter is parsed successfully */
249 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
251 struct cmd_derivate_filter_result * res = parsed_result;
252 struct cs_block *csb;
254 csb = cs_from_name(res->cs.csname);
256 printf_P(PSTR("null csb\r\n"));
261 pid_set_derivate_filter(&csb->pid, res->size);
263 printf_P(PSTR("%s %s %u\r\n"),
266 pid_get_derivate_filter(&csb->pid));
269 prog_char str_derivate_filter_arg0[] = "derivate_filter";
270 parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
271 parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
273 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
274 parse_pgm_inst_t cmd_derivate_filter = {
275 .f = cmd_derivate_filter_parsed, /* function to call */
276 .data = (void *)1, /* 2nd arg of func */
277 .help_str = help_derivate_filter,
278 .tokens = { /* token list, NULL terminated */
279 (prog_void *)&cmd_derivate_filter_arg0,
280 (prog_void *)&cmd_csb_name_tok,
281 (prog_void *)&cmd_derivate_filter_size,
288 struct cmd_derivate_filter_show_result {
289 struct cmd_cs_result cs;
293 prog_char str_derivate_filter_show_arg[] = "show";
294 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);
296 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
297 parse_pgm_inst_t cmd_derivate_filter_show = {
298 .f = cmd_derivate_filter_parsed, /* function to call */
299 .data = NULL, /* 2nd arg of func */
300 .help_str = help_derivate_filter_show,
301 .tokens = { /* token list, NULL terminated */
302 (prog_void *)&cmd_derivate_filter_arg0,
303 (prog_void *)&cmd_csb_name_tok,
304 (prog_void *)&cmd_derivate_filter_show_arg,
310 /**********************************************************/
311 /* Consign for control system */
313 /* this structure is filled when cmd_consign is parsed successfully */
314 struct cmd_consign_result {
315 struct cmd_cs_result cs;
319 /* function called when cmd_consign is parsed successfully */
320 static void cmd_consign_parsed(void * parsed_result, void *data)
322 struct cmd_consign_result * res = parsed_result;
323 struct cs_block *csb;
325 csb = cs_from_name(res->cs.csname);
327 printf_P(PSTR("null csb\r\n"));
331 cs_set_consign(&csb->cs, res->p);
334 prog_char str_consign_arg0[] = "consign";
335 parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
336 parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
338 prog_char help_consign[] = "Set consign value";
339 parse_pgm_inst_t cmd_consign = {
340 .f = cmd_consign_parsed, /* function to call */
341 .data = NULL, /* 2nd arg of func */
342 .help_str = help_consign,
343 .tokens = { /* token list, NULL terminated */
344 (prog_void *)&cmd_consign_arg0,
345 (prog_void *)&cmd_csb_name_tok,
346 (prog_void *)&cmd_consign_p,
352 /**********************************************************/
353 /* Maximums for control system */
355 /* this structure is filled when cmd_maximum is parsed successfully */
356 struct cmd_maximum_result {
357 struct cmd_cs_result cs;
363 /* function called when cmd_maximum is parsed successfully */
364 static void cmd_maximum_parsed(void *parsed_result, void *show)
366 struct cmd_maximum_result * res = parsed_result;
368 struct cs_block *csb;
370 csb = cs_from_name(res->cs.csname);
372 printf_P(PSTR("null csb\r\n"));
377 pid_set_maximums(&csb->pid, res->in, res->i, res->out);
379 printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
381 pid_get_max_in(&csb->pid),
382 pid_get_max_I(&csb->pid),
383 pid_get_max_out(&csb->pid));
386 prog_char str_maximum_arg0[] = "maximum";
387 parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
388 parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
389 parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
390 parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
392 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
393 parse_pgm_inst_t cmd_maximum = {
394 .f = cmd_maximum_parsed, /* function to call */
395 .data = NULL, /* 2nd arg of func */
396 .help_str = help_maximum,
397 .tokens = { /* token list, NULL terminated */
398 (prog_void *)&cmd_maximum_arg0,
399 (prog_void *)&cmd_csb_name_tok,
400 (prog_void *)&cmd_maximum_in,
401 (prog_void *)&cmd_maximum_i,
402 (prog_void *)&cmd_maximum_out,
409 /* this structure is filled when cmd_maximum is parsed successfully */
410 struct cmd_maximum_show_result {
411 struct cmd_cs_result cs;
414 prog_char str_maximum_show_arg[] = "show";
415 parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
417 prog_char help_maximum_show[] = "Show maximum values for PID";
418 parse_pgm_inst_t cmd_maximum_show = {
419 .f = cmd_maximum_parsed, /* function to call */
420 .data = (void *)1, /* 2nd arg of func */
421 .help_str = help_maximum_show,
422 .tokens = { /* token list, NULL terminated */
423 (prog_void *)&cmd_maximum_arg0,
424 (prog_void *)&cmd_csb_name_tok,
425 (prog_void *)&cmd_maximum_show_arg,
430 /**********************************************************/
431 /* Quadramp for control system */
433 /* this structure is filled when cmd_quadramp is parsed successfully */
434 struct cmd_quadramp_result {
435 struct cmd_cs_result cs;
442 /* function called when cmd_quadramp is parsed successfully */
443 static void cmd_quadramp_parsed(void *parsed_result, void *show)
445 struct cmd_quadramp_result * res = parsed_result;
447 struct cs_block *csb;
449 csb = cs_from_name(res->cs.csname);
451 printf_P(PSTR("null csb\r\n"));
456 quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
457 quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
460 printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
462 csb->qr.var_2nd_ord_pos,
463 csb->qr.var_2nd_ord_neg,
464 csb->qr.var_1st_ord_pos,
465 csb->qr.var_1st_ord_neg);
468 prog_char str_quadramp_arg0[] = "quadramp";
469 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
470 parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
471 parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
472 parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
473 parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
475 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
476 parse_pgm_inst_t cmd_quadramp = {
477 .f = cmd_quadramp_parsed, /* function to call */
478 .data = NULL, /* 2nd arg of func */
479 .help_str = help_quadramp,
480 .tokens = { /* token list, NULL terminated */
481 (prog_void *)&cmd_quadramp_arg0,
482 (prog_void *)&cmd_csb_name_tok,
483 (prog_void *)&cmd_quadramp_ap,
484 (prog_void *)&cmd_quadramp_an,
485 (prog_void *)&cmd_quadramp_sp,
486 (prog_void *)&cmd_quadramp_sn,
494 struct cmd_quadramp_show_result {
495 struct cmd_cs_result cs;
499 prog_char str_quadramp_show_arg[] = "show";
500 parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
502 prog_char help_quadramp_show[] = "Get quadramp values for control system";
503 parse_pgm_inst_t cmd_quadramp_show = {
504 .f = cmd_quadramp_parsed, /* function to call */
505 .data = (void *)1, /* 2nd arg of func */
506 .help_str = help_quadramp_show,
507 .tokens = { /* token list, NULL terminated */
508 (prog_void *)&cmd_quadramp_arg0,
509 (prog_void *)&cmd_csb_name_tok,
510 (prog_void *)&cmd_quadramp_show_arg,
517 /**********************************************************/
518 /* cs_status show for control system */
520 /* this structure is filled when cmd_cs_status is parsed successfully */
521 struct cmd_cs_status_result {
522 struct cmd_cs_result cs;
526 /* function called when cmd_cs_status is parsed successfully */
527 static void cmd_cs_status_parsed(void *parsed_result, void *data)
529 struct cmd_cs_status_result *res = parsed_result;
530 struct cs_block *csb;
532 uint8_t print_pid = 0, print_cs = 0;
534 csb = cs_from_name(res->cs.csname);
536 printf_P(PSTR("null csb\r\n"));
539 if (strcmp_P(res->arg, PSTR("on")) == 0) {
541 printf_P(PSTR("%s is on\r\n"), res->cs.csname);
544 else if (strcmp_P(res->arg, PSTR("off")) == 0) {
546 printf_P(PSTR("%s is off\r\n"), res->cs.csname);
549 else if (strcmp_P(res->arg, PSTR("show")) == 0) {
552 else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
556 else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
559 else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
564 printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
567 dump_cs(res->cs.csname, &csb->cs);
569 dump_pid(res->cs.csname, &csb->pid);
571 } while(loop && !cmdline_keypressed());
574 prog_char str_cs_status_arg0[] = "cs_status";
575 parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
576 prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
577 parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
579 prog_char help_cs_status[] = "Show cs status";
580 parse_pgm_inst_t cmd_cs_status = {
581 .f = cmd_cs_status_parsed, /* function to call */
582 .data = NULL, /* 2nd arg of func */
583 .help_str = help_cs_status,
584 .tokens = { /* token list, NULL terminated */
585 (prog_void *)&cmd_cs_status_arg0,
586 (prog_void *)&cmd_csb_name_tok,
587 (prog_void *)&cmd_cs_status_arg,
593 /**********************************************************/
594 /* Blocking_I for control system */
596 /* this structure is filled when cmd_blocking_i is parsed successfully */
597 struct cmd_blocking_i_result {
598 struct cmd_cs_result cs;
605 /* function called when cmd_blocking_i is parsed successfully */
606 static void cmd_blocking_i_parsed(void *parsed_result, void *show)
608 struct cmd_blocking_i_result * res = parsed_result;
610 struct cs_block *csb;
612 csb = cs_from_name(res->cs.csname);
614 printf_P(PSTR("null csb\r\n"));
619 bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
622 printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
631 prog_char str_blocking_i_arg0[] = "blocking";
632 parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
633 parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
634 parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
635 parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
636 parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
638 prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
639 parse_pgm_inst_t cmd_blocking_i = {
640 .f = cmd_blocking_i_parsed, /* function to call */
641 .data = NULL, /* 2nd arg of func */
642 .help_str = help_blocking_i,
643 .tokens = { /* token list, NULL terminated */
644 (prog_void *)&cmd_blocking_i_arg0,
645 (prog_void *)&cmd_csb_name_tok,
646 (prog_void *)&cmd_blocking_i_k1,
647 (prog_void *)&cmd_blocking_i_k2,
648 (prog_void *)&cmd_blocking_i_i,
649 (prog_void *)&cmd_blocking_i_cpt,
656 struct cmd_blocking_i_show_result {
657 struct cmd_cs_result cs;
661 prog_char str_blocking_i_show_arg[] = "show";
662 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);
664 prog_char help_blocking_i_show[] = "Show blocking detection values";
665 parse_pgm_inst_t cmd_blocking_i_show = {
666 .f = cmd_blocking_i_parsed, /* function to call */
667 .data = (void *)1, /* 2nd arg of func */
668 .help_str = help_blocking_i_show,
669 .tokens = { /* token list, NULL terminated */
670 (prog_void *)&cmd_blocking_i_arg0,
671 (prog_void *)&cmd_csb_name_tok,
672 (prog_void *)&cmd_blocking_i_show_arg,