cc268ba4407e2034eb1fda6728eb94b32ad63387
[aversive.git] / projects / microb2010 / tests / beacon_tsop / commands_cs.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_cs.c,v 1.4 2009-05-02 10:08:09 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 <uart.h>
31 #include <pwm_ng.h>
32 #include <pid.h>
33 #include <rdline.h>
34 #include <parse.h>
35 #include <parse_string.h>
36 #include <parse_num.h>
37
38 #include "main.h"
39 #include "cmdline.h"
40
41 extern uint32_t cs_consign;
42
43 /**********************************************************/
44 /* Gains for control system */
45
46 /* this structure is filled when cmd_gain is parsed successfully */
47 struct cmd_gain_result {
48         fixed_string_t arg0;
49         int16_t p;
50         int16_t i;
51         int16_t d;
52 };
53
54 /* function called when cmd_gain is parsed successfully */
55 static void cmd_gain_parsed(void * parsed_result, void *show)
56 {
57         struct cmd_gain_result *res = parsed_result;
58
59         if (!show) 
60                 pid_set_gains(&beacon_tsop.pid, res->p, res->i, res->d);
61
62         printf_P(PSTR("gain %d %d %d\r\n"),
63                  pid_get_gain_P(&beacon_tsop.pid),
64                  pid_get_gain_I(&beacon_tsop.pid),
65                  pid_get_gain_D(&beacon_tsop.pid));
66 }
67
68 prog_char str_gain_arg0[] = "gain";
69 parse_pgm_token_string_t cmd_gain_arg0 =
70         TOKEN_STRING_INITIALIZER(struct cmd_gain_result,
71                                  arg0, str_gain_arg0);
72 parse_pgm_token_num_t cmd_gain_p = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, p, INT16);
73 parse_pgm_token_num_t cmd_gain_i = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, i, INT16);
74 parse_pgm_token_num_t cmd_gain_d = TOKEN_NUM_INITIALIZER(struct cmd_gain_result, d, INT16);
75
76 prog_char help_gain[] = "Set gain values for PID";
77 parse_pgm_inst_t cmd_gain = {
78         .f = cmd_gain_parsed,  /* function to call */
79         .data = NULL,      /* 2nd arg of func */
80         .help_str = help_gain,
81         .tokens = {        /* token list, NULL terminated */
82                 (prog_void *)&cmd_gain_arg0, 
83                 (prog_void *)&cmd_gain_p, 
84                 (prog_void *)&cmd_gain_i, 
85                 (prog_void *)&cmd_gain_d, 
86                 NULL,
87         },
88 };
89
90 /* show */
91 /* this structure is filled when cmd_gain is parsed successfully */
92 struct cmd_gain_show_result {
93         fixed_string_t arg0;
94         fixed_string_t show;
95 };
96
97 prog_char str_gain_show_arg[] = "show";
98 parse_pgm_token_string_t cmd_gain_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_gain_show_result, show, str_gain_show_arg);
99
100 prog_char help_gain_show[] = "Show gain values for PID";
101 parse_pgm_inst_t cmd_gain_show = {
102         .f = cmd_gain_parsed,  /* function to call */
103         .data = (void *)1,      /* 2nd arg of func */
104         .help_str = help_gain_show,
105         .tokens = {        /* token list, NULL terminated */
106                 (prog_void *)&cmd_gain_arg0, 
107                 (prog_void *)&cmd_gain_show_arg,
108                 NULL,
109         },
110 };
111
112 /**********************************************************/
113 /* Derivate_Filters for control system */
114
115 /* this structure is filled when cmd_derivate_filter is parsed successfully */
116 struct cmd_derivate_filter_result {
117         fixed_string_t arg0;
118         uint8_t size;
119 };
120
121 /* function called when cmd_derivate_filter is parsed successfully */
122 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
123 {
124         struct cmd_derivate_filter_result * res = parsed_result;
125
126         if (!show) 
127                 pid_set_derivate_filter(&beacon_tsop.pid, res->size);
128
129         printf_P(PSTR("derivate_filter %u\r\n"), 
130                  pid_get_derivate_filter(&beacon_tsop.pid));
131 }
132
133 prog_char str_derivate_filter_arg0[] = "derivate_filter";
134 parse_pgm_token_string_t cmd_derivate_filter_arg0 =
135         TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result,
136                                  arg0, str_derivate_filter_arg0);
137 parse_pgm_token_num_t cmd_derivate_filter_size =
138         TOKEN_NUM_INITIALIZER(struct cmd_derivate_filter_result, size, UINT32);
139
140 prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I, out)";
141 parse_pgm_inst_t cmd_derivate_filter = {
142         .f = cmd_derivate_filter_parsed,  /* function to call */
143         .data = (void *)1,      /* 2nd arg of func */
144         .help_str = help_derivate_filter,
145         .tokens = {        /* token list, NULL terminated */
146                 (prog_void *)&cmd_derivate_filter_arg0, 
147                 (prog_void *)&cmd_derivate_filter_size, 
148                 NULL,
149         },
150 };
151
152 /* show */
153
154 struct cmd_derivate_filter_show_result {
155         fixed_string_t arg0;
156         fixed_string_t show;
157 };
158
159 prog_char str_derivate_filter_show_arg[] = "show";
160 parse_pgm_token_string_t cmd_derivate_filter_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_show_result, show, str_derivate_filter_show_arg);
161
162 prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
163 parse_pgm_inst_t cmd_derivate_filter_show = {
164         .f = cmd_derivate_filter_parsed,  /* function to call */
165         .data = NULL,      /* 2nd arg of func */
166         .help_str = help_derivate_filter_show,
167         .tokens = {        /* token list, NULL terminated */
168                 (prog_void *)&cmd_derivate_filter_arg0, 
169                 (prog_void *)&cmd_derivate_filter_show_arg,
170                 NULL,
171         },
172 };
173
174
175 /**********************************************************/
176 /* Maximums for control system */
177
178 /* this structure is filled when cmd_maximum is parsed successfully */
179 struct cmd_maximum_result {
180         fixed_string_t arg0;
181         uint32_t in;
182         uint32_t i;
183         uint32_t out;
184 };
185
186 /* function called when cmd_maximum is parsed successfully */
187 static void cmd_maximum_parsed(void *parsed_result, void *show)
188 {
189         struct cmd_maximum_result * res = parsed_result;
190         
191         if (!show)
192                 pid_set_maximums(&beacon_tsop.pid, res->in, res->i, res->out);
193
194         printf_P(PSTR("maximum %lu %lu %lu\r\n"), 
195                  pid_get_max_in(&beacon_tsop.pid),
196                  pid_get_max_I(&beacon_tsop.pid),
197                  pid_get_max_out(&beacon_tsop.pid));
198 }
199
200 prog_char str_maximum_arg0[] = "maximum";
201 parse_pgm_token_string_t cmd_maximum_arg0 =
202         TOKEN_STRING_INITIALIZER(struct cmd_maximum_result,
203                                  arg0, str_maximum_arg0);
204 parse_pgm_token_num_t cmd_maximum_in =
205         TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, in, UINT32);
206 parse_pgm_token_num_t cmd_maximum_i =
207         TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, i, UINT32);
208 parse_pgm_token_num_t cmd_maximum_out =
209         TOKEN_NUM_INITIALIZER(struct cmd_maximum_result, out, UINT32);
210
211 prog_char help_maximum[] = "Set maximum values for PID (in, I, out)";
212 parse_pgm_inst_t cmd_maximum = {
213         .f = cmd_maximum_parsed,  /* function to call */
214         .data = NULL,      /* 2nd arg of func */
215         .help_str = help_maximum,
216         .tokens = {        /* token list, NULL terminated */
217                 (prog_void *)&cmd_maximum_arg0, 
218                 (prog_void *)&cmd_maximum_in, 
219                 (prog_void *)&cmd_maximum_i, 
220                 (prog_void *)&cmd_maximum_out, 
221                 NULL,
222         },
223 };
224
225 /* show */
226
227 /* this structure is filled when cmd_maximum is parsed successfully */
228 struct cmd_maximum_show_result {
229         fixed_string_t arg0;
230         fixed_string_t show;
231 };
232 prog_char str_maximum_show_arg[] = "show";
233 parse_pgm_token_string_t cmd_maximum_show_arg =
234         TOKEN_STRING_INITIALIZER(struct cmd_maximum_show_result, show, str_maximum_show_arg);
235
236 prog_char help_maximum_show[] = "Show maximum values for PID";
237 parse_pgm_inst_t cmd_maximum_show = {
238         .f = cmd_maximum_parsed,  /* function to call */
239         .data = (void *)1,      /* 2nd arg of func */
240         .help_str = help_maximum_show,
241         .tokens = {        /* token list, NULL terminated */
242                 (prog_void *)&cmd_maximum_arg0, 
243                 (prog_void *)&cmd_maximum_show_arg,
244                 NULL,
245         },
246 };
247
248
249 /**********************************************************/
250 /* Consigns for control system */
251
252 /* this structure is filled when cmd_consign is parsed successfully */
253 struct cmd_consign_result {
254         fixed_string_t arg0;
255         uint32_t cons;
256 };
257
258 /* function called when cmd_consign is parsed successfully */
259 static void cmd_consign_parsed(void *parsed_result, void *show)
260 {
261         struct cmd_consign_result * res = parsed_result;
262         
263         if (!show)
264                 cs_consign = res->cons;
265
266         printf_P(PSTR("consign %lu\r\n"), cs_consign);
267 }
268
269 prog_char str_consign_arg0[] = "consign";
270 parse_pgm_token_string_t cmd_consign_arg0 =
271         TOKEN_STRING_INITIALIZER(struct cmd_consign_result,
272                                  arg0, str_consign_arg0);
273 parse_pgm_token_num_t cmd_consign_cons =
274         TOKEN_NUM_INITIALIZER(struct cmd_consign_result, cons, UINT32);
275
276 prog_char help_consign[] = "Set consign";
277 parse_pgm_inst_t cmd_consign = {
278         .f = cmd_consign_parsed,  /* function to call */
279         .data = NULL,      /* 2nd arg of func */
280         .help_str = help_consign,
281         .tokens = {        /* token list, NULL terminated */
282                 (prog_void *)&cmd_consign_arg0, 
283                 (prog_void *)&cmd_consign_cons, 
284                 NULL,
285         },
286 };
287
288 /* show */
289
290 /* this structure is filled when cmd_consign is parsed successfully */
291 struct cmd_consign_show_result {
292         fixed_string_t arg0;
293         fixed_string_t show;
294 };
295 prog_char str_consign_show_arg[] = "show";
296 parse_pgm_token_string_t cmd_consign_show_arg =
297         TOKEN_STRING_INITIALIZER(struct cmd_consign_show_result, show, str_consign_show_arg);
298
299 prog_char help_consign_show[] = "Show consign values for PID";
300 parse_pgm_inst_t cmd_consign_show = {
301         .f = cmd_consign_parsed,  /* function to call */
302         .data = (void *)1,      /* 2nd arg of func */
303         .help_str = help_consign_show,
304         .tokens = {        /* token list, NULL terminated */
305                 (prog_void *)&cmd_consign_arg0, 
306                 (prog_void *)&cmd_consign_show_arg,
307                 NULL,
308         },
309 };
310