work on beacon
[aversive.git] / projects / microb2010 / mainboard / commands_gen.c
1 /*
2  *  Copyright Droids Corporation (2008)
3  * 
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.
8  *
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.
13  *
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
17  *
18  *  Revision : $Id: commands_gen.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
19  *
20  *  Olivier MATZ <zer0@droids-corp.org> 
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
29
30 #include <ax12.h>
31 #include <uart.h>
32 #include <pwm_ng.h>
33 #include <time.h>
34 #include <encoders_spi.h>
35 #include <adc.h>
36
37 #include <scheduler.h>
38 #include <scheduler_stats.h>
39 #include <pid.h>
40 #include <quadramp.h>
41 #include <control_system_manager.h>
42 #include <trajectory_manager.h>
43 #include <vect_base.h>
44 #include <lines.h>
45 #include <polygon.h>
46 #include <obstacle_avoidance.h>
47 #include <blocking_detection_manager.h>
48 #include <robot_system.h>
49 #include <position_manager.h>
50
51 #include <rdline.h>
52 #include <parse.h>
53 #include <parse_string.h>
54 #include <parse_num.h>
55
56 #include <diagnostic.h>
57
58 #include "main.h"
59 #include "cmdline.h"
60 #include "sensor.h"
61
62 /**********************************************************/
63 /* Reset */
64
65 /* this structure is filled when cmd_reset is parsed successfully */
66 struct cmd_reset_result {
67         fixed_string_t arg0;
68 };
69
70 /* function called when cmd_reset is parsed successfully */
71 static void cmd_reset_parsed(void * parsed_result, void * data)
72 {
73         reset();
74 }
75
76 prog_char str_reset_arg0[] = "reset";
77 parse_pgm_token_string_t cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
78
79 prog_char help_reset[] = "Reset the board";
80 parse_pgm_inst_t cmd_reset = {
81         .f = cmd_reset_parsed,  /* function to call */
82         .data = NULL,      /* 2nd arg of func */
83         .help_str = help_reset,
84         .tokens = {        /* token list, NULL terminated */
85                 (prog_void *)&cmd_reset_arg0, 
86                 NULL,
87         },
88 };
89
90 /**********************************************************/
91 /* Bootloader */
92
93 /* this structure is filled when cmd_bootloader is parsed successfully */
94 struct cmd_bootloader_result {
95         fixed_string_t arg0;
96 };
97
98 /* function called when cmd_bootloader is parsed successfully */
99 static void cmd_bootloader_parsed(void *parsed_result, void *data)
100 {
101         bootloader();
102 }
103
104 prog_char str_bootloader_arg0[] = "bootloader";
105 parse_pgm_token_string_t cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
106
107 prog_char help_bootloader[] = "Launch the bootloader";
108 parse_pgm_inst_t cmd_bootloader = {
109         .f = cmd_bootloader_parsed,  /* function to call */
110         .data = NULL,      /* 2nd arg of func */
111         .help_str = help_bootloader,
112         .tokens = {        /* token list, NULL terminated */
113                 (prog_void *)&cmd_bootloader_arg0, 
114                 NULL,
115         },
116 };
117
118 /**********************************************************/
119 /* Encoders tests */
120
121 /* this structure is filled when cmd_encoders is parsed successfully */
122 struct cmd_encoders_result {
123         fixed_string_t arg0;
124         fixed_string_t arg1;
125 };
126
127 /* function called when cmd_encoders is parsed successfully */
128 static void cmd_encoders_parsed(void *parsed_result, void *data)
129 {
130         struct cmd_encoders_result *res = parsed_result;
131
132         if (!strcmp_P(res->arg1, PSTR("reset"))) {
133                 encoders_spi_set_value((void *)0, 0);
134                 encoders_spi_set_value((void *)1, 0);
135                 encoders_spi_set_value((void *)2, 0);
136                 encoders_spi_set_value((void *)3, 0);
137                 return;
138         }
139
140         /* show */
141         while(!cmdline_keypressed()) {
142                 printf_P(PSTR("% .8ld % .8ld % .8ld % .8ld\r\n"), 
143                          encoders_spi_get_value((void *)0),
144                          encoders_spi_get_value((void *)1),
145                          encoders_spi_get_value((void *)2),
146                          encoders_spi_get_value((void *)3));
147                 wait_ms(100);
148         }
149 }
150
151 prog_char str_encoders_arg0[] = "encoders";
152 parse_pgm_token_string_t cmd_encoders_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg0, str_encoders_arg0);
153 prog_char str_encoders_arg1[] = "show#reset";
154 parse_pgm_token_string_t cmd_encoders_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_encoders_result, arg1, str_encoders_arg1);
155
156 prog_char help_encoders[] = "Show encoders values";
157 parse_pgm_inst_t cmd_encoders = {
158         .f = cmd_encoders_parsed,  /* function to call */
159         .data = NULL,      /* 2nd arg of func */
160         .help_str = help_encoders,
161         .tokens = {        /* token list, NULL terminated */
162                 (prog_void *)&cmd_encoders_arg0, 
163                 (prog_void *)&cmd_encoders_arg1, 
164                 NULL,
165         },
166 };
167
168 /**********************************************************/
169 /* Scheduler show */
170
171 /* this structure is filled when cmd_scheduler is parsed successfully */
172 struct cmd_scheduler_result {
173         fixed_string_t arg0;
174         fixed_string_t arg1;
175 };
176
177 /* function called when cmd_scheduler is parsed successfully */
178 static void cmd_scheduler_parsed(void *parsed_result, void *data)
179 {
180         scheduler_dump_events();
181         scheduler_stats_dump();
182 }
183
184 prog_char str_scheduler_arg0[] = "scheduler";
185 parse_pgm_token_string_t cmd_scheduler_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg0, str_scheduler_arg0);
186 prog_char str_scheduler_arg1[] = "show";
187 parse_pgm_token_string_t cmd_scheduler_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scheduler_result, arg1, str_scheduler_arg1);
188
189 prog_char help_scheduler[] = "Show scheduler events";
190 parse_pgm_inst_t cmd_scheduler = {
191         .f = cmd_scheduler_parsed,  /* function to call */
192         .data = NULL,      /* 2nd arg of func */
193         .help_str = help_scheduler,
194         .tokens = {        /* token list, NULL terminated */
195                 (prog_void *)&cmd_scheduler_arg0, 
196                 (prog_void *)&cmd_scheduler_arg1, 
197                 NULL,
198         },
199 };
200
201 /**********************************************************/
202 /* Pwms tests */
203
204 /* this structure is filled when cmd_pwm is parsed successfully */
205 struct cmd_pwm_result {
206         fixed_string_t arg0;
207         fixed_string_t arg1;
208         int16_t arg2;
209 };
210
211 /* function called when cmd_pwm is parsed successfully */
212 static void cmd_pwm_parsed(void * parsed_result, void * data)
213 {
214         void * pwm_ptr = NULL;
215         struct cmd_pwm_result * res = parsed_result;
216         
217         if (!strcmp_P(res->arg1, PSTR("1(4A)")))
218                 pwm_ptr = &gen.pwm1_4A;
219         else if (!strcmp_P(res->arg1, PSTR("2(4B)")))
220                 pwm_ptr = &gen.pwm2_4B;
221         else if (!strcmp_P(res->arg1, PSTR("3(1A)")))
222                 pwm_ptr = &gen.pwm3_1A;
223         else if (!strcmp_P(res->arg1, PSTR("4(1B)")))
224                 pwm_ptr = &gen.pwm4_1B;
225
226         else if (!strcmp_P(res->arg1, PSTR("s1(3C)")))
227                 pwm_ptr = &gen.servo1;
228         else if (!strcmp_P(res->arg1, PSTR("s2(5A)")))
229                 pwm_ptr = &gen.servo2;
230         else if (!strcmp_P(res->arg1, PSTR("s3(5B)")))
231                 pwm_ptr = &gen.servo3;
232         else if (!strcmp_P(res->arg1, PSTR("s3(5C)")))
233                 pwm_ptr = &gen.servo4;
234         
235         if (pwm_ptr)
236                 pwm_ng_set(pwm_ptr, res->arg2);
237
238         printf_P(PSTR("done\r\n"));
239 }
240
241 prog_char str_pwm_arg0[] = "pwm";
242 parse_pgm_token_string_t cmd_pwm_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg0, str_pwm_arg0);
243 prog_char str_pwm_arg1[] = "1(4A)#2(4B)#3(1A)#4(1B)#s1(3C)#s2(5A)#s3(5B)#s4(5C)";
244 parse_pgm_token_string_t cmd_pwm_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pwm_result, arg1, str_pwm_arg1);
245 parse_pgm_token_num_t cmd_pwm_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_pwm_result, arg2, INT16);
246
247 prog_char help_pwm[] = "Set pwm values [-4096 ; 4095]";
248 parse_pgm_inst_t cmd_pwm = {
249         .f = cmd_pwm_parsed,  /* function to call */
250         .data = NULL,      /* 2nd arg of func */
251         .help_str = help_pwm,
252         .tokens = {        /* token list, NULL terminated */
253                 (prog_void *)&cmd_pwm_arg0, 
254                 (prog_void *)&cmd_pwm_arg1, 
255                 (prog_void *)&cmd_pwm_arg2, 
256                 NULL,
257         },
258 };
259
260 /**********************************************************/
261 /* Adcs tests */
262
263 /* this structure is filled when cmd_adc is parsed successfully */
264 struct cmd_adc_result {
265         fixed_string_t arg0;
266         fixed_string_t arg1;
267 };
268
269 /* function called when cmd_adc is parsed successfully */
270 static void cmd_adc_parsed(void *parsed_result, void *data)
271 {
272         struct cmd_adc_result *res = parsed_result;
273         uint8_t i, loop = 0;
274
275         if (!strcmp_P(res->arg1, PSTR("loop_show")))
276                 loop = 1;
277         
278         do {
279                 printf_P(PSTR("ADC values: "));
280                 for (i=0; i<ADC_MAX; i++) {
281                         printf_P(PSTR("%.4d "), sensor_get_adc(i));
282                 }
283                 printf_P(PSTR("\r\n"));
284                 wait_ms(100);
285         } while (loop && !cmdline_keypressed());
286 }
287
288 prog_char str_adc_arg0[] = "adc";
289 parse_pgm_token_string_t cmd_adc_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg0, str_adc_arg0);
290 prog_char str_adc_arg1[] = "show#loop_show";
291 parse_pgm_token_string_t cmd_adc_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_adc_result, arg1, str_adc_arg1);
292
293 prog_char help_adc[] = "Show adc values";
294 parse_pgm_inst_t cmd_adc = {
295         .f = cmd_adc_parsed,  /* function to call */
296         .data = NULL,      /* 2nd arg of func */
297         .help_str = help_adc,
298         .tokens = {        /* token list, NULL terminated */
299                 (prog_void *)&cmd_adc_arg0, 
300                 (prog_void *)&cmd_adc_arg1, 
301                 NULL,
302         },
303 };
304
305
306 /**********************************************************/
307 /* Sensors tests */
308
309 /* this structure is filled when cmd_sensor is parsed successfully */
310 struct cmd_sensor_result {
311         fixed_string_t arg0;
312         fixed_string_t arg1;
313 };
314
315 /* function called when cmd_sensor is parsed successfully */
316 static void cmd_sensor_parsed(void *parsed_result, void *data)
317 {
318         struct cmd_sensor_result *res = parsed_result;
319         uint8_t i, loop = 0;
320
321         if (!strcmp_P(res->arg1, PSTR("loop_show")))
322                 loop = 1;
323         
324         do {
325                 printf_P(PSTR("SENSOR values: "));
326                 for (i=0; i<SENSOR_MAX; i++) {
327                         printf_P(PSTR("%d "), !!sensor_get(i));
328                 }
329                 printf_P(PSTR("\r\n"));
330                 wait_ms(100);
331         } while (loop && !cmdline_keypressed());
332 }
333
334 prog_char str_sensor_arg0[] = "sensor";
335 parse_pgm_token_string_t cmd_sensor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg0, str_sensor_arg0);
336 prog_char str_sensor_arg1[] = "show#loop_show";
337 parse_pgm_token_string_t cmd_sensor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_sensor_result, arg1, str_sensor_arg1);
338
339 prog_char help_sensor[] = "Show sensor values";
340 parse_pgm_inst_t cmd_sensor = {
341         .f = cmd_sensor_parsed,  /* function to call */
342         .data = NULL,      /* 2nd arg of func */
343         .help_str = help_sensor,
344         .tokens = {        /* token list, NULL terminated */
345                 (prog_void *)&cmd_sensor_arg0, 
346                 (prog_void *)&cmd_sensor_arg1, 
347                 NULL,
348         },
349 };
350
351
352 /**********************************************************/
353 /* Log */
354
355 /* this structure is filled when cmd_log is parsed successfully */
356 struct cmd_log_result {
357         fixed_string_t arg0;
358         fixed_string_t arg1;
359         uint8_t arg2;
360         fixed_string_t arg3;
361 };
362
363 /* keep it sync with string choice */
364 static const prog_char uart_log[] = "uart";
365 static const prog_char rs_log[] = "rs";
366 static const prog_char traj_log[] = "traj";
367 static const prog_char i2c_log[] = "i2c";
368 static const prog_char oa_log[] = "oa";
369 static const prog_char strat_log[] = "strat";
370 static const prog_char i2cproto_log[] = "i2cproto";
371 static const prog_char sensor_log[] = "sensor";
372 static const prog_char block_log[] = "bd";
373 static const prog_char cs_log[] = "cs";
374
375 struct log_name_and_num {
376         const prog_char * name;
377         uint8_t num;
378 };
379
380 static const struct log_name_and_num log_name_and_num[] = {
381         { uart_log, E_UART },
382         { rs_log, E_ROBOT_SYSTEM },
383         { traj_log, E_TRAJECTORY },
384         { i2c_log, E_I2C },
385         { oa_log, E_OA },
386         { strat_log, E_USER_STRAT },
387         { i2cproto_log, E_USER_I2C_PROTO },
388         { sensor_log, E_USER_SENSOR },
389         { block_log, E_BLOCKING_DETECTION_MANAGER },
390         { cs_log, E_USER_CS },
391 };
392
393 static uint8_t
394 log_name2num(const char * s)
395 {
396         uint8_t i;
397         
398         for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
399                 if (!strcmp_P(s, log_name_and_num[i].name)) {
400                         return log_name_and_num[i].num;
401                 }
402         }
403         return 0;
404 }
405
406 const prog_char *
407 log_num2name(uint8_t num)
408 {
409         uint8_t i;
410         
411         for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
412                 if (num ==  log_name_and_num[i].num) {
413                         return log_name_and_num[i].name;
414                 }
415         }
416         return NULL;
417 }
418
419 /* function called when cmd_log is parsed successfully */
420 static void cmd_log_do_show(void)
421 {
422         uint8_t i, empty=1;
423         const prog_char * name;
424
425         printf_P(PSTR("log level is %d\r\n"), gen.log_level);
426         for (i=0; i<NB_LOGS; i++) {
427                 name = log_num2name(gen.logs[i]);
428                 if (name) {
429                         printf_P(PSTR("log type %S is on\r\n"), name);
430                         empty = 0;
431                 }
432         }
433         if (empty)
434                 printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
435 }
436
437 /* function called when cmd_log is parsed successfully */
438 static void cmd_log_parsed(void * parsed_result, void * data)
439 {
440         struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
441
442         if (!strcmp_P(res->arg1, PSTR("level"))) {
443                 gen.log_level = res->arg2;
444         }
445
446         /* else it is a show */
447         cmd_log_do_show();
448 }
449
450 prog_char str_log_arg0[] = "log";
451 parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
452 prog_char str_log_arg1[] = "level";
453 parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
454 parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
455
456 prog_char help_log[] = "Set log options: level (0 -> 5)";
457 parse_pgm_inst_t cmd_log = {
458         .f = cmd_log_parsed,  /* function to call */
459         .data = NULL,      /* 2nd arg of func */
460         .help_str = help_log,
461         .tokens = {        /* token list, NULL terminated */
462                 (prog_void *)&cmd_log_arg0, 
463                 (prog_void *)&cmd_log_arg1, 
464                 (prog_void *)&cmd_log_arg2, 
465                 NULL,
466         },
467 };
468
469 prog_char str_log_arg1_show[] = "show";
470 parse_pgm_token_string_t cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
471
472 prog_char help_log_show[] = "Show configured logs";
473 parse_pgm_inst_t cmd_log_show = {
474         .f = cmd_log_parsed,  /* function to call */
475         .data = NULL,      /* 2nd arg of func */
476         .help_str = help_log_show,
477         .tokens = {        /* token list, NULL terminated */
478                 (prog_void *)&cmd_log_arg0, 
479                 (prog_void *)&cmd_log_arg1_show, 
480                 NULL,
481         },
482 };
483
484 /* this structure is filled when cmd_log is parsed successfully */
485 struct cmd_log_type_result {
486         fixed_string_t arg0;
487         fixed_string_t arg1;
488         fixed_string_t arg2;
489         fixed_string_t arg3;
490 };
491
492 /* function called when cmd_log is parsed successfully */
493 static void cmd_log_type_parsed(void * parsed_result, void * data)
494 {
495         struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
496         uint8_t lognum;
497         uint8_t i;
498         
499         lognum = log_name2num(res->arg2);
500         if (lognum == 0) {
501                 printf_P(PSTR("Cannot find log num\r\n"));
502                 return;
503         }
504
505         if (!strcmp_P(res->arg3, PSTR("on"))) {
506                 for (i=0; i<NB_LOGS; i++) {
507                         if (gen.logs[i] == lognum) {
508                                 printf_P(PSTR("Already on\r\n"));
509                                 return;
510                         }
511                 }
512                 for (i=0; i<NB_LOGS; i++) {
513                         if (gen.logs[i] == 0) {
514                                 gen.logs[i] = lognum;
515                                 break;
516                         }
517                 }
518                 if (i==NB_LOGS) {
519                         printf_P(PSTR("no more room\r\n"));
520                 }
521         }
522         else if (!strcmp_P(res->arg3, PSTR("off"))) {
523                 for (i=0; i<NB_LOGS; i++) {
524                         if (gen.logs[i] == lognum) {
525                                 gen.logs[i] = 0;
526                                 break;
527                         }
528                 }
529                 if (i==NB_LOGS) {
530                         printf_P(PSTR("already off\r\n"));
531                 }
532         }
533         cmd_log_do_show();
534 }
535
536 prog_char str_log_arg1_type[] = "type";
537 parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
538 /* keep it sync with log_name_and_num above */
539 prog_char str_log_arg2_type[] = "uart#rs#servo#traj#i2c#oa#strat#i2cproto#ext#sensor#bd#cs";
540 parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
541 prog_char str_log_arg3[] = "on#off";
542 parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
543
544 prog_char help_log_type[] = "Set log type";
545 parse_pgm_inst_t cmd_log_type = {
546         .f = cmd_log_type_parsed,  /* function to call */
547         .data = NULL,      /* 2nd arg of func */
548         .help_str = help_log_type,
549         .tokens = {        /* token list, NULL terminated */
550                 (prog_void *)&cmd_log_arg0,
551                 (prog_void *)&cmd_log_arg1_type,
552                 (prog_void *)&cmd_log_arg2_type,
553                 (prog_void *)&cmd_log_arg3,
554                 NULL,
555         },
556 };
557
558
559 /**********************************************************/
560 /* Stack_Space */
561
562 /* this structure is filled when cmd_stack_space is parsed successfully */
563 struct cmd_stack_space_result {
564         fixed_string_t arg0;
565 };
566
567 /* function called when cmd_stack_space is parsed successfully */
568 static void cmd_stack_space_parsed(void *parsed_result, void *data)
569 {
570         printf("res stack: %d\r\n", min_stack_space_available());
571         
572 }
573
574 prog_char str_stack_space_arg0[] = "stack_space";
575 parse_pgm_token_string_t cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
576
577 prog_char help_stack_space[] = "Display remaining stack space";
578 parse_pgm_inst_t cmd_stack_space = {
579         .f = cmd_stack_space_parsed,  /* function to call */
580         .data = NULL,      /* 2nd arg of func */
581         .help_str = help_stack_space,
582         .tokens = {        /* token list, NULL terminated */
583                 (prog_void *)&cmd_stack_space_arg0, 
584                 NULL,
585         },
586 };