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