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