2 * Copyright Droids Corporation (2009)
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_mainboard.c,v 1.7 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>
29 #include <avr/eeprom.h>
33 #include <clock_time.h>
37 #include <control_system_manager.h>
38 #include <trajectory_manager.h>
39 #include <blocking_detection_manager.h>
40 #include <robot_system.h>
41 #include <position_manager.h>
45 #include <parse_string.h>
46 #include <parse_num.h>
52 #include "strat_base.h"
54 struct cmd_event_result {
61 /* function called when cmd_event is parsed successfully */
62 static void cmd_event_parsed(void *parsed_result, void *data)
66 struct cmd_event_result * res = parsed_result;
68 if (!strcmp_P(res->arg1, PSTR("all"))) {
69 bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
70 DO_BD | DO_TIMER | DO_POWER;
71 if (!strcmp_P(res->arg2, PSTR("on")))
72 mainboard.flags |= bit;
73 else if (!strcmp_P(res->arg2, PSTR("off")))
74 mainboard.flags &= bit;
76 printf_P(PSTR("encoders is %s\r\n"),
77 (DO_ENCODERS & mainboard.flags) ? "on":"off");
78 printf_P(PSTR("cs is %s\r\n"),
79 (DO_CS & mainboard.flags) ? "on":"off");
80 printf_P(PSTR("rs is %s\r\n"),
81 (DO_RS & mainboard.flags) ? "on":"off");
82 printf_P(PSTR("pos is %s\r\n"),
83 (DO_POS & mainboard.flags) ? "on":"off");
84 printf_P(PSTR("bd is %s\r\n"),
85 (DO_BD & mainboard.flags) ? "on":"off");
86 printf_P(PSTR("timer is %s\r\n"),
87 (DO_TIMER & mainboard.flags) ? "on":"off");
88 printf_P(PSTR("power is %s\r\n"),
89 (DO_POWER & mainboard.flags) ? "on":"off");
94 if (!strcmp_P(res->arg1, PSTR("encoders")))
96 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
100 else if (!strcmp_P(res->arg1, PSTR("rs")))
102 else if (!strcmp_P(res->arg1, PSTR("pos")))
104 else if (!strcmp_P(res->arg1, PSTR("bd")))
106 else if (!strcmp_P(res->arg1, PSTR("timer"))) {
110 else if (!strcmp_P(res->arg1, PSTR("power")))
113 if (!strcmp_P(res->arg2, PSTR("on")))
114 mainboard.flags |= bit;
115 else if (!strcmp_P(res->arg2, PSTR("off"))) {
116 if (!strcmp_P(res->arg1, PSTR("cs"))) {
117 pwm_ng_set(LEFT_PWM, 0);
118 pwm_ng_set(RIGHT_PWM, 0);
120 mainboard.flags &= (~bit);
122 printf_P(PSTR("%s is %s\r\n"), res->arg1,
123 (bit & mainboard.flags) ? "on":"off");
126 prog_char str_event_arg0[] = "event";
127 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
128 prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
129 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
130 prog_char str_event_arg2[] = "on#off#show";
131 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
133 prog_char help_event[] = "Enable/disable events";
134 parse_pgm_inst_t cmd_event = {
135 .f = cmd_event_parsed, /* function to call */
136 .data = NULL, /* 2nd arg of func */
137 .help_str = help_event,
138 .tokens = { /* token list, NULL terminated */
139 (prog_void *)&cmd_event_arg0,
140 (prog_void *)&cmd_event_arg1,
141 (prog_void *)&cmd_event_arg2,
146 /**********************************************************/
149 /* this structure is filled when cmd_interact is parsed successfully */
150 struct cmd_interact_result {
154 static void print_cs(void)
156 printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
157 "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"),
158 cs_get_consign(&mainboard.distance.cs),
159 cs_get_consign(&mainboard.angle.cs),
160 cs_get_filtered_consign(&mainboard.distance.cs),
161 cs_get_filtered_consign(&mainboard.angle.cs),
162 cs_get_error(&mainboard.distance.cs),
163 cs_get_error(&mainboard.angle.cs),
164 cs_get_out(&mainboard.distance.cs),
165 cs_get_out(&mainboard.angle.cs));
168 static void print_pos(void)
170 printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
171 position_get_x_s16(&mainboard.pos),
172 position_get_y_s16(&mainboard.pos),
173 position_get_a_deg_s16(&mainboard.pos));
176 static void print_time(void)
178 printf_P(PSTR("time %d\r\n"),time_get_s());
182 static void print_sensors(void)
185 if (sensor_start_switch())
186 printf_P(PSTR("Start switch | "));
188 printf_P(PSTR(" | "));
190 if (IR_DISP_SENSOR())
191 printf_P(PSTR("IR disp | "));
193 printf_P(PSTR(" | "));
195 printf_P(PSTR("\r\n"));
199 static void print_pid(void)
201 printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
202 "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
203 pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
204 pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
205 pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
206 pid_get_value_out(&mainboard.distance.pid),
207 pid_get_value_in(&mainboard.angle.pid) * pid_get_gain_P(&mainboard.angle.pid),
208 pid_get_value_I(&mainboard.angle.pid) * pid_get_gain_I(&mainboard.angle.pid),
209 pid_get_value_D(&mainboard.angle.pid) * pid_get_gain_D(&mainboard.angle.pid),
210 pid_get_value_out(&mainboard.angle.pid));
213 #define PRINT_POS (1<<0)
214 #define PRINT_PID (1<<1)
215 #define PRINT_CS (1<<2)
216 #define PRINT_SENSORS (1<<3)
217 #define PRINT_TIME (1<<4)
218 #define PRINT_BLOCKING (1<<5)
220 static void cmd_interact_parsed(void * parsed_result, void * data)
229 printf_P(PSTR("Display debugs:\r\n"
235 /* " 6:blocking\r\n" */
242 mainboard.flags &= (~DO_CS);
243 pwm_set_and_save(LEFT_PWM, 0);
244 pwm_set_and_save(RIGHT_PWM, 0);
247 if (print & PRINT_POS) {
251 if (print & PRINT_PID) {
255 if (print & PRINT_CS) {
259 if (print & PRINT_SENSORS) {
263 if (print & PRINT_TIME) {
266 /* if (print & PRINT_BLOCKING) { */
267 /* printf_P(PSTR("%s %s blocking=%d\r\n"), */
268 /* mainboard.blocking ? "BLOCK1":" ", */
269 /* rs_is_blocked(&mainboard.rs) ? "BLOCK2":" ", */
270 /* rs_get_blocking(&mainboard.rs)); */
273 c = cmdline_getchar();
278 cmd = vt100_parser(&vt100, c);
286 case '1': print ^= PRINT_POS; break;
287 case '2': print ^= PRINT_PID; break;
288 case '3': print ^= PRINT_CS; break;
289 case '4': print ^= PRINT_SENSORS; break;
290 case '5': print ^= PRINT_TIME; break;
291 case '6': print ^= PRINT_BLOCKING; break;
294 /* if (mainboard.flags & DO_CS) */
295 /* strat_hardstop(); */
296 pwm_set_and_save(LEFT_PWM, 0);
297 pwm_set_and_save(RIGHT_PWM, 0);
300 pwm_set_and_save(LEFT_PWM, 0);
301 pwm_set_and_save(RIGHT_PWM, 0);
310 pwm_set_and_save(LEFT_PWM, 1200);
311 pwm_set_and_save(RIGHT_PWM, 1200);
314 pwm_set_and_save(LEFT_PWM, -1200);
315 pwm_set_and_save(RIGHT_PWM, 1200);
318 pwm_set_and_save(LEFT_PWM, -1200);
319 pwm_set_and_save(RIGHT_PWM, -1200);
322 pwm_set_and_save(LEFT_PWM, 1200);
323 pwm_set_and_save(RIGHT_PWM, -1200);
331 prog_char str_interact_arg0[] = "interact";
332 parse_pgm_token_string_t cmd_interact_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_interact_result, arg0, str_interact_arg0);
334 prog_char help_interact[] = "Interactive mode";
335 parse_pgm_inst_t cmd_interact = {
336 .f = cmd_interact_parsed, /* function to call */
337 .data = NULL, /* 2nd arg of func */
338 .help_str = help_interact,
339 .tokens = { /* token list, NULL terminated */
340 (prog_void *)&cmd_interact_arg0,
346 /**********************************************************/
349 /* this structure is filled when cmd_rs is parsed successfully */
350 struct cmd_rs_result {
355 /* function called when cmd_rs is parsed successfully */
356 static void cmd_rs_parsed(void *parsed_result, void *data)
358 // struct cmd_rs_result *res = parsed_result;
360 printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
361 cs_get_consign(&mainboard.angle.cs),
362 cs_get_filtered_feedback(&mainboard.angle.cs),
363 cs_get_out(&mainboard.angle.cs));
364 printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
365 cs_get_consign(&mainboard.distance.cs),
366 cs_get_filtered_feedback(&mainboard.distance.cs),
367 cs_get_out(&mainboard.distance.cs));
368 printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
371 } while(!cmdline_keypressed());
374 prog_char str_rs_arg0[] = "rs";
375 parse_pgm_token_string_t cmd_rs_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg0, str_rs_arg0);
376 prog_char str_rs_arg1[] = "show";
377 parse_pgm_token_string_t cmd_rs_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_rs_result, arg1, str_rs_arg1);
379 prog_char help_rs[] = "Show rs (robot system) values";
380 parse_pgm_inst_t cmd_rs = {
381 .f = cmd_rs_parsed, /* function to call */
382 .data = NULL, /* 2nd arg of func */
384 .tokens = { /* token list, NULL terminated */
385 (prog_void *)&cmd_rs_arg0,
386 (prog_void *)&cmd_rs_arg1,
392 /**********************************************************/
395 /* this structure is filled when cmd_fessor is parsed successfully */
396 struct cmd_fessor_result {
401 /* function called when cmd_fessor is parsed successfully */
402 static void cmd_fessor_parsed(void * parsed_result, void * data)
404 struct cmd_fessor_result * res = parsed_result;
406 if (!strcmp_P(res->arg1, PSTR("up"))) {
409 else if (!strcmp_P(res->arg1, PSTR("down"))) {
412 else if (!strcmp_P(res->arg1, PSTR("stop"))) {
415 else if (!strcmp_P(res->arg1, PSTR("auto"))) {
419 printf_P(PSTR("done\r\n"));
422 prog_char str_fessor_arg0[] = "fessor";
423 parse_pgm_token_string_t cmd_fessor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_result, arg0, str_fessor_arg0);
424 prog_char str_fessor_arg1[] = "auto#up#down#stop";
425 parse_pgm_token_string_t cmd_fessor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_result, arg1, str_fessor_arg1);
427 prog_char help_fessor[] = "fessor auto mode: fessor auto delay_up delay_down";
428 parse_pgm_inst_t cmd_fessor = {
429 .f = cmd_fessor_parsed, /* function to call */
430 .data = NULL, /* 2nd arg of func */
431 .help_str = help_fessor,
432 .tokens = { /* token list, NULL terminated */
433 (prog_void *)&cmd_fessor_arg0,
434 (prog_void *)&cmd_fessor_arg1,
439 /**********************************************************/
440 /* Fessor_Paramss tests */
442 /* this structure is filled when cmd_fessor_params is parsed successfully */
443 struct cmd_fessor_params_result {
450 /* function called when cmd_fessor_params is parsed successfully */
451 static void cmd_fessor_params_parsed(void * parsed_result, void * data)
453 struct cmd_fessor_params_result * res = parsed_result;
456 if (!strcmp_P(res->arg1, PSTR("delay"))) {
457 fessor_set_delays(res->arg2, res->arg3);
459 else if (!strcmp_P(res->arg1, PSTR("coef"))) {
460 fessor_set_coefs(res->arg2, res->arg3);
462 else if (!strcmp_P(res->arg1, PSTR("pos"))) {
463 fessor_set_pos(res->arg2, res->arg3);
467 fessor_dump_params();
470 prog_char str_fessor_params_arg0[] = "fessor_params";
471 parse_pgm_token_string_t cmd_fessor_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg0, str_fessor_params_arg0);
472 prog_char str_fessor_params_arg1[] = "delay#pos#coef";
473 parse_pgm_token_string_t cmd_fessor_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg1, str_fessor_params_arg1);
474 parse_pgm_token_num_t cmd_fessor_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_fessor_params_result, arg2, INT32);
475 parse_pgm_token_num_t cmd_fessor_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_fessor_params_result, arg3, INT32);
477 prog_char help_fessor_params[] = "Set fessor_params values";
478 parse_pgm_inst_t cmd_fessor_params = {
479 .f = cmd_fessor_params_parsed, /* function to call */
480 .data = NULL, /* 2nd arg of func */
481 .help_str = help_fessor_params,
482 .tokens = { /* token list, NULL terminated */
483 (prog_void *)&cmd_fessor_params_arg0,
484 (prog_void *)&cmd_fessor_params_arg1,
485 (prog_void *)&cmd_fessor_params_arg2,
486 (prog_void *)&cmd_fessor_params_arg3,
491 prog_char str_fessor_params_arg1_show[] = "show";
492 parse_pgm_token_string_t cmd_fessor_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_fessor_params_result, arg1, str_fessor_params_arg1_show);
494 prog_char help_fessor_params_show[] = "show fessor params";
495 parse_pgm_inst_t cmd_fessor_params_show = {
496 .f = cmd_fessor_params_parsed, /* function to call */
497 .data = NULL, /* 2nd arg of func */
498 .help_str = help_fessor_params_show,
499 .tokens = { /* token list, NULL terminated */
500 (prog_void *)&cmd_fessor_params_arg0,
501 (prog_void *)&cmd_fessor_params_arg1_show,
509 /**********************************************************/
510 /* Elevators tests */
512 /* this structure is filled when cmd_elevator is parsed successfully */
513 struct cmd_elevator_result {
518 /* function called when cmd_elevator is parsed successfully */
519 static void cmd_elevator_parsed(void * parsed_result, void * data)
521 struct cmd_elevator_result * res = parsed_result;
523 if (!strcmp_P(res->arg1, PSTR("up"))) {
526 else if (!strcmp_P(res->arg1, PSTR("down"))) {
529 else if (!strcmp_P(res->arg1, PSTR("stop"))) {
532 else if (!strcmp_P(res->arg1, PSTR("auto"))) {
536 printf_P(PSTR("done\r\n"));
539 prog_char str_elevator_arg0[] = "elevator";
540 parse_pgm_token_string_t cmd_elevator_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_result, arg0, str_elevator_arg0);
541 prog_char str_elevator_arg1[] = "auto#up#down#stop";
542 parse_pgm_token_string_t cmd_elevator_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_result, arg1, str_elevator_arg1);
544 prog_char help_elevator[] = "elevator auto mode: elevator auto delay_up delay_down";
545 parse_pgm_inst_t cmd_elevator = {
546 .f = cmd_elevator_parsed, /* function to call */
547 .data = NULL, /* 2nd arg of func */
548 .help_str = help_elevator,
549 .tokens = { /* token list, NULL terminated */
550 (prog_void *)&cmd_elevator_arg0,
551 (prog_void *)&cmd_elevator_arg1,
556 /**********************************************************/
557 /* Elevator_Paramss tests */
559 /* this structure is filled when cmd_elevator_params is parsed successfully */
560 struct cmd_elevator_params_result {
567 /* function called when cmd_elevator_params is parsed successfully */
568 static void cmd_elevator_params_parsed(void * parsed_result, void * data)
570 struct cmd_elevator_params_result * res = parsed_result;
573 if (!strcmp_P(res->arg1, PSTR("delay"))) {
574 elevator_set_delays(res->arg2, res->arg3);
576 else if (!strcmp_P(res->arg1, PSTR("coef"))) {
577 elevator_set_coefs(res->arg2, res->arg3);
579 else if (!strcmp_P(res->arg1, PSTR("pos"))) {
580 elevator_set_pos(res->arg2, res->arg3);
584 elevator_dump_params();
587 prog_char str_elevator_params_arg0[] = "elevator_params";
588 parse_pgm_token_string_t cmd_elevator_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg0, str_elevator_params_arg0);
589 prog_char str_elevator_params_arg1[] = "delay#pos#coef";
590 parse_pgm_token_string_t cmd_elevator_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg1, str_elevator_params_arg1);
591 parse_pgm_token_num_t cmd_elevator_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_elevator_params_result, arg2, INT32);
592 parse_pgm_token_num_t cmd_elevator_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_elevator_params_result, arg3, INT32);
594 prog_char help_elevator_params[] = "Set elevator_params values";
595 parse_pgm_inst_t cmd_elevator_params = {
596 .f = cmd_elevator_params_parsed, /* function to call */
597 .data = NULL, /* 2nd arg of func */
598 .help_str = help_elevator_params,
599 .tokens = { /* token list, NULL terminated */
600 (prog_void *)&cmd_elevator_params_arg0,
601 (prog_void *)&cmd_elevator_params_arg1,
602 (prog_void *)&cmd_elevator_params_arg2,
603 (prog_void *)&cmd_elevator_params_arg3,
608 prog_char str_elevator_params_arg1_show[] = "show";
609 parse_pgm_token_string_t cmd_elevator_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_elevator_params_result, arg1, str_elevator_params_arg1_show);
611 prog_char help_elevator_params_show[] = "show elevator params";
612 parse_pgm_inst_t cmd_elevator_params_show = {
613 .f = cmd_elevator_params_parsed, /* function to call */
614 .data = NULL, /* 2nd arg of func */
615 .help_str = help_elevator_params_show,
616 .tokens = { /* token list, NULL terminated */
617 (prog_void *)&cmd_elevator_params_arg0,
618 (prog_void *)&cmd_elevator_params_arg1_show,
628 /**********************************************************/
631 /* this structure is filled when cmd_test is parsed successfully */
632 struct cmd_test_result {
636 /* function called when cmd_test is parsed successfully */
637 static void cmd_test_parsed(void *parsed_result, void *data)
641 prog_char str_test_arg0[] = "test";
642 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
644 prog_char help_test[] = "Test function";
645 parse_pgm_inst_t cmd_test = {
646 .f = cmd_test_parsed, /* function to call */
647 .data = NULL, /* 2nd arg of func */
648 .help_str = help_test,
649 .tokens = { /* token list, NULL terminated */
650 (prog_void *)&cmd_test_arg0,