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_gen.c,v 1.6 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>
32 #include <clock_time.h>
33 #include <encoders_microb.h>
36 #include <scheduler.h>
37 #include <scheduler_stats.h>
40 #include <control_system_manager.h>
41 #include <trajectory_manager.h>
42 #include <blocking_detection_manager.h>
43 #include <robot_system.h>
44 #include <position_manager.h>
48 #include <parse_string.h>
49 #include <parse_num.h>
51 #include <diagnostic.h>
59 /**********************************************************/
62 /* this structure is filled when cmd_reset is parsed successfully */
63 struct cmd_reset_result {
67 /* function called when cmd_reset is parsed successfully */
68 static void cmd_reset_parsed(void * parsed_result, void * data)
73 prog_char str_reset_arg0[] = "reset";
74 parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
76 prog_char help_reset[] = "Reset the board";
77 parse_pgm_inst_t cmd_reset = {
78 .f = cmd_reset_parsed, /* function to call */
79 .data = NULL, /* 2nd arg of func */
80 .help_str = help_reset,
81 .tokens = { /* token list, NULL terminated */
82 (prog_void *)&cmd_reset_arg0,
87 /**********************************************************/
90 /* this structure is filled when cmd_bootloader is parsed successfully */
91 struct cmd_bootloader_result {
95 /* function called when cmd_bootloader is parsed successfully */
96 static void cmd_bootloader_parsed(void *parsed_result, void *data)
101 prog_char str_bootloader_arg0[] = "bootloader";
102 parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
104 prog_char help_bootloader[] = "Launch the bootloader";
105 parse_pgm_inst_t cmd_bootloader = {
106 .f = cmd_bootloader_parsed, /* function to call */
107 .data = NULL, /* 2nd arg of func */
108 .help_str = help_bootloader,
109 .tokens = { /* token list, NULL terminated */
110 (prog_void *)&cmd_bootloader_arg0,
115 /**********************************************************/
118 /* this structure is filled when cmd_encoders is parsed successfully */
119 struct cmd_encoders_result {
124 /* function called when cmd_encoders is parsed successfully */
125 static void cmd_encoders_parsed(void *parsed_result, void *data)
127 struct cmd_encoders_result *res = parsed_result;
129 if (!strcmp_P(res->arg1, PSTR("reset"))) {
130 encoders_microb_set_value((void *)0, 0);
131 encoders_microb_set_value((void *)1, 0);
132 encoders_microb_set_value((void *)2, 0);
133 encoders_microb_set_value((void *)3, 0);
138 while(!cmdline_keypressed()) {
139 printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"),
140 encoders_microb_get_value((void *)0),
141 encoders_microb_get_value((void *)1),
142 encoders_microb_get_value((void *)2),
143 encoders_microb_get_value((void *)3));
148 prog_char str_encoders_arg0[] = "encoders";
149 parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
150 prog_char str_encoders_arg1[] = "show#reset";
151 parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
153 prog_char help_encoders[] = "Show encoders values";
154 parse_pgm_inst_t cmd_encoders = {
155 .f = cmd_encoders_parsed, /* function to call */
156 .data = NULL, /* 2nd arg of func */
157 .help_str = help_encoders,
158 .tokens = { /* token list, NULL terminated */
159 (prog_void *)&cmd_encoders_arg0,
160 (prog_void *)&cmd_encoders_arg1,
165 /**********************************************************/
168 /* this structure is filled when cmd_scheduler is parsed successfully */
169 struct cmd_scheduler_result {
174 /* function called when cmd_scheduler is parsed successfully */
175 static void cmd_scheduler_parsed(void *parsed_result, void *data)
177 scheduler_dump_events();
178 scheduler_stats_dump();
181 prog_char str_scheduler_arg0[] = "scheduler";
182 parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
183 prog_char str_scheduler_arg1[] = "show";
184 parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
186 prog_char help_scheduler[] = "Show scheduler events";
187 parse_pgm_inst_t cmd_scheduler = {
188 .f = cmd_scheduler_parsed, /* function to call */
189 .data = NULL, /* 2nd arg of func */
190 .help_str = help_scheduler,
191 .tokens = { /* token list, NULL terminated */
192 (prog_void *)&cmd_scheduler_arg0,
193 (prog_void *)&cmd_scheduler_arg1,
198 /**********************************************************/
201 /* this structure is filled when cmd_pwm is parsed successfully */
202 struct cmd_pwm_result {
208 /* function called when cmd_pwm is parsed successfully */
209 static void cmd_pwm_parsed(void * parsed_result, void * data)
211 void * pwm_ptr = NULL;
212 struct cmd_pwm_result * res = parsed_result;
214 if (!strcmp_P(res->arg1, PSTR("1(1A)")))
215 pwm_ptr = &gen.pwm1_1A;
216 else if (!strcmp_P(res->arg1, PSTR("2(1B)")))
217 pwm_ptr = &gen.pwm2_1B;
218 else if (!strcmp_P(res->arg1, PSTR("3(3A)")))
219 pwm_ptr = &gen.pwm3_3A;
220 else if (!strcmp_P(res->arg1, PSTR("4(3B)")))
221 pwm_ptr = &gen.pwm4_3B;
224 pwm_ng_set(pwm_ptr, res->arg2);
226 printf_P(PSTR("done %p\r\n"), pwm_ptr);
229 prog_char str_pwm_arg0[] = "pwm";
230 parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
231 prog_char str_pwm_arg1[] = "1(1A)#2(1B)#3(3A)#4(3B)";
232 parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
233 parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
235 prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
236 parse_pgm_inst_t cmd_pwm = {
237 .f = cmd_pwm_parsed, /* function to call */
238 .data = NULL, /* 2nd arg of func */
239 .help_str = help_pwm,
240 .tokens = { /* token list, NULL terminated */
241 (prog_void *)&cmd_pwm_arg0,
242 (prog_void *)&cmd_pwm_arg1,
243 (prog_void *)&cmd_pwm_arg2,
248 /**********************************************************/
251 /* this structure is filled when cmd_adc is parsed successfully */
252 struct cmd_adc_result {
257 /* function called when cmd_adc is parsed successfully */
258 static void cmd_adc_parsed(void *parsed_result, void *data)
260 struct cmd_adc_result *res = parsed_result;
263 if (!strcmp_P(res->arg1, PSTR("loop_show")))
267 printf_P(PSTR("ADC values: "));
268 for (i=0; i<ADC_MAX; i++) {
269 printf_P(PSTR("%.4d "), sensor_get_adc(i));
271 printf_P(PSTR("\r\n"));
273 } while (loop && !cmdline_keypressed());
276 prog_char str_adc_arg0[] = "adc";
277 parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
278 prog_char str_adc_arg1[] = "show#loop_show";
279 parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
281 prog_char help_adc[] = "Show adc values";
282 parse_pgm_inst_t cmd_adc = {
283 .f = cmd_adc_parsed, /* function to call */
284 .data = NULL, /* 2nd arg of func */
285 .help_str = help_adc,
286 .tokens = { /* token list, NULL terminated */
287 (prog_void *)&cmd_adc_arg0,
288 (prog_void *)&cmd_adc_arg1,
294 /**********************************************************/
297 /* this structure is filled when cmd_sensor is parsed successfully */
298 struct cmd_sensor_result {
303 /* function called when cmd_sensor is parsed successfully */
304 static void cmd_sensor_parsed(void *parsed_result, void *data)
306 struct cmd_sensor_result *res = parsed_result;
309 if (!strcmp_P(res->arg1, PSTR("loop_show")))
313 printf_P(PSTR("SENSOR values: "));
314 for (i=0; i<SENSOR_MAX; i++) {
315 printf_P(PSTR("%d "), !!sensor_get(i));
317 printf_P(PSTR("\r\n"));
319 } while (loop && !cmdline_keypressed());
322 prog_char str_sensor_arg0[] = "sensor";
323 parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
324 prog_char str_sensor_arg1[] = "show#loop_show";
325 parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
327 prog_char help_sensor[] = "Show sensor values";
328 parse_pgm_inst_t cmd_sensor = {
329 .f = cmd_sensor_parsed, /* function to call */
330 .data = NULL, /* 2nd arg of func */
331 .help_str = help_sensor,
332 .tokens = { /* token list, NULL terminated */
333 (prog_void *)&cmd_sensor_arg0,
334 (prog_void *)&cmd_sensor_arg1,
340 /**********************************************************/
343 /* this structure is filled when cmd_log is parsed successfully */
344 struct cmd_log_result {
351 /* keep it sync with string choice */
352 static const prog_char uart_log[] = "uart";
353 static const prog_char rs_log[] = "rs";
354 static const prog_char traj_log[] = "traj";
355 static const prog_char i2c_log[] = "i2c";
356 static const prog_char oa_log[] = "oa";
357 static const prog_char strat_log[] = "strat";
358 static const prog_char i2cproto_log[] = "i2cproto";
359 static const prog_char sensor_log[] = "sensor";
360 static const prog_char block_log[] = "bd";
361 static const prog_char cs_log[] = "cs";
363 struct log_name_and_num {
364 const prog_char * name;
368 static const struct log_name_and_num log_name_and_num[] = {
369 { uart_log, E_UART },
370 { rs_log, E_ROBOT_SYSTEM },
371 { traj_log, E_TRAJECTORY },
374 { strat_log, E_USER_STRAT },
375 { i2cproto_log, E_USER_I2C_PROTO },
376 { sensor_log, E_USER_SENSOR },
377 { block_log, E_BLOCKING_DETECTION_MANAGER },
378 { cs_log, E_USER_CS },
382 log_name2num(const char * s)
386 for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
387 if (!strcmp_P(s, log_name_and_num[i].name)) {
388 return log_name_and_num[i].num;
395 log_num2name(uint8_t num)
399 for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
400 if (num == log_name_and_num[i].num) {
401 return log_name_and_num[i].name;
407 /* function called when cmd_log is parsed successfully */
408 static void cmd_log_do_show(void)
411 const prog_char * name;
413 printf_P(PSTR("log level is %d\r\n"), gen.log_level);
414 for (i=0; i<NB_LOGS; i++) {
415 name = log_num2name(gen.logs[i]);
417 printf_P(PSTR("log type %S is on\r\n"), name);
422 printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
425 /* function called when cmd_log is parsed successfully */
426 static void cmd_log_parsed(void * parsed_result, void * data)
428 struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
430 if (!strcmp_P(res->arg1, PSTR("level"))) {
431 gen.log_level = res->arg2;
434 /* else it is a show */
438 prog_char str_log_arg0[] = "log";
439 parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
440 prog_char str_log_arg1[] = "level";
441 parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
442 parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
444 prog_char help_log[] = "Set log options: level (0 -> 5)";
445 parse_pgm_inst_t cmd_log = {
446 .f = cmd_log_parsed, /* function to call */
447 .data = NULL, /* 2nd arg of func */
448 .help_str = help_log,
449 .tokens = { /* token list, NULL terminated */
450 (prog_void *)&cmd_log_arg0,
451 (prog_void *)&cmd_log_arg1,
452 (prog_void *)&cmd_log_arg2,
457 prog_char str_log_arg1_show[] = "show";
458 parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
460 prog_char help_log_show[] = "Show configured logs";
461 parse_pgm_inst_t cmd_log_show = {
462 .f = cmd_log_parsed, /* function to call */
463 .data = NULL, /* 2nd arg of func */
464 .help_str = help_log_show,
465 .tokens = { /* token list, NULL terminated */
466 (prog_void *)&cmd_log_arg0,
467 (prog_void *)&cmd_log_arg1_show,
472 /* this structure is filled when cmd_log is parsed successfully */
473 struct cmd_log_type_result {
480 /* function called when cmd_log is parsed successfully */
481 static void cmd_log_type_parsed(void * parsed_result, void * data)
483 struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
487 lognum = log_name2num(res->arg2);
489 printf_P(PSTR("Cannot find log num\r\n"));
493 if (!strcmp_P(res->arg3, PSTR("on"))) {
494 for (i=0; i<NB_LOGS; i++) {
495 if (gen.logs[i] == lognum) {
496 printf_P(PSTR("Already on\r\n"));
500 for (i=0; i<NB_LOGS; i++) {
501 if (gen.logs[i] == 0) {
502 gen.logs[i] = lognum;
507 printf_P(PSTR("no more room\r\n"));
510 else if (!strcmp_P(res->arg3, PSTR("off"))) {
511 for (i=0; i<NB_LOGS; i++) {
512 if (gen.logs[i] == lognum) {
518 printf_P(PSTR("already off\r\n"));
524 prog_char str_log_arg1_type[] = "type";
525 parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
526 /* keep it sync with log_name_and_num above */
527 prog_char str_log_arg2_type[] = "uart#rs#servo#traj#i2c#oa#strat#i2cproto#ext#sensor#bd#cs";
528 parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
529 prog_char str_log_arg3[] = "on#off";
530 parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
532 prog_char help_log_type[] = "Set log type";
533 parse_pgm_inst_t cmd_log_type = {
534 .f = cmd_log_type_parsed, /* function to call */
535 .data = NULL, /* 2nd arg of func */
536 .help_str = help_log_type,
537 .tokens = { /* token list, NULL terminated */
538 (prog_void *)&cmd_log_arg0,
539 (prog_void *)&cmd_log_arg1_type,
540 (prog_void *)&cmd_log_arg2_type,
541 (prog_void *)&cmd_log_arg3,
547 /**********************************************************/
550 /* this structure is filled when cmd_stack_space is parsed successfully */
551 struct cmd_stack_space_result {
555 /* function called when cmd_stack_space is parsed successfully */
556 static void cmd_stack_space_parsed(void *parsed_result, void *data)
558 printf("res stack: %d\r\n", min_stack_space_available());
562 prog_char str_stack_space_arg0[] = "stack_space";
563 parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
565 prog_char help_stack_space[] = "Display remaining stack space";
566 parse_pgm_inst_t cmd_stack_space = {
567 .f = cmd_stack_space_parsed, /* function to call */
568 .data = NULL, /* 2nd arg of func */
569 .help_str = help_stack_space,
570 .tokens = { /* token list, NULL terminated */
571 (prog_void *)&cmd_stack_space_arg0,
577 /**********************************************************/
581 /* this structure is filled when cmd_servo is parsed successfully */
582 struct cmd_servo_result {
588 /* function called when cmd_servo is parsed successfully */
589 static void cmd_servo_parsed(void * parsed_result, void * data)
591 struct cmd_servo_result *res = (struct cmd_servo_result *) parsed_result;
592 action_servo_set(res->num, res->us);
595 prog_char str_servo_arg0[] = "servo";
596 parse_pgm_token_string_t cmd_servo_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg0, str_servo_arg0);
597 parse_pgm_token_num_t cmd_servo_num = TOKEN_NUM_INITIALIZER(struct cmd_servo_result, num, UINT8);
598 parse_pgm_token_num_t cmd_servo_us = TOKEN_NUM_INITIALIZER(struct cmd_servo_result, us, UINT16);
600 prog_char help_servo[] = "Set servo position/speed";
601 parse_pgm_inst_t cmd_servo = {
602 .f = cmd_servo_parsed, /* function to call */
603 .data = NULL, /* 2nd arg of func */
604 .help_str = help_servo,
605 .tokens = { /* token list, NULL terminated */
606 (prog_void *)&cmd_servo_arg0,
607 (prog_void *)&cmd_servo_num,
608 (prog_void *)&cmd_servo_us,