2 * Copyright Droids Corporation (2008)
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.
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.
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
18 * Revision : $Id: commands_cs.c,v 1.4 2009-05-02 10:08:09 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
35 #include <parse_string.h>
36 #include <parse_num.h>
41 extern uint32_t cs_consign;
43 /**********************************************************/
44 /* Gains for control system */
46 /* this structure is filled when cmd_gain is parsed successfully */
47 struct cmd_gain_result {
54 /* function called when cmd_gain is parsed successfully */
55 static void cmd_gain_parsed(void * parsed_result, void *show)
57 struct cmd_gain_result *res = parsed_result;
60 pid_set_gains(&beacon_tsop.pid, res->p, res->i, res->d);
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));
68 prog_char str_gain_arg0[] = "gain";
69 parse_pgm_token_string_t cmd_gain_arg0 =
70 TOKEN_STRING_INITIALIZER(struct cmd_gain_result,
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);
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,
91 /* this structure is filled when cmd_gain is parsed successfully */
92 struct cmd_gain_show_result {
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);
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,
112 /**********************************************************/
113 /* Derivate_Filters for control system */
115 /* this structure is filled when cmd_derivate_filter is parsed successfully */
116 struct cmd_derivate_filter_result {
121 /* function called when cmd_derivate_filter is parsed successfully */
122 static void cmd_derivate_filter_parsed(void *parsed_result, void *show)
124 struct cmd_derivate_filter_result * res = parsed_result;
127 pid_set_derivate_filter(&beacon_tsop.pid, res->size);
129 printf_P(PSTR("derivate_filter %u\r\n"),
130 pid_get_derivate_filter(&beacon_tsop.pid));
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);
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,
154 struct cmd_derivate_filter_show_result {
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);
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,
175 /**********************************************************/
176 /* Maximums for control system */
178 /* this structure is filled when cmd_maximum is parsed successfully */
179 struct cmd_maximum_result {
186 /* function called when cmd_maximum is parsed successfully */
187 static void cmd_maximum_parsed(void *parsed_result, void *show)
189 struct cmd_maximum_result * res = parsed_result;
192 pid_set_maximums(&beacon_tsop.pid, res->in, res->i, res->out);
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));
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);
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,
227 /* this structure is filled when cmd_maximum is parsed successfully */
228 struct cmd_maximum_show_result {
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);
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,
249 /**********************************************************/
250 /* Consigns for control system */
252 /* this structure is filled when cmd_consign is parsed successfully */
253 struct cmd_consign_result {
258 /* function called when cmd_consign is parsed successfully */
259 static void cmd_consign_parsed(void *parsed_result, void *show)
261 struct cmd_consign_result * res = parsed_result;
264 cs_consign = res->cons;
266 printf_P(PSTR("consign %lu\r\n"), cs_consign);
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);
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,
290 /* this structure is filled when cmd_consign is parsed successfully */
291 struct cmd_consign_show_result {
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);
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,