/*
* 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
*
* Revision : $Id: commands_cobboard.c,v 1.6 2009-11-08 17:25:00 zer0 Exp $
*
- * Olivier MATZ <zer0@droids-corp.org>
+ * Olivier MATZ <zer0@droids-corp.org>
*/
#include <stdio.h>
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;
}
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")))
}
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);
.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,
},
};
.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,
},
};
state_init();
else if (!strcmp_P(res->arg1, PSTR("eject")))
state_set_mode(I2C_COBBOARD_MODE_EJECT);
+ else if (!strcmp_P(res->arg1, PSTR("kickstand_up")))
+ state_set_mode(I2C_COBBOARD_MODE_KICKSTAND_UP);
+ else if (!strcmp_P(res->arg1, PSTR("kickstand_down")))
+ state_set_mode(I2C_COBBOARD_MODE_KICKSTAND_DOWN);
+ 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#kickstand_up#kickstand_down";
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";
.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,
},
};
__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);
.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,
},
};
};
/* 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;
.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,
},
};
.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,
},
};
.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,
},
};
.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,
},
};
.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,
},
};
.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,
},
};
.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,
},
};
.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,
},
};
.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,
},
};
__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 */
spickle_dump_params();
}
-prog_char str_spickle_params2_arg0[] = "spickle_params2";
+prog_char str_spickle_params2_arg0[] = "spickle_params";
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 =
.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,
},
};
.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,
},
};
.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,
},
};