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 struct csb_list csb_list[] = {
58 { .name = csb_roller_str, .csb = &ballboard.roller },
59 { .name = csb_forktrans_str, .csb = &ballboard.forktrans },
60 { .name = csb_forkrot_str, .csb = &ballboard.forkrot },
63 struct cmd_cs_result {
64 fixed_string_t cmdname;
65 fixed_string_t csname;
68 /* token to be used for all cs-related commands */
69 prog_char str_csb_name[] = "roller#forktrans#forkrot";
70 parse_pgm_token_string_t cmd_csb_name_tok = TOKEN_STRING_INITIALIZER(struct cmd_cs_result, csname, str_csb_name);
72 struct cs_block *cs_from_name(const char *name)
76 for (i=0; i<(sizeof(csb_list)/sizeof(*csb_list)); i++) {
77 if (!strcmp_P(name, csb_list[i].name))
78 return csb_list[i].csb;
83 /**********************************************************/
84 /* Gains for control system */
86 /* this structure is filled when cmd_gain is parsed successfully */
87 struct cmd_gain_result {
88 struct cmd_cs_result cs;
94 /* function called when cmd_gain is parsed successfully */
95 static void cmd_gain_parsed(void * parsed_result, void *show)
97 struct cmd_gain_result *res = parsed_result;
100 csb = cs_from_name(res->cs.csname);
102 printf_P(PSTR("null csb\r\n"));
107 pid_set_gains(&csb->pid, res->p, res->i, res->d);
109 printf_P(PSTR("%s %s %d %d %d\r\n"),
112 pid_get_gain_P(&csb->pid),
113 pid_get_gain_I(&csb->pid),
114 pid_get_gain_D(&csb->pid));
117 prog_char str_gain_arg0[] = "gain";
118 parse_pgm_token_string_t cmd_gain_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_gain_result, cs.cmdname, str_gain_arg0);
119 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
120 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
121 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
123 prog_char help_gain[] = "Set gain values for PID";
124 parse_pgm_inst_t cmd_gain = {
125 .f = cmd_gain_parsed, /* function to call */
126 .data = NULL, /* 2nd arg of func */
127 .help_str = help_gain,
128 .tokens = { /* token list, NULL terminated */
129 (prog_void *)&cmd_gain_arg0,
130 (prog_void *)&cmd_csb_name_tok,
131 (prog_void *)&cmd_gain_p,
132 (prog_void *)&cmd_gain_i,
133 (prog_void *)&cmd_gain_d,
139 /* this structure is filled when cmd_gain is parsed successfully */
140 struct cmd_gain_show_result {
141 struct cmd_cs_result cs;
145 prog_char str_gain_show_arg[] = "show";
146 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
148 prog_char help_gain_show[] = "Show gain values for PID";
149 parse_pgm_inst_t cmd_gain_show = {
150 .f = cmd_gain_parsed, /* function to call */
151 .data = (void *)1, /* 2nd arg of func */
152 .help_str = help_gain_show,
153 .tokens = { /* token list, NULL terminated */
154 (prog_void *)&cmd_gain_arg0,
155 (prog_void *)&cmd_csb_name_tok,
156 (prog_void *)&cmd_gain_show_arg,
161 /**********************************************************/
162 /* Speeds for control system */
164 /* this structure is filled when cmd_speed is parsed successfully */
165 struct cmd_speed_result {
166 struct cmd_cs_result cs;
170 /* function called when cmd_speed is parsed successfully */
171 static void cmd_speed_parsed(void *parsed_result, void *show)
173 struct cmd_speed_result * res = parsed_result;
175 struct cs_block *csb;
177 csb = cs_from_name(res->cs.csname);
179 printf_P(PSTR("null csb\r\n"));
185 ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
187 printf_P(PSTR("%s %lu\r\n"),
191 printf_P(PSTR("TODO\r\n"));
195 prog_char str_speed_arg0[] = "speed";
196 parse_pgm_token_string_t cmd_speed_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_speed_result, cs.cmdname, str_speed_arg0);
197 parse_pgm_token_num_t cmd_speed_s = TOKEN_NUM_INITIALIZER(struct cmd_speed_result, s, UINT16);
199 prog_char help_speed[] = "Set speed values for ramp filter";
200 parse_pgm_inst_t cmd_speed = {
201 .f = cmd_speed_parsed, /* function to call */
202 .data = NULL, /* 2nd arg of func */
203 .help_str = help_speed,
204 .tokens = { /* token list, NULL terminated */
205 (prog_void *)&cmd_speed_arg0,
206 (prog_void *)&cmd_csb_name_tok,
207 (prog_void *)&cmd_speed_s,
213 struct cmd_speed_show_result {
214 struct cmd_cs_result cs;
218 prog_char str_speed_show_arg[] = "show";
219 parse_pgm_token_string_t cmd_speed_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_speed_show_result, show, str_speed_show_arg);
221 prog_char help_speed_show[] = "Show speed values for ramp filter";
222 parse_pgm_inst_t cmd_speed_show = {
223 .f = cmd_speed_parsed, /* function to call */
224 .data = (void *)1, /* 2nd arg of func */
225 .help_str = help_speed_show,
226 .tokens = { /* token list, NULL terminated */
227 (prog_void *)&cmd_speed_arg0,
228 (prog_void *)&cmd_csb_name_tok,
229 (prog_void *)&cmd_speed_show_arg,
234 /**********************************************************/
235 /* Derivate_Filters for control system */
237 /* this structure is filled when cmd_derivate_filter is parsed successfully */
238 struct cmd_derivate_filter_result {
239 struct cmd_cs_result cs;
243 /* function called when cmd_derivate_filter is parsed successfully */
244 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
246 struct cmd_derivate_filter_result * res = parsed_result;
247 struct cs_block *csb;
249 csb = cs_from_name(res->cs.csname);
251 printf_P(PSTR("null csb\r\n"));
256 pid_set_derivate_filter(&csb->pid, res->size);
258 printf_P(PSTR("%s %s %u\r\n"),
261 pid_get_derivate_filter(&csb->pid));
264 prog_char str_derivate_filter_arg0[] = "derivate_filter";
265 parse_pgm_token_string_t cmd_derivate_filter_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, cs.cmdname, str_derivate_filter_arg0);
266 parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
268 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
269 parse_pgm_inst_t cmd_derivate_filter = {
270 .f = cmd_derivate_filter_parsed, /* function to call */
271 .data = (void *)1, /* 2nd arg of func */
272 .help_str = help_derivate_filter,
273 .tokens = { /* token list, NULL terminated */
274 (prog_void *)&cmd_derivate_filter_arg0,
275 (prog_void *)&cmd_csb_name_tok,
276 (prog_void *)&cmd_derivate_filter_size,
283 struct cmd_derivate_filter_show_result {
284 struct cmd_cs_result cs;
288 prog_char str_derivate_filter_show_arg[] = "show";
289 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);
291 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
292 parse_pgm_inst_t cmd_derivate_filter_show = {
293 .f = cmd_derivate_filter_parsed, /* function to call */
294 .data = NULL, /* 2nd arg of func */
295 .help_str = help_derivate_filter_show,
296 .tokens = { /* token list, NULL terminated */
297 (prog_void *)&cmd_derivate_filter_arg0,
298 (prog_void *)&cmd_csb_name_tok,
299 (prog_void *)&cmd_derivate_filter_show_arg,
305 /**********************************************************/
306 /* Consign for control system */
308 /* this structure is filled when cmd_consign is parsed successfully */
309 struct cmd_consign_result {
310 struct cmd_cs_result cs;
314 /* function called when cmd_consign is parsed successfully */
315 static void cmd_consign_parsed(void * parsed_result, void *data)
317 struct cmd_consign_result * res = parsed_result;
318 struct cs_block *csb;
320 csb = cs_from_name(res->cs.csname);
322 printf_P(PSTR("null csb\r\n"));
326 cs_set_consign(&csb->cs, res->p);
329 prog_char str_consign_arg0[] = "consign";
330 parse_pgm_token_string_t cmd_consign_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_consign_result, cs.cmdname, str_consign_arg0);
331 parse_pgm_token_num_t cmd_consign_p = TOKEN_NUM_INITIALIZER(struct cmd_consign_result, p, INT32);
333 prog_char help_consign[] = "Set consign value";
334 parse_pgm_inst_t cmd_consign = {
335 .f = cmd_consign_parsed, /* function to call */
336 .data = NULL, /* 2nd arg of func */
337 .help_str = help_consign,
338 .tokens = { /* token list, NULL terminated */
339 (prog_void *)&cmd_consign_arg0,
340 (prog_void *)&cmd_csb_name_tok,
341 (prog_void *)&cmd_consign_p,
347 /**********************************************************/
348 /* Maximums for control system */
350 /* this structure is filled when cmd_maximum is parsed successfully */
351 struct cmd_maximum_result {
352 struct cmd_cs_result cs;
358 /* function called when cmd_maximum is parsed successfully */
359 static void cmd_maximum_parsed(void *parsed_result, void *show)
361 struct cmd_maximum_result * res = parsed_result;
363 struct cs_block *csb;
365 csb = cs_from_name(res->cs.csname);
367 printf_P(PSTR("null csb\r\n"));
372 pid_set_maximums(&csb->pid, res->in, res->i, res->out);
374 printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
376 pid_get_max_in(&csb->pid),
377 pid_get_max_I(&csb->pid),
378 pid_get_max_out(&csb->pid));
381 prog_char str_maximum_arg0[] = "maximum";
382 parse_pgm_token_string_t cmd_maximum_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_maximum_result, cs.cmdname, str_maximum_arg0);
383 parse_pgm_token_num_t cmd_maximum_in = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
384 parse_pgm_token_num_t cmd_maximum_i = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
385 parse_pgm_token_num_t cmd_maximum_out = TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
387 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
388 parse_pgm_inst_t cmd_maximum = {
389 .f = cmd_maximum_parsed, /* function to call */
390 .data = NULL, /* 2nd arg of func */
391 .help_str = help_maximum,
392 .tokens = { /* token list, NULL terminated */
393 (prog_void *)&cmd_maximum_arg0,
394 (prog_void *)&cmd_csb_name_tok,
395 (prog_void *)&cmd_maximum_in,
396 (prog_void *)&cmd_maximum_i,
397 (prog_void *)&cmd_maximum_out,
404 /* this structure is filled when cmd_maximum is parsed successfully */
405 struct cmd_maximum_show_result {
406 struct cmd_cs_result cs;
409 prog_char str_maximum_show_arg[] = "show";
410 parse_pgm_token_string_t cmd_maximum_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
412 prog_char help_maximum_show[] = "Show maximum values for PID";
413 parse_pgm_inst_t cmd_maximum_show = {
414 .f = cmd_maximum_parsed, /* function to call */
415 .data = (void *)1, /* 2nd arg of func */
416 .help_str = help_maximum_show,
417 .tokens = { /* token list, NULL terminated */
418 (prog_void *)&cmd_maximum_arg0,
419 (prog_void *)&cmd_csb_name_tok,
420 (prog_void *)&cmd_maximum_show_arg,
425 /**********************************************************/
426 /* Quadramp for control system */
428 /* this structure is filled when cmd_quadramp is parsed successfully */
429 struct cmd_quadramp_result {
430 struct cmd_cs_result cs;
437 /* function called when cmd_quadramp is parsed successfully */
438 static void cmd_quadramp_parsed(void *parsed_result, void *show)
440 struct cmd_quadramp_result * res = parsed_result;
442 struct cs_block *csb;
444 csb = cs_from_name(res->cs.csname);
446 printf_P(PSTR("null csb\r\n"));
451 quadramp_set_1st_order_vars(&csb->qr, res->sp, res->sn);
452 quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
455 printf_P(PSTR("quadramp %s %2.2f %2.2f %2.2f %2.2f\r\n"),
457 csb->qr.var_2nd_ord_pos,
458 csb->qr.var_2nd_ord_neg,
459 csb->qr.var_1st_ord_pos,
460 csb->qr.var_1st_ord_neg);
463 prog_char str_quadramp_arg0[] = "quadramp";
464 parse_pgm_token_string_t cmd_quadramp_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_result, cs.cmdname, str_quadramp_arg0);
465 parse_pgm_token_num_t cmd_quadramp_ap = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, ap, FLOAT);
466 parse_pgm_token_num_t cmd_quadramp_an = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, an, FLOAT);
467 parse_pgm_token_num_t cmd_quadramp_sp = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sp, FLOAT);
468 parse_pgm_token_num_t cmd_quadramp_sn = TOKEN_NUM_INITIALIZER(struct cmd_quadramp_result, sn, FLOAT);
470 prog_char help_quadramp[] = "Set quadramp values (acc+, acc-, speed+, speed-)";
471 parse_pgm_inst_t cmd_quadramp = {
472 .f = cmd_quadramp_parsed, /* function to call */
473 .data = NULL, /* 2nd arg of func */
474 .help_str = help_quadramp,
475 .tokens = { /* token list, NULL terminated */
476 (prog_void *)&cmd_quadramp_arg0,
477 (prog_void *)&cmd_csb_name_tok,
478 (prog_void *)&cmd_quadramp_ap,
479 (prog_void *)&cmd_quadramp_an,
480 (prog_void *)&cmd_quadramp_sp,
481 (prog_void *)&cmd_quadramp_sn,
489 struct cmd_quadramp_show_result {
490 struct cmd_cs_result cs;
494 prog_char str_quadramp_show_arg[] = "show";
495 parse_pgm_token_string_t cmd_quadramp_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_quadramp_show_result, show, str_quadramp_show_arg);
497 prog_char help_quadramp_show[] = "Get quadramp values for control system";
498 parse_pgm_inst_t cmd_quadramp_show = {
499 .f = cmd_quadramp_parsed, /* function to call */
500 .data = (void *)1, /* 2nd arg of func */
501 .help_str = help_quadramp_show,
502 .tokens = { /* token list, NULL terminated */
503 (prog_void *)&cmd_quadramp_arg0,
504 (prog_void *)&cmd_csb_name_tok,
505 (prog_void *)&cmd_quadramp_show_arg,
512 /**********************************************************/
513 /* cs_status show for control system */
515 /* this structure is filled when cmd_cs_status is parsed successfully */
516 struct cmd_cs_status_result {
517 struct cmd_cs_result cs;
521 /* function called when cmd_cs_status is parsed successfully */
522 static void cmd_cs_status_parsed(void *parsed_result, void *data)
524 struct cmd_cs_status_result *res = parsed_result;
525 struct cs_block *csb;
527 uint8_t print_pid = 0, print_cs = 0;
529 csb = cs_from_name(res->cs.csname);
531 printf_P(PSTR("null csb\r\n"));
534 if (strcmp_P(res->arg, PSTR("on")) == 0) {
536 printf_P(PSTR("%s is on\r\n"), res->cs.csname);
539 else if (strcmp_P(res->arg, PSTR("off")) == 0) {
541 printf_P(PSTR("%s is off\r\n"), res->cs.csname);
544 else if (strcmp_P(res->arg, PSTR("show")) == 0) {
547 else if (strcmp_P(res->arg, PSTR("loop_show")) == 0) {
551 else if (strcmp_P(res->arg, PSTR("pid_show")) == 0) {
554 else if (strcmp_P(res->arg, PSTR("pid_loop_show")) == 0) {
559 printf_P(PSTR("%s cs is %s\r\n"), res->cs.csname, csb->on ? "on":"off");
562 dump_cs(res->cs.csname, &csb->cs);
564 dump_pid(res->cs.csname, &csb->pid);
566 } while(loop && !cmdline_keypressed());
569 prog_char str_cs_status_arg0[] = "cs_status";
570 parse_pgm_token_string_t cmd_cs_status_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, cs.cmdname, str_cs_status_arg0);
571 prog_char str_cs_status_arg[] = "pid_show#pid_loop_show#show#loop_show#on#off";
572 parse_pgm_token_string_t cmd_cs_status_arg = TOKEN_STRING_INITIALIZER(struct cmd_cs_status_result, arg, str_cs_status_arg);
574 prog_char help_cs_status[] = "Show cs status";
575 parse_pgm_inst_t cmd_cs_status = {
576 .f = cmd_cs_status_parsed, /* function to call */
577 .data = NULL, /* 2nd arg of func */
578 .help_str = help_cs_status,
579 .tokens = { /* token list, NULL terminated */
580 (prog_void *)&cmd_cs_status_arg0,
581 (prog_void *)&cmd_csb_name_tok,
582 (prog_void *)&cmd_cs_status_arg,
588 /**********************************************************/
589 /* Blocking_I for control system */
591 /* this structure is filled when cmd_blocking_i is parsed successfully */
592 struct cmd_blocking_i_result {
593 struct cmd_cs_result cs;
600 /* function called when cmd_blocking_i is parsed successfully */
601 static void cmd_blocking_i_parsed(void *parsed_result, void *show)
603 struct cmd_blocking_i_result * res = parsed_result;
605 struct cs_block *csb;
607 csb = cs_from_name(res->cs.csname);
609 printf_P(PSTR("null csb\r\n"));
614 bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
617 printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
626 prog_char str_blocking_i_arg0[] = "blocking";
627 parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, cs.cmdname, str_blocking_i_arg0);
628 parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k1, INT32);
629 parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, k2, INT32);
630 parse_pgm_token_num_t cmd_blocking_i_i = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, i, UINT32);
631 parse_pgm_token_num_t cmd_blocking_i_cpt = TOKEN_NUM_INITIALIZER(struct cmd_blocking_i_result, cpt, UINT16);
633 prog_char help_blocking_i[] = "Set blocking detection values (k1, k2, i, cpt)";
634 parse_pgm_inst_t cmd_blocking_i = {
635 .f = cmd_blocking_i_parsed, /* function to call */
636 .data = NULL, /* 2nd arg of func */
637 .help_str = help_blocking_i,
638 .tokens = { /* token list, NULL terminated */
639 (prog_void *)&cmd_blocking_i_arg0,
640 (prog_void *)&cmd_csb_name_tok,
641 (prog_void *)&cmd_blocking_i_k1,
642 (prog_void *)&cmd_blocking_i_k2,
643 (prog_void *)&cmd_blocking_i_i,
644 (prog_void *)&cmd_blocking_i_cpt,
651 struct cmd_blocking_i_show_result {
652 struct cmd_cs_result cs;
656 prog_char str_blocking_i_show_arg[] = "show";
657 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);
659 prog_char help_blocking_i_show[] = "Show blocking detection values";
660 parse_pgm_inst_t cmd_blocking_i_show = {
661 .f = cmd_blocking_i_parsed, /* function to call */
662 .data = (void *)1, /* 2nd arg of func */
663 .help_str = help_blocking_i_show,
664 .tokens = { /* token list, NULL terminated */
665 (prog_void *)&cmd_blocking_i_arg0,
666 (prog_void *)&cmd_csb_name_tok,
667 (prog_void *)&cmd_blocking_i_show_arg,