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_cobboard.c,v 1.6 2009-11-08 17:25:00 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>
46 #include "../common/i2c_commands.h"
51 #include "i2c_protocol.h"
55 extern uint16_t state_debug;
57 struct cmd_event_result {
64 /* function called when cmd_event is parsed successfully */
65 static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void *data)
69 struct cmd_event_result * res = parsed_result;
71 if (!strcmp_P(res->arg1, PSTR("all"))) {
72 bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER;
73 if (!strcmp_P(res->arg2, PSTR("on")))
74 cobboard.flags |= bit;
75 else if (!strcmp_P(res->arg2, PSTR("off")))
76 cobboard.flags &= bit;
78 printf_P(PSTR("encoders is %s\r\n"),
79 (DO_ENCODERS & cobboard.flags) ? "on":"off");
80 printf_P(PSTR("cs is %s\r\n"),
81 (DO_CS & cobboard.flags) ? "on":"off");
82 printf_P(PSTR("bd is %s\r\n"),
83 (DO_BD & cobboard.flags) ? "on":"off");
84 printf_P(PSTR("power is %s\r\n"),
85 (DO_POWER & cobboard.flags) ? "on":"off");
90 if (!strcmp_P(res->arg1, PSTR("encoders")))
92 else if (!strcmp_P(res->arg1, PSTR("cs"))) {
95 else if (!strcmp_P(res->arg1, PSTR("bd")))
97 else if (!strcmp_P(res->arg1, PSTR("power")))
101 if (!strcmp_P(res->arg2, PSTR("on")))
102 cobboard.flags |= bit;
103 else if (!strcmp_P(res->arg2, PSTR("off"))) {
104 if (!strcmp_P(res->arg1, PSTR("cs"))) {
105 pwm_ng_set(LEFT_SPICKLE_PWM, 0);
106 pwm_ng_set(RIGHT_SPICKLE_PWM, 0);
107 pwm_ng_set(SHOVEL_PWM, 0);
109 cobboard.flags &= (~bit);
111 printf_P(PSTR("%s is %s\r\n"), res->arg1,
112 (bit & cobboard.flags) ? "on":"off");
115 prog_char str_event_arg0[] = "event";
116 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
117 prog_char str_event_arg1[] = "all#encoders#cs#bd#power";
118 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
119 prog_char str_event_arg2[] = "on#off#show";
120 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
122 prog_char help_event[] = "Enable/disable events";
123 parse_pgm_inst_t cmd_event = {
124 .f = cmd_event_parsed, /* function to call */
125 .data = NULL, /* 2nd arg of func */
126 .help_str = help_event,
127 .tokens = { /* token list, NULL terminated */
128 (prog_void *)&cmd_event_arg0,
129 (prog_void *)&cmd_event_arg1,
130 (prog_void *)&cmd_event_arg2,
135 /**********************************************************/
138 /* this structure is filled when cmd_color is parsed successfully */
139 struct cmd_color_result {
141 fixed_string_t color;
144 /* function called when cmd_color is parsed successfully */
145 static void cmd_color_parsed(void *parsed_result, __attribute__((unused)) void *data)
147 struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
148 if (!strcmp_P(res->color, PSTR("red"))) {
149 cobboard.our_color = I2C_COLOR_RED;
151 else if (!strcmp_P(res->color, PSTR("green"))) {
152 cobboard.our_color = I2C_COLOR_GREEN;
154 printf_P(PSTR("Done\r\n"));
157 prog_char str_color_arg0[] = "color";
158 parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0);
159 prog_char str_color_color[] = "green#red";
160 parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color);
162 prog_char help_color[] = "Set our color";
163 parse_pgm_inst_t cmd_color = {
164 .f = cmd_color_parsed, /* function to call */
165 .data = NULL, /* 2nd arg of func */
166 .help_str = help_color,
167 .tokens = { /* token list, NULL terminated */
168 (prog_void *)&cmd_color_arg0,
169 (prog_void *)&cmd_color_color,
174 /**********************************************************/
177 /* this structure is filled when cmd_state1 is parsed successfully */
178 struct cmd_state1_result {
183 /* function called when cmd_state1 is parsed successfully */
184 static void cmd_state1_parsed(void *parsed_result,
185 __attribute__((unused)) void *data)
187 struct cmd_state1_result *res = parsed_result;
188 struct i2c_cmd_cobboard_set_mode command;
190 if (!strcmp_P(res->arg1, PSTR("init"))) {
195 if (!strcmp_P(res->arg1, PSTR("manual")))
196 command.mode = I2C_COBBOARD_MODE_MANUAL;
197 else if (!strcmp_P(res->arg1, PSTR("harvest")))
198 command.mode = I2C_COBBOARD_MODE_HARVEST;
199 state_set_mode(&command);
202 prog_char str_state1_arg0[] = "cobboard";
203 parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0);
204 prog_char str_state1_arg1[] = "init#manual";
205 parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1);
207 prog_char help_state1[] = "set cobboard mode";
208 parse_pgm_inst_t cmd_state1 = {
209 .f = cmd_state1_parsed, /* function to call */
210 .data = NULL, /* 2nd arg of func */
211 .help_str = help_state1,
212 .tokens = { /* token list, NULL terminated */
213 (prog_void *)&cmd_state1_arg0,
214 (prog_void *)&cmd_state1_arg1,
219 /**********************************************************/
222 /* this structure is filled when cmd_state2 is parsed successfully */
223 struct cmd_state2_result {
229 /* function called when cmd_state2 is parsed successfully */
230 static void cmd_state2_parsed(void *parsed_result,
231 __attribute__((unused)) void *data)
233 struct cmd_state2_result *res = parsed_result;
234 struct i2c_cmd_cobboard_set_mode command;
237 if (!strcmp_P(res->arg2, PSTR("left")))
238 side = I2C_LEFT_SIDE;
239 else if (!strcmp_P(res->arg2, PSTR("right")))
240 side = I2C_RIGHT_SIDE;
242 if (!strcmp_P(res->arg1, PSTR("yyy"))) {
244 else if (!strcmp_P(res->arg1, PSTR("xxx"))) {
248 state_set_mode(&command);
251 prog_char str_state2_arg0[] = "cobboard";
252 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
253 prog_char str_state2_arg1[] = "xxx";
254 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
255 prog_char str_state2_arg2[] = "left#right";
256 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
258 prog_char help_state2[] = "set cobboard mode";
259 parse_pgm_inst_t cmd_state2 = {
260 .f = cmd_state2_parsed, /* function to call */
261 .data = NULL, /* 2nd arg of func */
262 .help_str = help_state2,
263 .tokens = { /* token list, NULL terminated */
264 (prog_void *)&cmd_state2_arg0,
265 (prog_void *)&cmd_state2_arg1,
266 (prog_void *)&cmd_state2_arg2,
271 /**********************************************************/
274 /* this structure is filled when cmd_state3 is parsed successfully */
275 struct cmd_state3_result {
281 /* function called when cmd_state3 is parsed successfully */
282 static void cmd_state3_parsed(void *parsed_result,
283 __attribute__((unused)) void *data)
285 struct cmd_state3_result *res = parsed_result;
286 struct i2c_cmd_cobboard_set_mode command;
288 if (!strcmp_P(res->arg1, PSTR("xxx"))) {
289 /* xxx = res->arg2 */
291 else if (!strcmp_P(res->arg1, PSTR("yyy"))) {
293 state_set_mode(&command);
296 prog_char str_state3_arg0[] = "cobboard";
297 parse_pgm_token_string_t cmd_state3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg0, str_state3_arg0);
298 prog_char str_state3_arg1[] = "xxx";
299 parse_pgm_token_string_t cmd_state3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state3_result, arg1, str_state3_arg1);
300 parse_pgm_token_num_t cmd_state3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_state3_result, arg2, UINT8);
302 prog_char help_state3[] = "set cobboard mode";
303 parse_pgm_inst_t cmd_state3 = {
304 .f = cmd_state3_parsed, /* function to call */
305 .data = NULL, /* 2nd arg of func */
306 .help_str = help_state3,
307 .tokens = { /* token list, NULL terminated */
308 (prog_void *)&cmd_state3_arg0,
309 (prog_void *)&cmd_state3_arg1,
310 (prog_void *)&cmd_state3_arg2,
315 /**********************************************************/
318 /* this structure is filled when cmd_state_machine is parsed successfully */
319 struct cmd_state_machine_result {
323 /* function called when cmd_state_machine is parsed successfully */
324 static void cmd_state_machine_parsed(__attribute__((unused)) void *parsed_result,
325 __attribute__((unused)) void *data)
330 prog_char str_state_machine_arg0[] = "state_machine";
331 parse_pgm_token_string_t cmd_state_machine_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_machine_result, arg0, str_state_machine_arg0);
333 prog_char help_state_machine[] = "launch state machine";
334 parse_pgm_inst_t cmd_state_machine = {
335 .f = cmd_state_machine_parsed, /* function to call */
336 .data = NULL, /* 2nd arg of func */
337 .help_str = help_state_machine,
338 .tokens = { /* token list, NULL terminated */
339 (prog_void *)&cmd_state_machine_arg0,
344 /**********************************************************/
347 /* this structure is filled when cmd_state_debug is parsed successfully */
348 struct cmd_state_debug_result {
353 /* function called when cmd_state_debug is parsed successfully */
354 static void cmd_state_debug_parsed(void *parsed_result,
355 __attribute__((unused)) void *data)
357 struct cmd_state_debug_result *res = parsed_result;
358 state_debug = res->on;
361 prog_char str_state_debug_arg0[] = "state_debug";
362 parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0);
363 parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8);
365 prog_char help_state_debug[] = "Set debug timer for state machine";
366 parse_pgm_inst_t cmd_state_debug = {
367 .f = cmd_state_debug_parsed, /* function to call */
368 .data = NULL, /* 2nd arg of func */
369 .help_str = help_state_debug,
370 .tokens = { /* token list, NULL terminated */
371 (prog_void *)&cmd_state_debug_arg0,
372 (prog_void *)&cmd_state_debug_on,
377 /**********************************************************/
380 /* this structure is filled when cmd_servo_door is parsed successfully */
381 struct cmd_servo_door_result {
386 /* function called when cmd_servo_door is parsed successfully */
387 static void cmd_servo_door_parsed(void *parsed_result,
388 __attribute__((unused)) void *data)
390 struct cmd_servo_door_result *res = parsed_result;
391 if (!strcmp_P(res->arg1, PSTR("open")))
393 else if (!strcmp_P(res->arg1, PSTR("closed")))
397 prog_char str_servo_door_arg0[] = "door";
398 parse_pgm_token_string_t cmd_servo_door_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg0, str_servo_door_arg0);
399 prog_char str_servo_door_arg1[] = "open#closed";
400 parse_pgm_token_string_t cmd_servo_door_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg1, str_servo_door_arg1);
402 prog_char help_servo_door[] = "Servo door function";
403 parse_pgm_inst_t cmd_servo_door = {
404 .f = cmd_servo_door_parsed, /* function to call */
405 .data = NULL, /* 2nd arg of func */
406 .help_str = help_servo_door,
407 .tokens = { /* token list, NULL terminated */
408 (prog_void *)&cmd_servo_door_arg0,
409 (prog_void *)&cmd_servo_door_arg1,
414 /**********************************************************/
417 /* this structure is filled when cmd_servo_carry is parsed successfully */
418 struct cmd_servo_carry_result {
423 /* function called when cmd_servo_carry is parsed successfully */
424 static void cmd_servo_carry_parsed(void *parsed_result,
425 __attribute__((unused)) void *data)
427 struct cmd_servo_carry_result *res = parsed_result;
428 if (!strcmp_P(res->arg1, PSTR("open")))
430 else if (!strcmp_P(res->arg1, PSTR("closed")))
434 prog_char str_servo_carry_arg0[] = "carry";
435 parse_pgm_token_string_t cmd_servo_carry_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg0, str_servo_carry_arg0);
436 prog_char str_servo_carry_arg1[] = "open#closed";
437 parse_pgm_token_string_t cmd_servo_carry_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_carry_result, arg1, str_servo_carry_arg1);
439 prog_char help_servo_carry[] = "Servo carry function";
440 parse_pgm_inst_t cmd_servo_carry = {
441 .f = cmd_servo_carry_parsed, /* function to call */
442 .data = NULL, /* 2nd arg of func */
443 .help_str = help_servo_carry,
444 .tokens = { /* token list, NULL terminated */
445 (prog_void *)&cmd_servo_carry_arg0,
446 (prog_void *)&cmd_servo_carry_arg1,
451 /**********************************************************/
454 /* this structure is filled when cmd_test is parsed successfully */
455 struct cmd_test_result {
459 /* function called when cmd_test is parsed successfully */
460 static void cmd_test_parsed(__attribute__((unused)) void *parsed_result,
461 __attribute__((unused)) void *data)
465 prog_char str_test_arg0[] = "test";
466 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
468 prog_char help_test[] = "Test function";
469 parse_pgm_inst_t cmd_test = {
470 .f = cmd_test_parsed, /* function to call */
471 .data = NULL, /* 2nd arg of func */
472 .help_str = help_test,
473 .tokens = { /* token list, NULL terminated */
474 (prog_void *)&cmd_test_arg0,