X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fcommands_cobboard.c;h=dbbdd791a85d624c56dcf8b8990e75e7eeca9a12;hp=249a69be9d7a15969d15f5bc2a4f55aeaabcb0a9;hb=b28db6087ed5fdab0bda8f23417ef7098d2a9ae5;hpb=57b3e145f2ff46e1a7e7d522c85f3c56821dbf48 diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 249a69b..dbbdd79 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -1,6 +1,6 @@ /* * Copyright Droids Corporation (2009) - * + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -17,7 +17,7 @@ * * Revision : $Id: commands_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include @@ -53,8 +53,6 @@ #include "spickle.h" #include "shovel.h" -extern uint16_t state_debug; - struct cmd_event_result { fixed_string_t arg0; fixed_string_t arg1; @@ -68,22 +66,24 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * u08 bit=0; struct cmd_event_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("all"))) { - bit = DO_ENCODERS | DO_CS | DO_BD | DO_POWER; + bit = 0xFF; if (!strcmp_P(res->arg2, PSTR("on"))) cobboard.flags |= bit; else if (!strcmp_P(res->arg2, PSTR("off"))) cobboard.flags &= bit; else { /* show */ - printf_P(PSTR("encoders is %s\r\n"), + printf_P(PSTR("encoders is %s\r\n"), (DO_ENCODERS & cobboard.flags) ? "on":"off"); - printf_P(PSTR("cs is %s\r\n"), + printf_P(PSTR("cs is %s\r\n"), (DO_CS & cobboard.flags) ? "on":"off"); - printf_P(PSTR("bd is %s\r\n"), + printf_P(PSTR("bd is %s\r\n"), (DO_BD & cobboard.flags) ? "on":"off"); - printf_P(PSTR("power is %s\r\n"), + printf_P(PSTR("power is %s\r\n"), (DO_POWER & cobboard.flags) ? "on":"off"); + printf_P(PSTR("errblock is %s\r\n"), + (DO_ERRBLOCKING & cobboard.flags) ? "on":"off"); } return; } @@ -97,6 +97,8 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * bit = DO_BD; else if (!strcmp_P(res->arg1, PSTR("power"))) bit = DO_POWER; + else if (!strcmp_P(res->arg1, PSTR("errblock"))) + bit = DO_ERRBLOCKING; if (!strcmp_P(res->arg2, PSTR("on"))) @@ -109,13 +111,13 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * } cobboard.flags &= (~bit); } - printf_P(PSTR("%s is %s\r\n"), res->arg1, + printf_P(PSTR("%s is %s\r\n"), res->arg1, (bit & cobboard.flags) ? "on":"off"); } prog_char str_event_arg0[] = "event"; parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0); -prog_char str_event_arg1[] = "all#encoders#cs#bd#power"; +prog_char str_event_arg1[] = "all#encoders#cs#bd#power#errblock"; parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1); prog_char str_event_arg2[] = "on#off#show"; parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2); @@ -126,9 +128,9 @@ parse_pgm_inst_t cmd_event = { .data = NULL, /* 2nd arg of func */ .help_str = help_event, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_event_arg0, - (prog_void *)&cmd_event_arg1, - (prog_void *)&cmd_event_arg2, + (prog_void *)&cmd_event_arg0, + (prog_void *)&cmd_event_arg1, + (prog_void *)&cmd_event_arg2, NULL, }, }; @@ -146,18 +148,18 @@ struct cmd_color_result { static void cmd_color_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; - if (!strcmp_P(res->color, PSTR("red"))) { - cobboard.our_color = I2C_COLOR_RED; + if (!strcmp_P(res->color, PSTR("yellow"))) { + cobboard.our_color = I2C_COLOR_YELLOW; } - else if (!strcmp_P(res->color, PSTR("green"))) { - cobboard.our_color = I2C_COLOR_GREEN; + else if (!strcmp_P(res->color, PSTR("blue"))) { + cobboard.our_color = I2C_COLOR_BLUE; } printf_P(PSTR("Done\r\n")); } prog_char str_color_arg0[] = "color"; parse_pgm_token_string_t cmd_color_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_color_result, arg0, str_color_arg0); -prog_char str_color_color[] = "green#red"; +prog_char str_color_color[] = "blue#yellow"; parse_pgm_token_string_t cmd_color_color = TOKEN_STRING_INITIALIZER(struct cmd_color_result, color, str_color_color); prog_char help_color[] = "Set our color"; @@ -166,8 +168,8 @@ parse_pgm_inst_t cmd_color = { .data = NULL, /* 2nd arg of func */ .help_str = help_color, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_color_arg0, - (prog_void *)&cmd_color_color, + (prog_void *)&cmd_color_arg0, + (prog_void *)&cmd_color_color, NULL, }, }; @@ -206,8 +208,8 @@ parse_pgm_inst_t cmd_state1 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state1, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state1_arg0, - (prog_void *)&cmd_state1_arg1, + (prog_void *)&cmd_state1_arg0, + (prog_void *)&cmd_state1_arg1, NULL, }, }; @@ -227,38 +229,26 @@ static void cmd_state2_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state2_result *res = parsed_result; - uint8_t side, mode = state_get_mode(); + uint8_t side; - if (!strcmp_P(res->arg2, PSTR("left"))) { + if (!strcmp_P(res->arg2, PSTR("left"))) side = I2C_LEFT_SIDE; - mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); - } - else { + else side = I2C_RIGHT_SIDE; - mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); - } if (!strcmp_P(res->arg1, PSTR("pack"))) { - /* nothing to do */ + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, 0); } else if (!strcmp_P(res->arg1, PSTR("deploy"))) { - if (side == I2C_LEFT_SIDE) - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - else - mode |= I2C_COBBOARD_MODE_R_DEPLOY; + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY); } else if (!strcmp_P(res->arg1, PSTR("harvest"))) { - if (side == I2C_LEFT_SIDE) { - mode |= I2C_COBBOARD_MODE_L_DEPLOY; - mode |= I2C_COBBOARD_MODE_L_HARVEST; - } - else { - mode |= I2C_COBBOARD_MODE_R_DEPLOY; - mode |= I2C_COBBOARD_MODE_R_HARVEST; - } + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY | + I2C_COBBOARD_SPK_AUTOHARVEST); } - - state_set_mode(mode); } prog_char str_state2_arg0[] = "cobboard"; @@ -274,9 +264,9 @@ parse_pgm_inst_t cmd_state2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state2_arg0, - (prog_void *)&cmd_state2_arg1, - (prog_void *)&cmd_state2_arg2, + (prog_void *)&cmd_state2_arg0, + (prog_void *)&cmd_state2_arg1, + (prog_void *)&cmd_state2_arg2, NULL, }, }; @@ -292,7 +282,7 @@ struct cmd_state3_result { }; /* function called when cmd_state3 is parsed successfully */ -static void cmd_state3_parsed(void *parsed_result, +static void cmd_state3_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state3_result *res = parsed_result; @@ -317,9 +307,9 @@ parse_pgm_inst_t cmd_state3 = { .data = NULL, /* 2nd arg of func */ .help_str = help_state3, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state3_arg0, - (prog_void *)&cmd_state3_arg1, - (prog_void *)&cmd_state3_arg2, + (prog_void *)&cmd_state3_arg0, + (prog_void *)&cmd_state3_arg1, + (prog_void *)&cmd_state3_arg2, NULL, }, }; @@ -348,7 +338,7 @@ parse_pgm_inst_t cmd_state_machine = { .data = NULL, /* 2nd arg of func */ .help_str = help_state_machine, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_machine_arg0, + (prog_void *)&cmd_state_machine_arg0, NULL, }, }; @@ -374,14 +364,14 @@ prog_char str_state_debug_arg0[] = "state_debug"; parse_pgm_token_string_t cmd_state_debug_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state_debug_result, arg0, str_state_debug_arg0); parse_pgm_token_num_t cmd_state_debug_on = TOKEN_NUM_INITIALIZER(struct cmd_state_debug_result, on, UINT8); -prog_char help_state_debug[] = "Set debug timer for state machine"; +prog_char help_state_debug[] = "Set debug for state machine"; parse_pgm_inst_t cmd_state_debug = { .f = cmd_state_debug_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ .help_str = help_state_debug, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_state_debug_arg0, - (prog_void *)&cmd_state_debug_on, + (prog_void *)&cmd_state_debug_arg0, + (prog_void *)&cmd_state_debug_on, NULL, }, }; @@ -404,11 +394,13 @@ static void cmd_servo_door_parsed(void *parsed_result, servo_door_open(); else if (!strcmp_P(res->arg1, PSTR("closed"))) servo_door_close(); + else if (!strcmp_P(res->arg1, PSTR("block"))) + servo_door_close(); } prog_char str_servo_door_arg0[] = "door"; parse_pgm_token_string_t cmd_servo_door_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg0, str_servo_door_arg0); -prog_char str_servo_door_arg1[] = "open#closed"; +prog_char str_servo_door_arg1[] = "open#closed#block"; parse_pgm_token_string_t cmd_servo_door_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_servo_door_result, arg1, str_servo_door_arg1); prog_char help_servo_door[] = "Servo door function"; @@ -417,8 +409,8 @@ parse_pgm_inst_t cmd_servo_door = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_door, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_door_arg0, - (prog_void *)&cmd_servo_door_arg1, + (prog_void *)&cmd_servo_door_arg0, + (prog_void *)&cmd_servo_door_arg1, NULL, }, }; @@ -438,19 +430,17 @@ static void cmd_cobroller_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_cobroller_result *res = parsed_result; + uint8_t side; - if (!strcmp_P(res->arg1, PSTR("left"))) { - if (!strcmp_P(res->arg2, PSTR("on"))) - left_cobroller_on(); - else if (!strcmp_P(res->arg2, PSTR("off"))) - left_cobroller_off(); - } - else if (!strcmp_P(res->arg1, PSTR("right"))) { - if (!strcmp_P(res->arg2, PSTR("on"))) - right_cobroller_on(); - else if (!strcmp_P(res->arg2, PSTR("off"))) - right_cobroller_off(); - } + if (!strcmp_P(res->arg1, PSTR("left"))) + side = I2C_LEFT_SIDE; + else + side = I2C_RIGHT_SIDE; + + if (!strcmp_P(res->arg2, PSTR("on"))) + cobroller_on(side); + else if (!strcmp_P(res->arg2, PSTR("off"))) + cobroller_off(side); } prog_char str_cobroller_arg0[] = "cobroller"; @@ -466,9 +456,9 @@ parse_pgm_inst_t cmd_cobroller = { .data = NULL, /* 2nd arg of func */ .help_str = help_cobroller, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_cobroller_arg0, - (prog_void *)&cmd_cobroller_arg1, - (prog_void *)&cmd_cobroller_arg2, + (prog_void *)&cmd_cobroller_arg0, + (prog_void *)&cmd_cobroller_arg1, + (prog_void *)&cmd_cobroller_arg2, NULL, }, }; @@ -506,8 +496,8 @@ parse_pgm_inst_t cmd_shovel = { .data = NULL, /* 2nd arg of func */ .help_str = help_shovel, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_shovel_arg0, - (prog_void *)&cmd_shovel_arg1, + (prog_void *)&cmd_shovel_arg0, + (prog_void *)&cmd_shovel_arg1, NULL, }, }; @@ -543,8 +533,8 @@ parse_pgm_inst_t cmd_servo_carry = { .data = NULL, /* 2nd arg of func */ .help_str = help_servo_carry, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_servo_carry_arg0, - (prog_void *)&cmd_servo_carry_arg1, + (prog_void *)&cmd_servo_carry_arg0, + (prog_void *)&cmd_servo_carry_arg1, NULL, }, }; @@ -577,6 +567,9 @@ static void cmd_spickle_parsed(void * parsed_result, else if (!strcmp_P(res->arg2, PSTR("pack"))) { spickle_pack(side); } + else if (!strcmp_P(res->arg2, PSTR("mid"))) { + spickle_mid(side); + } printf_P(PSTR("done\r\n")); } @@ -586,7 +579,7 @@ parse_pgm_token_string_t cmd_spickle_arg0 = prog_char str_spickle_arg1[] = "left#right"; parse_pgm_token_string_t cmd_spickle_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1); -prog_char str_spickle_arg2[] = "deploy#pack"; +prog_char str_spickle_arg2[] = "deploy#pack#mid"; parse_pgm_token_string_t cmd_spickle_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg2, str_spickle_arg2); @@ -596,9 +589,9 @@ parse_pgm_inst_t cmd_spickle = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_arg0, - (prog_void *)&cmd_spickle_arg1, - (prog_void *)&cmd_spickle_arg2, + (prog_void *)&cmd_spickle_arg0, + (prog_void *)&cmd_spickle_arg1, + (prog_void *)&cmd_spickle_arg2, NULL, }, }; @@ -613,6 +606,7 @@ struct cmd_spickle_params_result { fixed_string_t arg2; int32_t arg3; int32_t arg4; + int32_t arg5; }; /* function called when cmd_spickle_params is parsed successfully */ @@ -633,9 +627,7 @@ static void cmd_spickle_params_parsed(void *parsed_result, side = I2C_RIGHT_SIDE; if (!strcmp_P(res->arg2, PSTR("pos"))) - spickle_set_pos(side, res->arg3, res->arg4); - else if (!strcmp_P(res->arg2, PSTR("delay"))) - spickle_set_delay(side, res->arg3, res->arg4); + spickle_set_pos(side, res->arg3, res->arg4, res->arg5); } prog_char str_spickle_params_arg0[] = "spickle_params"; @@ -644,25 +636,28 @@ parse_pgm_token_string_t cmd_spickle_params_arg0 = prog_char str_spickle_params_arg1[] = "left#right"; parse_pgm_token_string_t cmd_spickle_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1); -prog_char str_spickle_params_arg2[] = "pos#delay"; +prog_char str_spickle_params_arg2[] = "pos"; parse_pgm_token_string_t cmd_spickle_params_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg2, str_spickle_params_arg2); parse_pgm_token_num_t cmd_spickle_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32); parse_pgm_token_num_t cmd_spickle_params_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg4, INT32); +parse_pgm_token_num_t cmd_spickle_params_arg5 = + TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg5, INT32); -prog_char help_spickle_params[] = "Set spickle pos values"; +prog_char help_spickle_params[] = "Set spickle pos values: left|right pos INTPACK INTMID INTDEPL"; parse_pgm_inst_t cmd_spickle_params = { .f = cmd_spickle_params_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params_arg0, - (prog_void *)&cmd_spickle_params_arg1, - (prog_void *)&cmd_spickle_params_arg2, - (prog_void *)&cmd_spickle_params_arg3, - (prog_void *)&cmd_spickle_params_arg4, + (prog_void *)&cmd_spickle_params_arg0, + (prog_void *)&cmd_spickle_params_arg1, + (prog_void *)&cmd_spickle_params_arg2, + (prog_void *)&cmd_spickle_params_arg3, + (prog_void *)&cmd_spickle_params_arg4, + (prog_void *)&cmd_spickle_params_arg5, NULL, }, }; @@ -677,8 +672,8 @@ parse_pgm_inst_t cmd_spickle_params_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params_arg0, - (prog_void *)&cmd_spickle_params_arg1_show, + (prog_void *)&cmd_spickle_params_arg0, + (prog_void *)&cmd_spickle_params_arg1_show, NULL, }, }; @@ -699,7 +694,7 @@ static void cmd_spickle_params2_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_spickle_params2_result * res = parsed_result; - + if (!strcmp_P(res->arg1, PSTR("coef"))) { spickle_set_coefs(res->arg2, res->arg3); } @@ -725,10 +720,10 @@ parse_pgm_inst_t cmd_spickle_params2 = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params2, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params2_arg0, - (prog_void *)&cmd_spickle_params2_arg1, - (prog_void *)&cmd_spickle_params2_arg2, - (prog_void *)&cmd_spickle_params2_arg3, + (prog_void *)&cmd_spickle_params2_arg0, + (prog_void *)&cmd_spickle_params2_arg1, + (prog_void *)&cmd_spickle_params2_arg2, + (prog_void *)&cmd_spickle_params2_arg3, NULL, }, }; @@ -743,8 +738,8 @@ parse_pgm_inst_t cmd_spickle_params2_show = { .data = NULL, /* 2nd arg of func */ .help_str = help_spickle_params2_show, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_spickle_params2_arg0, - (prog_void *)&cmd_spickle_params2_arg1_show, + (prog_void *)&cmd_spickle_params2_arg0, + (prog_void *)&cmd_spickle_params2_arg1_show, NULL, }, }; @@ -773,7 +768,7 @@ parse_pgm_inst_t cmd_test = { .data = NULL, /* 2nd arg of func */ .help_str = help_test, .tokens = { /* token list, NULL terminated */ - (prog_void *)&cmd_test_arg0, + (prog_void *)&cmd_test_arg0, NULL, }, };