X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fcobboard%2Fcommands_cobboard.c;h=9cfda19daf050f92f3fe3d2138203a313081104c;hp=4ea3b806b07cf1e78326c43c3e016a99f54c2cc6;hb=d23a4fdeed4e90d004d407942b230c188d012465;hpb=fa8546ea39c7442ad3bf5a822a72a2b50a41045d diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 4ea3b80..9cfda19 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 @@ -68,20 +68,22 @@ static void cmd_event_parsed(void *parsed_result, __attribute__((unused)) void * 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; } @@ -95,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"))) @@ -107,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); @@ -124,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, }, }; @@ -164,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, }, }; @@ -189,13 +193,17 @@ static void cmd_state1_parsed(void *parsed_result, state_init(); else if (!strcmp_P(res->arg1, PSTR("eject"))) state_set_mode(I2C_COBBOARD_MODE_EJECT); + else if (!strcmp_P(res->arg1, PSTR("ignore_i2c"))) + state_set_i2c_ignore(1); + else if (!strcmp_P(res->arg1, PSTR("care_i2c"))) + state_set_i2c_ignore(0); /* other commands */ } prog_char str_state1_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0); -prog_char str_state1_arg1[] = "init#eject"; +prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c"; parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1); prog_char help_state1[] = "set cobboard mode"; @@ -204,8 +212,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, }, }; @@ -225,43 +233,46 @@ 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("weak_pack"))) { + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_WEAK); } 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); + } + else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) { + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY | + I2C_COBBOARD_SPK_NO_MOVE); + } + else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) { + state_set_mode(I2C_COBBOARD_MODE_HARVEST); + state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY | + I2C_COBBOARD_SPK_AUTOHARVEST | + I2C_COBBOARD_SPK_NO_MOVE); } - - state_set_mode(mode); } prog_char str_state2_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0); -prog_char str_state2_arg1[] = "harvest#deploy#pack"; +prog_char str_state2_arg1[] = "harvest#deploy#pack#weak_pack#harvest_nomove#deploy_nomove"; parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1); prog_char str_state2_arg2[] = "left#right"; parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2); @@ -272,9 +283,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, }, }; @@ -290,7 +301,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; @@ -315,9 +326,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, }, }; @@ -346,7 +357,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, }, }; @@ -378,8 +389,8 @@ parse_pgm_inst_t cmd_state_debug = { .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, }, }; @@ -417,8 +428,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, }, }; @@ -464,9 +475,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, }, }; @@ -504,8 +515,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, }, }; @@ -541,8 +552,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, }, }; @@ -597,9 +608,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, }, }; @@ -680,8 +691,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, }, }; @@ -702,9 +713,12 @@ 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); + + if (!strcmp_P(res->arg1, PSTR("wcoef"))) { + spickle_set_wcoefs(res->arg2, res->arg3); + } + else if (!strcmp_P(res->arg1, PSTR("scoef"))) { + spickle_set_scoefs(res->arg2, res->arg3); } /* else show */ @@ -714,7 +728,7 @@ static void cmd_spickle_params2_parsed(void *parsed_result, prog_char str_spickle_params2_arg0[] = "spickle_params2"; parse_pgm_token_string_t cmd_spickle_params2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg0, str_spickle_params2_arg0); -prog_char str_spickle_params2_arg1[] = "coef"; +prog_char str_spickle_params2_arg1[] = "wcoef#scoef"; parse_pgm_token_string_t cmd_spickle_params2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg1, str_spickle_params2_arg1); parse_pgm_token_num_t cmd_spickle_params2_arg2 = @@ -728,10 +742,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, }, }; @@ -746,8 +760,81 @@ 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, + }, +}; + +/**********************************************************/ +/* Set Shovel Params */ + +/* this structure is filled when cmd_shovel_current is parsed successfully */ +struct cmd_shovel_current_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; +}; + +/* function called when cmd_shovel_current is parsed successfully */ +static void cmd_shovel_current_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_shovel_current_result * res = parsed_result; + uint8_t enable; + int32_t k1, k2; + + if (!strcmp_P(res->arg1, PSTR("set"))) + shovel_set_current_limit_coefs(res->arg2, res->arg3); + else if (!strcmp_P(res->arg1, PSTR("on"))) + shovel_current_limit_enable(1); + else if (!strcmp_P(res->arg1, PSTR("off"))) + shovel_current_limit_enable(0); + + /* else show */ + enable = shovel_get_current_limit_coefs(&k1, &k2); + printf_P(PSTR("enabled=%d k1=%"PRIi32" k2=%"PRIi32"\r\n"), + enable, k1, k2); +} + +prog_char str_shovel_current_arg0[] = "shovel_current"; +parse_pgm_token_string_t cmd_shovel_current_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg0, str_shovel_current_arg0); +prog_char str_shovel_current_arg1[] = "set"; +parse_pgm_token_string_t cmd_shovel_current_arg1 = + TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg1, str_shovel_current_arg1); +parse_pgm_token_num_t cmd_shovel_current_arg2 = + TOKEN_NUM_INITIALIZER(struct cmd_shovel_current_result, arg2, INT32); +parse_pgm_token_num_t cmd_shovel_current_arg3 = + TOKEN_NUM_INITIALIZER(struct cmd_shovel_current_result, arg3, INT32); + +prog_char help_shovel_current[] = "Set shovel_current values"; +parse_pgm_inst_t cmd_shovel_current = { + .f = cmd_shovel_current_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_shovel_current, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_shovel_current_arg0, + (prog_void *)&cmd_shovel_current_arg1, + (prog_void *)&cmd_shovel_current_arg2, + (prog_void *)&cmd_shovel_current_arg3, + NULL, + }, +}; + +prog_char str_shovel_current_arg1_show[] = "show#on#off"; +parse_pgm_token_string_t cmd_shovel_current_arg1_show = + TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg1, str_shovel_current_arg1_show); + +prog_char help_shovel_current_show[] = "show shovel params"; +parse_pgm_inst_t cmd_shovel_current_show = { + .f = cmd_shovel_current_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_shovel_current_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_shovel_current_arg0, + (prog_void *)&cmd_shovel_current_arg1_show, NULL, }, }; @@ -776,7 +863,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, }, };