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"))) {
68 bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
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");
86 if (!strcmp_P(res->arg1, PSTR("encoders")))
88 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
91 else if (!strcmp_P(res->arg1, PSTR("bd")))
93 else if (!strcmp_P(res->arg1, PSTR("power")))
97 if (!strcmp_P(res->arg2, PSTR("on")))
98 ballboard.flags |= bit;
99 else if (!strcmp_P(res->arg2, PSTR("off"))) {
100 if (!strcmp_P(res->arg1, PSTR("cs"))) {
101 pwm_ng_set(ROLLER_PWM, 0);
102 pwm_ng_set(FORKTRANS_PWM, 0);
103 pwm_ng_set(FORKROT_PWM, 0);
105 ballboard.flags &= (~bit);
107 printf_P(PSTR("%s is %s\r\n"), res->arg1,
108 (bit & ballboard.flags) ? "on":"off");
111 prog_char str_event_arg0[] = "event";
112 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
113 prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
114 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
115 prog_char str_event_arg2[] = "on#off#show";
116 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
118 prog_char help_event[] = "Enable/disable events";
119 parse_pgm_inst_t cmd_event = {
120 .f = cmd_event_parsed, /* function to call */
121 .data = NULL, /* 2nd arg of func */
122 .help_str = help_event,
123 .tokens = { /* token list, NULL terminated */
124 (prog_void *)&cmd_event_arg0,
125 (prog_void *)&cmd_event_arg1,
126 (prog_void *)&cmd_event_arg2,
131 /**********************************************************/
134 /* this structure is filled when cmd_color is parsed successfully */
135 struct cmd_color_result {
137 fixed_string_t color;
140 /* function called when cmd_color is parsed successfully */
141 static void cmd_color_parsed(void *parsed_result, void *data)
143 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
144 if (!strcmp_P(res->color, PSTR("yellow"))) {
145 ballboard.our_color = I2C_COLOR_YELLOW;
147 else if (!strcmp_P(res->color, PSTR("blue"))) {
148 ballboard.our_color = I2C_COLOR_BLUE;
150 printf_P(PSTR("Done\r\n"));
153 prog_char str_color_arg0[] = "color";
154 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
155 prog_char str_color_color[] = "blue#yellow";
156 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
158 prog_char help_color[] = "Set our color";
159 parse_pgm_inst_t cmd_color = {
160 .f = cmd_color_parsed, /* function to call */
161 .data = NULL, /* 2nd arg of func */
162 .help_str = help_color,
163 .tokens = { /* token list, NULL terminated */
164 (prog_void *)&cmd_color_arg0,
165 (prog_void *)&cmd_color_color,
171 /**********************************************************/
174 /* this structure is filled when cmd_state1 is parsed successfully */
175 struct cmd_state1_result {
180 /* function called when cmd_state1 is parsed successfully */
181 static void cmd_state1_parsed(void *parsed_result,
182 __attribute__((unused)) void *data)
184 struct cmd_state1_result *res = parsed_result;
186 if (!strcmp_P(res->arg1, PSTR("init")))
188 else if (!strcmp_P(res->arg1, PSTR("off")))
189 state_set_mode(I2C_BALLBOARD_MODE_OFF);
190 else if (!strcmp_P(res->arg1, PSTR("eject")))
191 state_set_mode(I2C_BALLBOARD_MODE_EJECT);
192 else if (!strcmp_P(res->arg1, PSTR("harvest")))
193 state_set_mode(I2C_BALLBOARD_MODE_HARVEST);
198 prog_char str_state1_arg0[] = "ballboard";
199 parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
200 prog_char str_state1_arg1[] = "init#eject#harvest#off";
201 parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
203 prog_char help_state1[] = "set ballboard mode";
204 parse_pgm_inst_t cmd_state1 = {
205 .f = cmd_state1_parsed, /* function to call */
206 .data = NULL, /* 2nd arg of func */
207 .help_str = help_state1,
208 .tokens = { /* token list, NULL terminated */
209 (prog_void *)&cmd_state1_arg0,
210 (prog_void *)&cmd_state1_arg1,
215 /**********************************************************/
218 /* this structure is filled when cmd_state2 is parsed successfully */
219 struct cmd_state2_result {
225 /* function called when cmd_state2 is parsed successfully */
226 static void cmd_state2_parsed(void *parsed_result,
227 __attribute__((unused)) void *data)
229 struct cmd_state2_result *res = parsed_result;
232 if (!strcmp_P(res->arg2, PSTR("left"))) {
233 if (!strcmp_P(res->arg1, PSTR("prepare")))
234 mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
235 else if (!strcmp_P(res->arg1, PSTR("take")))
236 mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
239 if (!strcmp_P(res->arg1, PSTR("prepare")))
240 mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
241 else if (!strcmp_P(res->arg1, PSTR("take")))
242 mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
244 //state_set_mode(mode);
247 prog_char str_state2_arg0[] = "ballboard";
248 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
249 prog_char str_state2_arg1[] = "prepare#take";
250 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
251 prog_char str_state2_arg2[] = "left#right";
252 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
254 prog_char help_state2[] = "set ballboard mode";
255 parse_pgm_inst_t cmd_state2 = {
256 .f = cmd_state2_parsed, /* function to call */
257 .data = NULL, /* 2nd arg of func */
258 .help_str = help_state2,
259 .tokens = { /* token list, NULL terminated */
260 (prog_void *)&cmd_state2_arg0,
261 (prog_void *)&cmd_state2_arg1,
262 (prog_void *)&cmd_state2_arg2,
267 /**********************************************************/
270 /* this structure is filled when cmd_state3 is parsed successfully */
271 struct cmd_state3_result {
277 /* function called when cmd_state3 is parsed successfully */
278 static void cmd_state3_parsed(void *parsed_result,
279 __attribute__((unused)) void *data)
281 struct cmd_state3_result *res = parsed_result;
283 if (!strcmp_P(res->arg1, PSTR("xxx"))) {
284 /* xxx = res->arg2 */
286 else if (!strcmp_P(res->arg1, PSTR("yyy"))) {
291 prog_char str_state3_arg0[] = "ballboard";
292 parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0);
293 prog_char str_state3_arg1[] = "xxx";
294 parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1);
295 parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, arg2, UINT8);
297 prog_char help_state3[] = "set ballboard mode";
298 parse_pgm_inst_t cmd_state3 = {
299 .f = cmd_state3_parsed, /* function to call */
300 .data = NULL, /* 2nd arg of func */
301 .help_str = help_state3,
302 .tokens = { /* token list, NULL terminated */
303 (prog_void *)&cmd_state3_arg0,
304 (prog_void *)&cmd_state3_arg1,
305 (prog_void *)&cmd_state3_arg2,
310 /**********************************************************/
313 /* this structure is filled when cmd_state_machine is parsed successfully */
314 struct cmd_state_machine_result {
318 /* function called when cmd_state_machine is parsed successfully */
319 static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result,
320 __attribute__((unused)) void *data)
325 prog_char str_state_machine_arg0[] = "state_machine";
326 parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0);
328 prog_char help_state_machine[] = "launch state machine";
329 parse_pgm_inst_t cmd_state_machine = {
330 .f = cmd_state_machine_parsed, /* function to call */
331 .data = NULL, /* 2nd arg of func */
332 .help_str = help_state_machine,
333 .tokens = { /* token list, NULL terminated */
334 (prog_void *)&cmd_state_machine_arg0,
339 /**********************************************************/
342 /* this structure is filled when cmd_state_debug is parsed successfully */
343 struct cmd_state_debug_result {
348 /* function called when cmd_state_debug is parsed successfully */
349 static void cmd_state_debug_parsed(void *parsed_result,
350 __attribute__((unused)) void *data)
352 struct cmd_state_debug_result *res = parsed_result;
353 state_debug = res->on;
356 prog_char str_state_debug_arg0[] = "state_debug";
357 parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0);
358 parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8);
360 prog_char help_state_debug[] = "Set debug for state machine";
361 parse_pgm_inst_t cmd_state_debug = {
362 .f = cmd_state_debug_parsed, /* function to call */
363 .data = NULL, /* 2nd arg of func */
364 .help_str = help_state_debug,
365 .tokens = { /* token list, NULL terminated */
366 (prog_void *)&cmd_state_debug_arg0,
367 (prog_void *)&cmd_state_debug_on,
372 /**********************************************************/
375 /* this structure is filled when cmd_fork is parsed successfully */
376 struct cmd_fork_result {
381 /* function called when cmd_fork is parsed successfully */
382 static void cmd_fork_parsed(void *parsed_result,
383 __attribute__((unused)) void *data)
385 struct cmd_fork_result *res = parsed_result;
386 if (!strcmp_P(res->arg1, PSTR("deploy")))
388 else if (!strcmp_P(res->arg1, PSTR("pack")))
390 else if (!strcmp_P(res->arg1, PSTR("left")))
392 else if (!strcmp_P(res->arg1, PSTR("right")))
394 else if (!strcmp_P(res->arg1, PSTR("middle")))
398 prog_char str_fork_arg0[] = "fork";
399 parse_pgm_token_string_t cmd_fork_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg0, str_fork_arg0);
400 prog_char str_fork_arg1[] = "deploy#pack#left#right#middle";
401 parse_pgm_token_string_t cmd_fork_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_fork_result, arg1, str_fork_arg1);
403 prog_char help_fork[] = "move fork";
404 parse_pgm_inst_t cmd_fork = {
405 .f = cmd_fork_parsed, /* function to call */
406 .data = NULL, /* 2nd arg of func */
407 .help_str = help_fork,
408 .tokens = { /* token list, NULL terminated */
409 (prog_void *)&cmd_fork_arg0,
410 (prog_void *)&cmd_fork_arg1,
415 /**********************************************************/
418 /* this structure is filled when cmd_roller is parsed successfully */
419 struct cmd_roller_result {
424 /* function called when cmd_roller is parsed successfully */
425 static void cmd_roller_parsed(void *parsed_result,
426 __attribute__((unused)) void *data)
428 struct cmd_roller_result *res = parsed_result;
429 if (!strcmp_P(res->arg1, PSTR("on")))
431 else if (!strcmp_P(res->arg1, PSTR("off")))
433 else if (!strcmp_P(res->arg1, PSTR("reverse")))
437 prog_char str_roller_arg0[] = "roller";
438 parse_pgm_token_string_t cmd_roller_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_roller_result, arg0, str_roller_arg0);
439 prog_char str_roller_arg1[] = "on#off#reverse";
440 parse_pgm_token_string_t cmd_roller_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_roller_result, arg1, str_roller_arg1);
442 prog_char help_roller[] = "Servo door function";
443 parse_pgm_inst_t cmd_roller = {
444 .f = cmd_roller_parsed, /* function to call */
445 .data = NULL, /* 2nd arg of func */
446 .help_str = help_roller,
447 .tokens = { /* token list, NULL terminated */
448 (prog_void *)&cmd_roller_arg0,
449 (prog_void *)&cmd_roller_arg1,
454 /**********************************************************/
457 /* this structure is filled when cmd_test is parsed successfully */
458 struct cmd_test_result {
462 /* function called when cmd_test is parsed successfully */
463 static void cmd_test_parsed(void *parsed_result, void *data)
465 //struct cmd_test_result *res = parsed_result;
468 prog_char str_test_arg0[] = "test";
469 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
471 prog_char help_test[] = "Test function";
472 parse_pgm_inst_t cmd_test = {
473 .f = cmd_test_parsed, /* function to call */
474 .data = NULL, /* 2nd arg of func */
475 .help_str = help_test,
476 .tokens = { /* token list, NULL terminated */
477 (prog_void *)&cmd_test_arg0,