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