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>
36 #include <control_system_manager.h>
37 #include <trajectory_manager.h>
38 #include <blocking_detection_manager.h>
39 #include <robot_system.h>
40 #include <position_manager.h>
44 #include <parse_string.h>
45 #include <parse_num.h>
52 const prog_char *name;
56 prog_char csb_angle_str[] = "angle";
57 prog_char csb_distance_str[] = "distance";
58 prog_char csb_fessor_str[] = "fessor";
59 prog_char csb_wheel_str[] = "wheel";
60 prog_char csb_elevator_str[] = "elevator";
62 struct csb_list csb_list[] = {
63 { .name = csb_angle_str, .csb = &mainboard.angle },
64 { .name = csb_distance_str, .csb = &mainboard.distance },
65 { .name = csb_fessor_str, .csb = &mainboard.fessor },
66 { .name = csb_wheel_str, .csb = &mainboard.wheel },
67 { .name = csb_elevator_str, .csb = &mainboard.elevator },
70 struct cmd_cs_result {
71 fixed_string_t cmdname;
72 fixed_string_t csname;
75 /* token to be used for all cs-related commands */
76 prog_char str_csb_name[] = "angle#distance#fessor#wheel#elevator";
77 parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
79 struct cs_block *cs_from_name(const char *name)
83 for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
84 if (!strcmp_P(name, csb_list[i].name))
85 return csb_list[i].csb;
90 /**********************************************************/
91 /* Gains for control system */
93 /* this structure is filled when cmd_gain is parsed successfully */
94 struct cmd_gain_result {
95 struct cmd_cs_result cs;
101 /* function called when cmd_gain is parsed successfully */
102 static void cmd_gain_parsed(void * parsed_result, void *show)
104 struct cmd_gain_result *res = parsed_result;
105 struct cs_block *csb;
107 csb = cs_from_name(res->cs.csname);
109 printf_P(PSTR("null csb\r\n"));
114 pid_set_gains(&csb->pid, res->p, res->i, res->d);
116 printf_P(PSTR("%s %s %d %d %d\r\n"),
119 pid_get_gain_P(&csb->pid),
120 pid_get_gain_I(&csb->pid),
121 pid_get_gain_D(&csb->pid));
124 prog_char str_gain_arg0[] = "gain";
125 parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
126 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
127 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
128 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
130 prog_char help_gain[] = "Set gain values for PID";
131 parse_pgm_inst_t cmd_gain = {
132 .f = cmd_gain_parsed, /* function to call */
133 .data = NULL, /* 2nd arg of func */
134 .help_str = help_gain,
135 .tokens = { /* token list, NULL terminated */
136 (prog_void *)&cmd_gain_arg0,
137 (prog_void *)&cmd_csb_name_tok,
138 (prog_void *)&cmd_gain_p,
139 (prog_void *)&cmd_gain_i,
140 (prog_void *)&cmd_gain_d,
146 /* this structure is filled when cmd_gain is parsed successfully */
147 struct cmd_gain_show_result {
148 struct cmd_cs_result cs;
152 prog_char str_gain_show_arg[] = "show";
153 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
155 prog_char help_gain_show[] = "Show gain values for PID";
156 parse_pgm_inst_t cmd_gain_show = {
157 .f = cmd_gain_parsed, /* function to call */
158 .data = (void *)1, /* 2nd arg of func */
159 .help_str = help_gain_show,
160 .tokens = { /* token list, NULL terminated */
161 (prog_void *)&cmd_gain_arg0,
162 (prog_void *)&cmd_csb_name_tok,
163 (prog_void *)&cmd_gain_show_arg,
168 /**********************************************************/
169 /* Speeds for control system */
171 /* this structure is filled when cmd_speed is parsed successfully */
172 struct cmd_speed_result {
173 struct cmd_cs_result cs;
177 /* function called when cmd_speed is parsed successfully */
178 static void cmd_speed_parsed(void *parsed_result, void *show)
180 struct cmd_speed_result * res = parsed_result;
182 struct cs_block *csb;
184 csb = cs_from_name(res->cs.csname);
186 printf_P(PSTR("null csb\r\n"));
192 ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
194 printf_P(PSTR("%s %lu\r\n"),
198 printf_P(PSTR("TODO\r\n"));
202 prog_char str_speed_arg0[] = "speed";
203 parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
204 parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
206 prog_char help_speed[] = "Set speed values for ramp filter";
207 parse_pgm_inst_t cmd_speed = {
208 .f = cmd_speed_parsed, /* function to call */
209 .data = NULL, /* 2nd arg of func */
210 .help_str = help_speed,
211 .tokens = { /* token list, NULL terminated */
212 (prog_void *)&cmd_speed_arg0,
213 (prog_void *)&cmd_csb_name_tok,
214 (prog_void *)&cmd_speed_s,
220 struct cmd_speed_show_result {
221 struct cmd_cs_result cs;
225 prog_char str_speed_show_arg[] = "show";
226 parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
228 prog_char help_speed_show[] = "Show speed values for ramp filter";
229 parse_pgm_inst_t cmd_speed_show = {
230 .f = cmd_speed_parsed, /* function to call */
231 .data = (void *)1, /* 2nd arg of func */
232 .help_str = help_speed_show,
233 .tokens = { /* token list, NULL terminated */
234 (prog_void *)&cmd_speed_arg0,
235 (prog_void *)&cmd_csb_name_tok,
236 (prog_void *)&cmd_speed_show_arg,
241 /**********************************************************/
242 /* Derivate_Filters for control system */
244 /* this structure is filled when cmd_derivate_filter is parsed successfully */
245 struct cmd_derivate_filter_result {
246 struct cmd_cs_result cs;
250 /* function called when cmd_derivate_filter is parsed successfully */
251 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
253 struct cmd_derivate_filter_result * res = parsed_result;
254 struct cs_block *csb;
256 csb = cs_from_name(res->cs.csname);
258 printf_P(PSTR("null csb\r\n"));
263 pid_set_derivate_filter(&csb->pid, res->size);
265 printf_P(PSTR("%s %s %u\r\n"),
268 pid_get_derivate_filter(&csb->pid));
271 prog_char str_derivate_filter_arg0[] = "derivate_filter";
272 parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
273 parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
275 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
276 parse_pgm_inst_t cmd_derivate_filter = {
277 .f = cmd_derivate_filter_parsed, /* function to call */
278 .data = (void *)1, /* 2nd arg of func */
279 .help_str = help_derivate_filter,
280 .tokens = { /* token list, NULL terminated */
281 (prog_void *)&cmd_derivate_filter_arg0,
282 (prog_void *)&cmd_csb_name_tok,
283 (prog_void *)&cmd_derivate_filter_size,
290 struct cmd_derivate_filter_show_result {
291 struct cmd_cs_result cs;
295 prog_char str_derivate_filter_show_arg[] = "show";
296 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);
298 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
299 parse_pgm_inst_t cmd_derivate_filter_show = {
300 .f = cmd_derivate_filter_parsed, /* function to call */
301 .data = NULL, /* 2nd arg of func */
302 .help_str = help_derivate_filter_show,
303 .tokens = { /* token list, NULL terminated */
304 (prog_void *)&cmd_derivate_filter_arg0,
305 (prog_void *)&cmd_csb_name_tok,
306 (prog_void *)&cmd_derivate_filter_show_arg,
312 /**********************************************************/
313 /* Consign for control system */
315 /* this structure is filled when cmd_consign is parsed successfully */
316 struct cmd_consign_result {
317 struct cmd_cs_result cs;
321 /* function called when cmd_consign is parsed successfully */
322 static void cmd_consign_parsed(void * parsed_result, void *data)
324 struct cmd_consign_result * res = parsed_result;
325 struct cs_block *csb;
327 csb = cs_from_name(res->cs.csname);
329 printf_P(PSTR("null csb\r\n"));
333 cs_set_consign(&csb->cs, res->p);
336 prog_char str_consign_arg0[] = "consign";
337 parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
338 parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
340 prog_char help_consign[] = "Set consign value";
341 parse_pgm_inst_t cmd_consign = {
342 .f = cmd_consign_parsed, /* function to call */
343 .data = NULL, /* 2nd arg of func */
344 .help_str = help_consign,
345 .tokens = { /* token list, NULL terminated */
346 (prog_void *)&cmd_consign_arg0,
347 (prog_void *)&cmd_csb_name_tok,
348 (prog_void *)&cmd_consign_p,
354 /**********************************************************/
355 /* Maximums for control system */
357 /* this structure is filled when cmd_maximum is parsed successfully */
358 struct cmd_maximum_result {
359 struct cmd_cs_result cs;
365 /* function called when cmd_maximum is parsed successfully */
366 static void cmd_maximum_parsed(void *parsed_result, void *show)
368 struct cmd_maximum_result * res = parsed_result;
370 struct cs_block *csb;
372 csb = cs_from_name(res->cs.csname);
374 printf_P(PSTR("null csb\r\n"));
379 pid_set_maximums(&csb->pid, res->in, res->i, res->out);
381 printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
383 pid_get_max_in(&csb->pid),
384 pid_get_max_I(&csb->pid),
385 pid_get_max_out(&csb->pid));
388 prog_char str_maximum_arg0[] = "maximum";
389 parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
390 parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
391 parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
392 parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
394 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
395 parse_pgm_inst_t cmd_maximum = {
396 .f = cmd_maximum_parsed, /* function to call */
397 .data = NULL, /* 2nd arg of func */
398 .help_str = help_maximum,
399 .tokens = { /* token list, NULL terminated */
400 (prog_void *)&cmd_maximum_arg0,
401 (prog_void *)&cmd_csb_name_tok,
402 (prog_void *)&cmd_maximum_in,
403 (prog_void *)&cmd_maximum_i,
404 (prog_void *)&cmd_maximum_out,
411 /* this structure is filled when cmd_maximum is parsed successfully */
412 struct cmd_maximum_show_result {
413 struct cmd_cs_result cs;
416 prog_char str_maximum_show_arg[] = "show";
417 parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
419 prog_char help_maximum_show[] = "Show maximum values for PID";
420 parse_pgm_inst_t cmd_maximum_show = {
421 .f = cmd_maximum_parsed, /* function to call */
422 .data = (void *)1, /* 2nd arg of func */
423 .help_str = help_maximum_show,
424 .tokens = { /* token list, NULL terminated */
425 (prog_void *)&cmd_maximum_arg0,
426 (prog_void *)&cmd_csb_name_tok,
427 (prog_void *)&cmd_maximum_show_arg,
432 /**********************************************************/
433 /* Quadramp for control system */
435 /* this structure is filled when cmd_quadramp is parsed successfully */
436 struct cmd_quadramp_result {
437 struct cmd_cs_result cs;
444 /* function called when cmd_quadramp is parsed successfully */
445 static void cmd_quadramp_parsed(void *parsed_result, void *show)
447 struct cmd_quadramp_result * res = parsed_result;
449 struct cs_block *csb;
451 csb = cs_from_name(res->cs.csname);
453 printf_P(PSTR("null csb\r\n"));
458 quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
459 quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
462 printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
464 csb->qr.var_2nd_ord_pos,
465 csb->qr.var_2nd_ord_neg,
466 csb->qr.var_1st_ord_pos,
467 csb->qr.var_1st_ord_neg);
470 prog_char str_quadramp_arg0[] = "quadramp";
471 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
472 parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, UINT32);
473 parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, UINT32);
474 parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, UINT32);
475 parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, UINT32);
477 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
478 parse_pgm_inst_t cmd_quadramp = {
479 .f = cmd_quadramp_parsed, /* function to call */
480 .data = NULL, /* 2nd arg of func */
481 .help_str = help_quadramp,
482 .tokens = { /* token list, NULL terminated */
483 (prog_void *)&cmd_quadramp_arg0,
484 (prog_void *)&cmd_csb_name_tok,
485 (prog_void *)&cmd_quadramp_ap,
486 (prog_void *)&cmd_quadramp_an,
487 (prog_void *)&cmd_quadramp_sp,
488 (prog_void *)&cmd_quadramp_sn,
496 struct cmd_quadramp_show_result {
497 struct cmd_cs_result cs;
501 prog_char str_quadramp_show_arg[] = "show";
502 parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
504 prog_char help_quadramp_show[] = "Get quadramp values for control system";
505 parse_pgm_inst_t cmd_quadramp_show = {
506 .f = cmd_quadramp_parsed, /* function to call */
507 .data = (void *)1, /* 2nd arg of func */
508 .help_str = help_quadramp_show,
509 .tokens = { /* token list, NULL terminated */
510 (prog_void *)&cmd_quadramp_arg0,
511 (prog_void *)&cmd_csb_name_tok,
512 (prog_void *)&cmd_quadramp_show_arg,
519 /**********************************************************/
520 /* cs_status show for control system */
522 /* this structure is filled when cmd_cs_status is parsed successfully */
523 struct cmd_cs_status_result {
524 struct cmd_cs_result cs;
528 /* function called when cmd_cs_status is parsed successfully */
529 static void cmd_cs_status_parsed(void *parsed_result, void *data)
531 struct cmd_cs_status_result *res = parsed_result;
532 struct cs_block *csb;
534 uint8_t print_pid = 0, print_cs = 0;
536 csb = cs_from_name(res->cs.csname);
538 printf_P(PSTR("null csb\r\n"));
541 if (strcmp_P(res->arg, PSTR("on")) == 0) {
543 printf_P(PSTR("%s is on\r\n"), res->cs.csname);
546 else if (strcmp_P(res->arg, PSTR("off")) == 0) {
548 printf_P(PSTR("%s is off\r\n"), res->cs.csname);
551 else if (strcmp_P(res->arg, PSTR("show")) == 0) {
554 else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
558 else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
561 else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
566 printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
569 dump_cs(res->cs.csname, &csb->cs);
571 dump_pid(res->cs.csname, &csb->pid);
573 } while(loop && !cmdline_keypressed());
576 prog_char str_cs_status_arg0[] = "cs_status";
577 parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
578 prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
579 parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
581 prog_char help_cs_status[] = "Show cs status";
582 parse_pgm_inst_t cmd_cs_status = {
583 .f = cmd_cs_status_parsed, /* function to call */
584 .data = NULL, /* 2nd arg of func */
585 .help_str = help_cs_status,
586 .tokens = { /* token list, NULL terminated */
587 (prog_void *)&cmd_cs_status_arg0,
588 (prog_void *)&cmd_csb_name_tok,
589 (prog_void *)&cmd_cs_status_arg,
595 /**********************************************************/
596 /* Blocking_I for control system */
598 /* this structure is filled when cmd_blocking_i is parsed successfully */
599 struct cmd_blocking_i_result {
600 struct cmd_cs_result cs;
607 /* function called when cmd_blocking_i is parsed successfully */
608 static void cmd_blocking_i_parsed(void *parsed_result, void *show)
610 struct cmd_blocking_i_result * res = parsed_result;
612 struct cs_block *csb;
614 csb = cs_from_name(res->cs.csname);
616 printf_P(PSTR("null csb\r\n"));
621 bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
624 printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
633 prog_char str_blocking_i_arg0[] = "blocking";
634 parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
635 parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
636 parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
637 parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
638 parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
640 prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
641 parse_pgm_inst_t cmd_blocking_i = {
642 .f = cmd_blocking_i_parsed, /* function to call */
643 .data = NULL, /* 2nd arg of func */
644 .help_str = help_blocking_i,
645 .tokens = { /* token list, NULL terminated */
646 (prog_void *)&cmd_blocking_i_arg0,
647 (prog_void *)&cmd_csb_name_tok,
648 (prog_void *)&cmd_blocking_i_k1,
649 (prog_void *)&cmd_blocking_i_k2,
650 (prog_void *)&cmd_blocking_i_i,
651 (prog_void *)&cmd_blocking_i_cpt,
658 struct cmd_blocking_i_show_result {
659 struct cmd_cs_result cs;
663 prog_char str_blocking_i_show_arg[] = "show";
664 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);
666 prog_char help_blocking_i_show[] = "Show blocking detection values";
667 parse_pgm_inst_t cmd_blocking_i_show = {
668 .f = cmd_blocking_i_parsed, /* function to call */
669 .data = (void *)1, /* 2nd arg of func */
670 .help_str = help_blocking_i_show,
671 .tokens = { /* token list, NULL terminated */
672 (prog_void *)&cmd_blocking_i_arg0,
673 (prog_void *)&cmd_csb_name_tok,
674 (prog_void *)&cmd_blocking_i_show_arg,