2 * Copyright Droids Corporation (2009)
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_ballboard.c,v 1.2 2009-04-24 19:30:42 zer0 Exp $
20 * Olivier MATZ <zer0@droids-corp.org>
26 #include <aversive/pgmspace.h>
27 #include <aversive/wait.h>
28 #include <aversive/error.h>
33 #include <clock_time.h>
38 #include <control_system_manager.h>
39 #include <blocking_detection_manager.h>
43 #include <parse_string.h>
44 #include <parse_num.h>
49 #include "../common/i2c_commands.h"
50 #include "i2c_protocol.h"
53 struct cmd_event_result {
60 /* function called when cmd_event is parsed successfully */
61 static void cmd_event_parsed(void *parsed_result, void *data)
65 struct cmd_event_result * res = parsed_result;
67 if (!strcmp_P(res->arg1, PSTR("all"))) {
69 if (!strcmp_P(res->arg2, PSTR("on")))
70 ballboard.flags |= bit;
71 else if (!strcmp_P(res->arg2, PSTR("off")))
72 ballboard.flags &= bit;
74 printf_P(PSTR("encoders is %s\r\n"),
75 (DO_ENCODERS & ballboard.flags) ? "on":"off");
76 printf_P(PSTR("cs is %s\r\n"),
77 (DO_CS & ballboard.flags) ? "on":"off");
78 printf_P(PSTR("bd is %s\r\n"),
79 (DO_BD & ballboard.flags) ? "on":"off");
80 printf_P(PSTR("power is %s\r\n"),
81 (DO_POWER & ballboard.flags) ? "on":"off");
82 printf_P(PSTR("errblock is %s\r\n"),
83 (DO_ERRBLOCKING & ballboard.flags) ? "on":"off");
88 if (!strcmp_P(res->arg1, PSTR("encoders")))
90 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
93 else if (!strcmp_P(res->arg1, PSTR("bd")))
95 else if (!strcmp_P(res->arg1, PSTR("power")))
97 else if (!strcmp_P(res->arg1, PSTR("errblock")))
101 if (!strcmp_P(res->arg2, PSTR("on")))
102 ballboard.flags |= bit;
103 else if (!strcmp_P(res->arg2, PSTR("off"))) {
104 if (!strcmp_P(res->arg1, PSTR("cs"))) {
105 pwm_ng_set(ROLLER_PWM, 0);
106 pwm_ng_set(FORKTRANS_PWM, 0);
107 pwm_ng_set(FORKROT_PWM, 0);
108 pwm_ng_set(BEACON_PWM, 0);
110 ballboard.flags &= (~bit);
112 printf_P(PSTR("%s is %s\r\n"), res->arg1,
113 (bit & ballboard.flags) ? "on":"off");
116 prog_char str_event_arg0[] = "event";
117 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
118 prog_char str_event_arg1[] = "all#encoders#cs#bd#power#errblock";
119 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
120 prog_char str_event_arg2[] = "on#off#show";
121 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
123 prog_char help_event[] = "Enable/disable events";
124 parse_pgm_inst_t cmd_event = {
125 .f = cmd_event_parsed, /* function to call */
126 .data = NULL, /* 2nd arg of func */
127 .help_str = help_event,
128 .tokens = { /* token list, NULL terminated */
129 (prog_void *)&cmd_event_arg0,
130 (prog_void *)&cmd_event_arg1,
131 (prog_void *)&cmd_event_arg2,
136 /**********************************************************/
139 /* this structure is filled when cmd_color is parsed successfully */
140 struct cmd_color_result {
142 fixed_string_t color;
145 /* function called when cmd_color is parsed successfully */
146 static void cmd_color_parsed(void *parsed_result, void *data)
148 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
149 if (!strcmp_P(res->color, PSTR("yellow"))) {
150 ballboard.our_color = I2C_COLOR_YELLOW;
152 else if (!strcmp_P(res->color, PSTR("blue"))) {
153 ballboard.our_color = I2C_COLOR_BLUE;
155 printf_P(PSTR("Done\r\n"));
158 prog_char str_color_arg0[] = "color";
159 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
160 prog_char str_color_color[] = "blue#yellow";
161 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
163 prog_char help_color[] = "Set our color";
164 parse_pgm_inst_t cmd_color = {
165 .f = cmd_color_parsed, /* function to call */
166 .data = NULL, /* 2nd arg of func */
167 .help_str = help_color,
168 .tokens = { /* token list, NULL terminated */
169 (prog_void *)&cmd_color_arg0,
170 (prog_void *)&cmd_color_color,
176 /**********************************************************/
179 /* this structure is filled when cmd_state1 is parsed successfully */
180 struct cmd_state1_result {
185 /* function called when cmd_state1 is parsed successfully */
186 static void cmd_state1_parsed(void *parsed_result,
187 __attribute__((unused)) void *data)
189 struct cmd_state1_result *res = parsed_result;
191 if (!strcmp_P(res->arg1, PSTR("init")))
193 else if (!strcmp_P(res->arg1, PSTR("off")))
194 state_set_mode(I2C_BALLBOARD_MODE_OFF);
195 else if (!strcmp_P(res->arg1, PSTR("eject")))
196 state_set_mode(I2C_BALLBOARD_MODE_EJECT);
197 else if (!strcmp_P(res->arg1, PSTR("harvest")))
198 state_set_mode(I2C_BALLBOARD_MODE_HARVEST);
199 else if (!strcmp_P(res->arg1, PSTR("prepare")))
200 state_set_mode(I2C_BALLBOARD_MODE_PREP_FORK);
201 else if (!strcmp_P(res->arg1, PSTR("take")))
202 state_set_mode(I2C_BALLBOARD_MODE_TAKE_FORK);
207 prog_char str_state1_arg0[] = "ballboard";
208 parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
209 prog_char str_state1_arg1[] = "init#eject#harvest#off#prepare#take";
210 parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
212 prog_char help_state1[] = "set ballboard mode";
213 parse_pgm_inst_t cmd_state1 = {
214 .f = cmd_state1_parsed, /* function to call */
215 .data = NULL, /* 2nd arg of func */
216 .help_str = help_state1,
217 .tokens = { /* token list, NULL terminated */
218 (prog_void *)&cmd_state1_arg0,
219 (prog_void *)&cmd_state1_arg1,
224 /**********************************************************/
227 /* this structure is filled when cmd_state2 is parsed successfully */
228 struct cmd_state2_result {
234 /* function called when cmd_state2 is parsed successfully */
235 static void cmd_state2_parsed(void *parsed_result,
236 __attribute__((unused)) void *data)
240 prog_char str_state2_arg0[] = "ballboard";
241 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
242 prog_char str_state2_arg1[] = "xxx";
243 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
244 prog_char str_state2_arg2[] = "left#right";
245 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
247 prog_char help_state2[] = "set ballboard mode";
248 parse_pgm_inst_t cmd_state2 = {
249 .f = cmd_state2_parsed, /* function to call */
250 .data = NULL, /* 2nd arg of func */
251 .help_str = help_state2,
252 .tokens = { /* token list, NULL terminated */
253 (prog_void *)&cmd_state2_arg0,
254 (prog_void *)&cmd_state2_arg1,
255 (prog_void *)&cmd_state2_arg2,
260 /**********************************************************/
263 /* this structure is filled when cmd_state3 is parsed successfully */
264 struct cmd_state3_result {
270 /* function called when cmd_state3 is parsed successfully */
271 static void cmd_state3_parsed(void *parsed_result,
272 __attribute__((unused)) void *data)
274 struct cmd_state3_result *res = parsed_result;
276 if (!strcmp_P(res->arg1, PSTR("xxx"))) {
277 /* xxx = res->arg2 */
279 else if (!strcmp_P(res->arg1, PSTR("yyy"))) {
284 prog_char str_state3_arg0[] = "ballboard";
285 parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0);
286 prog_char str_state3_arg1[] = "xxx";
287 parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1);
288 parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, arg2, UINT8);
290 prog_char help_state3[] = "set ballboard mode";
291 parse_pgm_inst_t cmd_state3 = {
292 .f = cmd_state3_parsed, /* function to call */
293 .data = NULL, /* 2nd arg of func */
294 .help_str = help_state3,
295 .tokens = { /* token list, NULL terminated */
296 (prog_void *)&cmd_state3_arg0,
297 (prog_void *)&cmd_state3_arg1,
298 (prog_void *)&cmd_state3_arg2,
303 /**********************************************************/
306 /* this structure is filled when cmd_state_machine is parsed successfully */
307 struct cmd_state_machine_result {
311 /* function called when cmd_state_machine is parsed successfully */
312 static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result,
313 __attribute__((unused)) void *data)
318 prog_char str_state_machine_arg0[] = "state_machine";
319 parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0);
321 prog_char help_state_machine[] = "launch state machine";
322 parse_pgm_inst_t cmd_state_machine = {
323 .f = cmd_state_machine_parsed, /* function to call */
324 .data = NULL, /* 2nd arg of func */
325 .help_str = help_state_machine,
326 .tokens = { /* token list, NULL terminated */
327 (prog_void *)&cmd_state_machine_arg0,
332 /**********************************************************/
335 /* this structure is filled when cmd_state_debug is parsed successfully */
336 struct cmd_state_debug_result {
341 /* function called when cmd_state_debug is parsed successfully */
342 static void cmd_state_debug_parsed(void *parsed_result,
343 __attribute__((unused)) void *data)
345 struct cmd_state_debug_result *res = parsed_result;
346 state_debug = res->on;
349 prog_char str_state_debug_arg0[] = "state_debug";
350 parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0);
351 parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8);
353 prog_char help_state_debug[] = "Set debug for state machine";
354 parse_pgm_inst_t cmd_state_debug = {
355 .f = cmd_state_debug_parsed, /* function to call */
356 .data = NULL, /* 2nd arg of func */
357 .help_str = help_state_debug,
358 .tokens = { /* token list, NULL terminated */
359 (prog_void *)&cmd_state_debug_arg0,
360 (prog_void *)&cmd_state_debug_on,
365 /**********************************************************/
368 /* this structure is filled when cmd_fork is parsed successfully */
369 struct cmd_fork_result {
374 /* function called when cmd_fork is parsed successfully */
375 static void cmd_fork_parsed(void *parsed_result,
376 __attribute__((unused)) void *data)
378 struct cmd_fork_result *res = parsed_result;
379 if (!strcmp_P(res->arg1, PSTR("deploy")))
381 else if (!strcmp_P(res->arg1, PSTR("pack")))
383 else if (!strcmp_P(res->arg1, PSTR("mid1")))
385 else if (!strcmp_P(res->arg1, PSTR("mid2")))
389 prog_char str_fork_arg0[] = "fork";
390 parse_pgm_token_string_t cmd_fork_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg0, str_fork_arg0);
391 prog_char str_fork_arg1[] = "deploy#pack#mid1#mid2";
392 parse_pgm_token_string_t cmd_fork_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg1, str_fork_arg1);
394 prog_char help_fork[] = "move fork";
395 parse_pgm_inst_t cmd_fork = {
396 .f = cmd_fork_parsed, /* function to call */
397 .data = NULL, /* 2nd arg of func */
398 .help_str = help_fork,
399 .tokens = { /* token list, NULL terminated */
400 (prog_void *)&cmd_fork_arg0,
401 (prog_void *)&cmd_fork_arg1,
406 /**********************************************************/
409 /* this structure is filled when cmd_roller is parsed successfully */
410 struct cmd_roller_result {
415 /* function called when cmd_roller is parsed successfully */
416 static void cmd_roller_parsed(void *parsed_result,
417 __attribute__((unused)) void *data)
419 struct cmd_roller_result *res = parsed_result;
420 if (!strcmp_P(res->arg1, PSTR("on")))
422 else if (!strcmp_P(res->arg1, PSTR("off")))
424 else if (!strcmp_P(res->arg1, PSTR("reverse")))
428 prog_char str_roller_arg0[] = "roller";
429 parse_pgm_token_string_t cmd_roller_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_roller_result, arg0, str_roller_arg0);
430 prog_char str_roller_arg1[] = "on#off#reverse";
431 parse_pgm_token_string_t cmd_roller_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_roller_result, arg1, str_roller_arg1);
433 prog_char help_roller[] = "Servo door function";
434 parse_pgm_inst_t cmd_roller = {
435 .f = cmd_roller_parsed, /* function to call */
436 .data = NULL, /* 2nd arg of func */
437 .help_str = help_roller,
438 .tokens = { /* token list, NULL terminated */
439 (prog_void *)&cmd_roller_arg0,
440 (prog_void *)&cmd_roller_arg1,
445 /**********************************************************/
448 /* this structure is filled when cmd_test is parsed successfully */
449 struct cmd_test_result {
453 /* function called when cmd_test is parsed successfully */
454 static void cmd_test_parsed(void *parsed_result, void *data)
456 //struct cmd_test_result *res = parsed_result;
459 prog_char str_test_arg0[] = "test";
460 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
462 prog_char help_test[] = "Test function";
463 parse_pgm_inst_t cmd_test = {
464 .f = cmd_test_parsed, /* function to call */
465 .data = NULL, /* 2nd arg of func */
466 .help_str = help_test,
467 .tokens = { /* token list, NULL terminated */
468 (prog_void *)&cmd_test_arg0,