better cobboard commands in strat_event
[aversive.git] / projects / microb2010 / cobboard / cmdline.c
1 /*  
2  *  Copyright Droids Corporation
3  *  Olivier Matz <zer0@droids-corp.org>
4  * 
5  *  This program is free software; you can redistribute it and/or modify
6  *  it under the terms of the GNU General Public License as published by
7  *  the Free Software Foundation; either version 2 of the License, or
8  *  (at your option) any later version.
9  *
10  *  This program is distributed in the hope that it will be useful,
11  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
12  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  *  GNU General Public License for more details.
14  *
15  *  You should have received a copy of the GNU General Public License
16  *  along with this program; if not, write to the Free Software
17  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
18  *
19  *  Revision : $Id: cmdline.c,v 1.3 2009-04-24 19:30:42 zer0 Exp $
20  *
21  */
22
23 #include <stdio.h>
24 #include <string.h>
25
26 #include <aversive.h>
27 #include <aversive/error.h>
28
29 #include <parse.h>
30 #include <rdline.h>
31 #include <ax12.h>
32 #include <uart.h>
33 #include <pwm_ng.h>
34 #include <clock_time.h>
35
36 #include <pid.h>
37 #include <quadramp.h>
38 #include <control_system_manager.h>
39 #include <blocking_detection_manager.h>
40
41 #include "main.h"
42 #include "cmdline.h"
43
44
45 /******** See in commands.c for the list of commands. */
46 extern parse_pgm_ctx_t main_ctx[];
47
48 static void write_char(char c) 
49 {
50         uart_send(CMDLINE_UART, c);
51 }
52
53 static void 
54 valid_buffer(const char *buf, __attribute__((unused)) uint8_t size) 
55 {
56         int8_t ret;
57         ret = parse(main_ctx, buf);
58         if (ret == PARSE_AMBIGUOUS)
59                 printf_P(PSTR("Ambiguous command\r\n"));
60         else if (ret == PARSE_NOMATCH)
61                 printf_P(PSTR("Command not found\r\n"));
62         else if (ret == PARSE_BAD_ARGS)
63                 printf_P(PSTR("Bad arguments\r\n"));
64 }
65
66 static int8_t 
67 complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
68                 int16_t *state)
69 {
70         return complete(main_ctx, buf, state, dstbuf, dstsize);
71 }
72
73
74 /* sending "pop" on cmdline uart resets the robot */
75 void emergency(char c)
76 {
77         static uint8_t i = 0;
78         
79         if ((i == 0 && c == 'p') ||
80             (i == 1 && c == 'o') ||
81             (i == 2 && c == 'p')) 
82                 i++;
83         else if ( !(i == 1 && c == 'p') )
84                 i = 0;
85         if (i == 3)
86                 reset();
87 }
88
89 /* log function, add a command to configure
90  * it dynamically */
91 void mylog(struct error * e, ...) 
92 {
93         va_list ap;
94         u16 stream_flags = stdout->flags;
95         uint8_t i;
96         time_h tv;
97
98         if (e->severity > ERROR_SEVERITY_ERROR) {
99                 if (gen.log_level < e->severity)
100                         return;
101                 
102                 for (i=0; i<NB_LOGS+1; i++)
103                         if (gen.logs[i] == e->err_num)
104                                 break;
105                 if (i == NB_LOGS+1)
106                         return;
107         }
108
109         va_start(ap, e);
110         tv = time_get_time();
111         printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
112         vfprintf_P(stdout, e->text, ap);
113         printf_P(PSTR("\r\n"));
114         va_end(ap);
115         stdout->flags = stream_flags;
116 }
117
118 int cmdline_interact(void)
119 {
120         const char *history, *buffer;
121         int8_t ret, same = 0;
122         int16_t c;
123         
124         rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
125         snprintf(gen.prompt, sizeof(gen.prompt), "cobboard > ");        
126         rdline_newline(&gen.rdl, gen.prompt);
127
128         while (1) {
129                 c = uart_recv_nowait(CMDLINE_UART);
130                 if (c == -1) 
131                         continue;
132                 ret = rdline_char_in(&gen.rdl, c);
133                 if (ret != 2 && ret != 0) {
134                         buffer = rdline_get_buffer(&gen.rdl);
135                         history = rdline_get_history_item(&gen.rdl, 0);
136                         if (history) {
137                                 same = !memcmp(buffer, history, strlen(history)) &&
138                                         buffer[strlen(history)] == '\n';
139                         }
140                         else
141                                 same = 0;
142                         if (strlen(buffer) > 1 && !same)
143                                 rdline_add_history(&gen.rdl, buffer);
144                         rdline_newline(&gen.rdl, gen.prompt);
145                 }
146         }
147
148         return 0;
149 }