- else if (!strcmp_P(res->arg2, PSTR("center")))
- side = I2C_CENTER_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("auto")))
- side = I2C_AUTO_SIDE;
-
- if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
- i2c_mechboard_mode_prepare_pickup(side);
- else if (!strcmp_P(res->arg1, PSTR("push_temple_disc")))
- i2c_mechboard_mode_push_temple_disc(side);
-}
-
-prog_char str_mechboard_setmode2_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg0, str_mechboard_setmode2_arg0);
-prog_char str_mechboard_setmode2_arg1[] = "prepare_pickup#push_temple_disc";
-parse_pgm_token_string_t cmd_mechboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg1, str_mechboard_setmode2_arg1);
-prog_char str_mechboard_setmode2_arg2[] = "left#right#auto#center";
-parse_pgm_token_string_t cmd_mechboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode2_result, arg2, str_mechboard_setmode2_arg2);
-
-prog_char help_mechboard_setmode2[] = "set mechboard mode (more, side)";
-parse_pgm_inst_t cmd_mechboard_setmode2 = {
- .f = cmd_mechboard_setmode2_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_setmode2,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode2_arg0,
- (prog_void *)&cmd_mechboard_setmode2_arg1,
- (prog_void *)&cmd_mechboard_setmode2_arg2,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Mechboard_Setmode3 */
-
-/* this structure is filled when cmd_mechboard_setmode3 is parsed successfully */
-struct cmd_mechboard_setmode3_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint8_t level;
-};
-
-/* function called when cmd_mechboard_setmode3 is parsed successfully */
-static void cmd_mechboard_setmode3_parsed(void *parsed_result, void *data)
-{
- struct cmd_mechboard_setmode3_result *res = parsed_result;
- if (!strcmp_P(res->arg1, PSTR("autobuild")))
- i2c_mechboard_mode_simple_autobuild(res->level);
- else if (!strcmp_P(res->arg1, PSTR("prepare_build")))
- i2c_mechboard_mode_prepare_build_both(res->level);
- else if (!strcmp_P(res->arg1, PSTR("prepare_inside")))
- i2c_mechboard_mode_prepare_inside_both(res->level);
- else if (!strcmp_P(res->arg1, PSTR("push_temple")))
- i2c_mechboard_mode_push_temple(res->level);
-}
-
-prog_char str_mechboard_setmode3_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg0, str_mechboard_setmode3_arg0);
-prog_char str_mechboard_setmode3_arg1[] = "autobuild#prepare_build#prepare_inside#push_temple";
-parse_pgm_token_string_t cmd_mechboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode3_result, arg1, str_mechboard_setmode3_arg1);
-parse_pgm_token_num_t cmd_mechboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode3_result, level, UINT8);
-
-prog_char help_mechboard_setmode3[] = "set mechboard mode (mode, level)";
-parse_pgm_inst_t cmd_mechboard_setmode3 = {
- .f = cmd_mechboard_setmode3_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_setmode3,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode3_arg0,
- (prog_void *)&cmd_mechboard_setmode3_arg1,
- (prog_void *)&cmd_mechboard_setmode3_arg2,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Mechboard_Setmode4 */
-
-/* this structure is filled when cmd_mechboard_setmode4 is parsed successfully */
-struct cmd_mechboard_setmode4_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint8_t level_l;
- uint8_t count_l;
- uint8_t dist_l;
- uint8_t level_r;
- uint8_t count_r;
- uint8_t dist_r;
- uint8_t do_lintel;
-};
-
-/* function called when cmd_mechboard_setmode4 is parsed successfully */
-static void cmd_mechboard_setmode4_parsed(void *parsed_result, void *data)
-{
- struct cmd_mechboard_setmode4_result *res = parsed_result;
- i2c_mechboard_mode_autobuild(res->level_l, res->count_l, res->dist_l,
- res->level_r, res->count_r, res->dist_r,
- res->do_lintel);
-}
-
-prog_char str_mechboard_setmode4_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode4_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg0, str_mechboard_setmode4_arg0);
-prog_char str_mechboard_setmode4_arg1[] = "autobuild";
-parse_pgm_token_string_t cmd_mechboard_setmode4_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode4_result, arg1, str_mechboard_setmode4_arg1);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_l, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_l, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_l, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, level_r, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, count_r, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, dist_r, UINT8);
-parse_pgm_token_num_t cmd_mechboard_setmode4_arg8 = TOKEN_NUM_INITIALIZER(struct cmd_mechboard_setmode4_result, do_lintel, UINT8);
-
-prog_char help_mechboard_setmode4[] = "set mechboard mode (autobuild level_l count_l dist_l level_r count_r dist_r lintel)";
-parse_pgm_inst_t cmd_mechboard_setmode4 = {
- .f = cmd_mechboard_setmode4_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_setmode4,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode4_arg0,
- (prog_void *)&cmd_mechboard_setmode4_arg1,
- (prog_void *)&cmd_mechboard_setmode4_arg2,
- (prog_void *)&cmd_mechboard_setmode4_arg3,
- (prog_void *)&cmd_mechboard_setmode4_arg4,
- (prog_void *)&cmd_mechboard_setmode4_arg5,
- (prog_void *)&cmd_mechboard_setmode4_arg6,
- (prog_void *)&cmd_mechboard_setmode4_arg7,
- (prog_void *)&cmd_mechboard_setmode4_arg8,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Mechboard_Setmode5 */
-
-/* this structure is filled when cmd_mechboard_setmode5 is parsed successfully */
-struct cmd_mechboard_setmode5_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- fixed_string_t arg2;
- fixed_string_t arg3;
-};
-
-/* function called when cmd_mechboard_setmode5 is parsed successfully */
-static void cmd_mechboard_setmode5_parsed(void *parsed_result, void * data)
-{
- struct cmd_mechboard_setmode5_result *res = parsed_result;
- uint8_t side = I2C_LEFT_SIDE, next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
-
- if (!strcmp_P(res->arg2, PSTR("left")))
- side = I2C_LEFT_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("right")))
- side = I2C_RIGHT_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("center")))
- side = I2C_CENTER_SIDE;
- else if (!strcmp_P(res->arg2, PSTR("auto")))
- side = I2C_AUTO_SIDE;
-
- if (!strcmp_P(res->arg3, PSTR("harvest")))
- next_mode = I2C_MECHBOARD_MODE_HARVEST;
- else if (!strcmp_P(res->arg3, PSTR("lazy_harvest")))
- next_mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
- else if (!strcmp_P(res->arg3, PSTR("pickup")))
- next_mode = I2C_MECHBOARD_MODE_PICKUP;
- else if (!strcmp_P(res->arg3, PSTR("clear")))
- next_mode = I2C_MECHBOARD_MODE_CLEAR;
- else if (!strcmp_P(res->arg3, PSTR("store")))
- next_mode = I2C_MECHBOARD_MODE_STORE;
- else if (!strcmp_P(res->arg3, PSTR("lazy_pickup")))
- next_mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
-
- if (!strcmp_P(res->arg1, PSTR("prepare_pickup")))
- i2c_mechboard_mode_prepare_pickup_next(side, next_mode);
-}
-
-prog_char str_mechboard_setmode5_arg0[] = "mechboard";
-parse_pgm_token_string_t cmd_mechboard_setmode5_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg0, str_mechboard_setmode5_arg0);
-prog_char str_mechboard_setmode5_arg1[] = "prepare_pickup";
-parse_pgm_token_string_t cmd_mechboard_setmode5_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg1, str_mechboard_setmode5_arg1);
-prog_char str_mechboard_setmode5_arg2[] = "left#right#auto#center";
-parse_pgm_token_string_t cmd_mechboard_setmode5_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg2, str_mechboard_setmode5_arg2);
-prog_char str_mechboard_setmode5_arg3[] = "harvest#pickup#store#lazy_harvest#lazy_pickup#clear";
-parse_pgm_token_string_t cmd_mechboard_setmode5_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_mechboard_setmode5_result, arg3, str_mechboard_setmode5_arg3);
-
-prog_char help_mechboard_setmode5[] = "set mechboard mode (more, side)";
-parse_pgm_inst_t cmd_mechboard_setmode5 = {
- .f = cmd_mechboard_setmode5_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_mechboard_setmode5,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_mechboard_setmode5_arg0,
- (prog_void *)&cmd_mechboard_setmode5_arg1,
- (prog_void *)&cmd_mechboard_setmode5_arg2,
- (prog_void *)&cmd_mechboard_setmode5_arg3,
- NULL,
- },
-};
-
-/**********************************************************/
-/* pickup wheels */
-
-/* this structure is filled when cmd_pickup_wheels is parsed successfully */
-struct cmd_pickup_wheels_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_pickup_wheels is parsed successfully */
-static void cmd_pickup_wheels_parsed(void *parsed_result, void *data)
-{
- struct cmd_pickup_wheels_result *res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("on")))
- pickup_wheels_on();
- else
- pickup_wheels_off();
-}
-
-prog_char str_pickup_wheels_arg0[] = "pickup_wheels";
-parse_pgm_token_string_t cmd_pickup_wheels_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg0, str_pickup_wheels_arg0);
-prog_char str_pickup_wheels_arg1[] = "on#off";
-parse_pgm_token_string_t cmd_pickup_wheels_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pickup_wheels_result, arg1, str_pickup_wheels_arg1);
-
-prog_char help_pickup_wheels[] = "Enable/disable pickup wheels";
-parse_pgm_inst_t cmd_pickup_wheels = {
- .f = cmd_pickup_wheels_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_pickup_wheels,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pickup_wheels_arg0,
- (prog_void *)&cmd_pickup_wheels_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Beacon_Start */
-
-/* this structure is filled when cmd_beacon_start is parsed successfully */
-struct cmd_beacon_start_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_beacon_start is parsed successfully */
-static void cmd_beacon_start_parsed(void *parsed_result, void *data)
-{
- struct cmd_beacon_start_result *res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("start")))
- i2c_sensorboard_set_beacon(1);
- else
- i2c_sensorboard_set_beacon(0);
-}
-
-prog_char str_beacon_start_arg0[] = "beacon";
-parse_pgm_token_string_t cmd_beacon_start_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg0, str_beacon_start_arg0);
-prog_char str_beacon_start_arg1[] = "start#stop";
-parse_pgm_token_string_t cmd_beacon_start_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_beacon_start_result, arg1, str_beacon_start_arg1);
-
-prog_char help_beacon_start[] = "Beacon enabled/disable";
-parse_pgm_inst_t cmd_beacon_start = {
- .f = cmd_beacon_start_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_beacon_start,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_beacon_start_arg0,
- (prog_void *)&cmd_beacon_start_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Pump_Current */
-
-/* this structure is filled when cmd_pump_current is parsed successfully */
-struct cmd_pump_current_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
-};
-
-/* function called when cmd_pump_current is parsed successfully */
-static void cmd_pump_current_parsed(__attribute__((unused)) void *parsed_result,
- __attribute__((unused)) void *data)
-{
- printf_P(PSTR("l1=%d l2=%d r1=%d r2=%d\r\n"),
- sensor_get_adc(ADC_CSENSE3), sensor_get_adc(ADC_CSENSE4),
- mechboard.pump_right1_current, mechboard.pump_right2_current);
-}
-
-prog_char str_pump_current_arg0[] = "pump_current";
-parse_pgm_token_string_t cmd_pump_current_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg0, str_pump_current_arg0);
-prog_char str_pump_current_arg1[] = "show";
-parse_pgm_token_string_t cmd_pump_current_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_pump_current_result, arg1, str_pump_current_arg1);
-
-prog_char help_pump_current[] = "dump pump current";
-parse_pgm_inst_t cmd_pump_current = {
- .f = cmd_pump_current_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_pump_current,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_pump_current_arg0,
- (prog_void *)&cmd_pump_current_arg1,
- NULL,
- },
-};
-
-/**********************************************************/
-/* Build_Test */
-
-/* this structure is filled when cmd_build_test is parsed successfully */
-struct cmd_build_test_result {
- fixed_string_t arg0;
-};
-
-/* function called when cmd_build_test is parsed successfully */
-static void cmd_build_test_parsed(void *parsed_result, void *data)
-{
- //struct cmd_build_test_result *res = parsed_result;
-
- printf_P(PSTR("lintel must be there\r\n"));
- i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
- I2C_MECHBOARD_MODE_HARVEST);
- wait_ms(500);
-
- printf_P(PSTR("Insert 4 colums\r\n"));
- while (get_column_count() != 4);
-
- i2c_mechboard_mode_prepare_build_both(0);
- trajectory_d_rel(&mainboard.traj, 200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- wait_ms(500);
-
- i2c_mechboard_mode_simple_autobuild(0);
- wait_ms(100);
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
- trajectory_d_rel(&mainboard.traj, -200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
- I2C_MECHBOARD_MODE_HARVEST);
-
- while (get_column_count() != 3);
-
- i2c_mechboard_mode_prepare_build_both(3);
- trajectory_d_rel(&mainboard.traj, 200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- wait_ms(500);
-
- i2c_mechboard_mode_autobuild(3, 1, I2C_AUTOBUILD_DEFAULT_DIST,
- 3, 2,I2C_AUTOBUILD_DEFAULT_DIST, 0);
- i2cproto_wait_update();
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
- trajectory_d_rel(&mainboard.traj, -200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- i2c_mechboard_mode_prepare_pickup(I2C_RIGHT_SIDE);
- wait_ms(500);
-
- i2c_mechboard_mode_harvest();
- while (get_column_count() != 3);
-
- i2c_mechboard_mode_prepare_build_both(5);
- trajectory_d_rel(&mainboard.traj, 200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- wait_ms(1000);
-
- i2c_mechboard_mode_autobuild(4, 2, I2C_AUTOBUILD_DEFAULT_DIST,
- 5, 1, I2C_AUTOBUILD_DEFAULT_DIST, 0);
- i2cproto_wait_update();
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
- trajectory_d_rel(&mainboard.traj, -200);
-}
-
-prog_char str_build_test_arg0[] = "build_test";
-parse_pgm_token_string_t cmd_build_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_build_test_result, arg0, str_build_test_arg0);
-
-prog_char help_build_test[] = "Build_Test function";
-parse_pgm_inst_t cmd_build_test = {
- .f = cmd_build_test_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_build_test,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_build_test_arg0,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Column_Test */
-
-/* this structure is filled when cmd_column_test is parsed successfully */
-struct cmd_column_test_result {
- fixed_string_t arg0;
- uint8_t level;
- int16_t dist;
- int8_t a1;
- int8_t a2;
- int8_t a3;
- int16_t arm_dist;
- int8_t nb_col;
-};
-
-/* function called when cmd_column_test is parsed successfully */
-static void cmd_column_test_parsed(void *parsed_result, void *data)
-{
- struct cmd_column_test_result *res = parsed_result;
- uint8_t level = res->level, debug = 0;
- uint8_t c, push = 0;
-
- /* default conf */
- if (data) {
- res->dist = 70;
- res->a1 = -20;
- res->a2 = 40;
- res->a3 = -20;
- res->arm_dist = 220;
- res->nb_col = 2;
- }
-
- if (!strcmp_P(res->arg0, PSTR("column_test_debug")))
- debug = 1;
- if (!strcmp_P(res->arg0, PSTR("column_test_push")))
- push = 1;
-
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
-
- /* Go to disc */
-
- trajectory_d_rel(&mainboard.traj, 200);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- /* go back, insert colums */
-
- trajectory_d_rel(&mainboard.traj, -200);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- i2c_mechboard_mode_prepare_pickup_next(I2C_LEFT_SIDE,
- I2C_MECHBOARD_MODE_HARVEST);
- printf_P(PSTR("Insert 4 colums\r\n"));
- while (get_column_count() != 4);
-
- /* build with left arm */
-
- i2c_mechboard_mode_prepare_inside_both(level);
- trajectory_d_rel(&mainboard.traj, 200-(res->dist));
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- if (debug)
- c = cmdline_getchar_wait();
-
- trajectory_a_rel(&mainboard.traj, res->a1);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- if (debug)
- c = cmdline_getchar_wait();
-
- i2c_mechboard_mode_prepare_build_select(level, -1);
- time_wait_ms(200);
- if (debug)
- c = cmdline_getchar_wait();
- i2c_mechboard_mode_autobuild(level, res->nb_col, res->arm_dist,
- 0, 0, res->arm_dist, 0);
- while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
- if (debug)
- c = cmdline_getchar_wait();
- i2c_mechboard_mode_prepare_inside_select(level+res->nb_col, -1);
-
- if (debug)
- c = cmdline_getchar_wait();
- /* build with right arm */
-
- trajectory_a_rel(&mainboard.traj, res->a2);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- if (debug)
- c = cmdline_getchar_wait();
- /* only ok for nb_col == 2 */
- if ((level + res->nb_col) >= 7)
- i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col + 1);
- else
- i2c_mechboard_mode_prepare_build_select(-1, level + res->nb_col);
- time_wait_ms(200);
- if (debug)
- c = cmdline_getchar_wait();
- i2c_mechboard_mode_autobuild(0, 0, res->arm_dist,
- level + res->nb_col, res->nb_col,
- res->arm_dist, 0);
- while (get_mechboard_mode() != I2C_MECHBOARD_MODE_AUTOBUILD);
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_AUTOBUILD);
-
-
- if (push) {
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, -100);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- i2c_mechboard_mode_push_temple_disc(I2C_RIGHT_SIDE);
- time_wait_ms(500);
- trajectory_d_rel(&mainboard.traj, 100);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- }
- else if (level == 1 || level == 0) {
- trajectory_d_rel(&mainboard.traj, -100);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- i2c_mechboard_mode_push_temple(level);
- time_wait_ms(400);
- strat_set_speed(200, SPEED_ANGLE_SLOW);
- trajectory_d_rel(&mainboard.traj, 120);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- }
-
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
- if (debug)
- c = cmdline_getchar_wait();
- i2c_mechboard_mode_prepare_inside_select(-1, level+res->nb_col*2);
-
- if (debug)
- c = cmdline_getchar_wait();
-
- trajectory_a_rel(&mainboard.traj, res->a3);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
-
- if (debug)
- c = cmdline_getchar_wait();
- /* go back, insert colums */
-
- trajectory_d_rel(&mainboard.traj, -100);
-
- return;
-}
-
-prog_char str_column_test_arg0[] = "column_test#column_test_debug#column_test_push";
-parse_pgm_token_string_t cmd_column_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_column_test_result, arg0, str_column_test_arg0);
-parse_pgm_token_num_t cmd_column_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, level, UINT8);
-
-prog_char help_column_test[] = "Column_Test function (level)";
-parse_pgm_inst_t cmd_column_test = {
- .f = cmd_column_test_parsed, /* function to call */
- .data = (void *)1, /* 2nd arg of func */
- .help_str = help_column_test,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_column_test_arg0,
- (prog_void *)&cmd_column_test_arg1,
- NULL,
- },
-};
-
-parse_pgm_token_num_t cmd_column_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, dist, INT16);
-parse_pgm_token_num_t cmd_column_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a1, INT8);
-parse_pgm_token_num_t cmd_column_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a2, INT8);
-parse_pgm_token_num_t cmd_column_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, a3, INT8);
-parse_pgm_token_num_t cmd_column_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, arm_dist, INT16);
-parse_pgm_token_num_t cmd_column_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_column_test_result, nb_col, INT8);
-
-prog_char help_column_test2[] = "Column_Test function (level, dist, a1, a2, a3, arm_dist, nb_col)";
-parse_pgm_inst_t cmd_column_test2 = {
- .f = cmd_column_test_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_column_test2,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_column_test_arg0,
- (prog_void *)&cmd_column_test_arg1,
- (prog_void *)&cmd_column_test_arg2,
- (prog_void *)&cmd_column_test_arg3,
- (prog_void *)&cmd_column_test_arg4,
- (prog_void *)&cmd_column_test_arg5,
- (prog_void *)&cmd_column_test_arg6,
- (prog_void *)&cmd_column_test_arg7,
- NULL,
- },
-};
-
-
-/**********************************************************/
-/* Pickup_Test */
-
-/* this structure is filled when cmd_pickup_test is parsed successfully */
-struct cmd_pickup_test_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- int16_t dist;
-};
-
-/* return red or green sensor */
-#define COLOR_IR_SENSOR() \
- ({ \
- uint8_t __ret = 0; \
- if (side == I2C_RIGHT_SIDE) \
- __ret = sensor_get(S_DISP_RIGHT); \
- else \
- __ret = sensor_get(S_DISP_LEFT); \
- __ret; \
- }) \
-/* column dispensers */
-#define COL_SCAN_MARGIN 200
-/* distance between the wheel axis and the IR sensor */
-
-/* function called when cmd_pickup_test is parsed successfully */
-static void cmd_pickup_test_parsed(void *parsed_result, void *data)
-{
- uint8_t err, side, first_try = 1;
- int8_t cols_count_before, cols_count_after, cols;
- struct cmd_pickup_test_result *res = parsed_result;
- int16_t pos1, pos2, pos;
- microseconds us;
- int16_t dist = res->dist;
- uint8_t timeout = 0;
-
- if (!strcmp_P(res->arg1, PSTR("left")))
- side = I2C_LEFT_SIDE;
- else
- side = I2C_RIGHT_SIDE;
-
- i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
- cols_count_before = get_column_count();
- position_set(&mainboard.pos, 0, 0, 0);
-
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, -1000);
- err = WAIT_COND_OR_TRAJ_END(!COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
- if (err) /* we should not reach end */
- goto fail;
- pos1 = position_get_x_s16(&mainboard.pos);
- printf_P(PSTR("pos1 = %d\r\n"), pos1);
-
- err = WAIT_COND_OR_TRAJ_END(COLOR_IR_SENSOR(), TRAJ_FLAGS_NO_NEAR);
- if (err)
- goto fail;
- pos2 = position_get_x_s16(&mainboard.pos);
- printf_P(PSTR("pos2 = %d\r\n"), pos2);
-
- pos = ABS(pos1 - pos2);
- printf_P(PSTR("pos = %d\r\n"), pos);
-
- trajectory_d_rel(&mainboard.traj, -dist + pos/2);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- if (side == I2C_LEFT_SIDE)
- trajectory_a_rel(&mainboard.traj, 90);
- else
- trajectory_a_rel(&mainboard.traj, -90);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
-
- pickup_wheels_on();
- retry:
- if (first_try)
- i2c_mechboard_mode_lazy_harvest();
- else
- i2c_mechboard_mode_prepare_pickup(I2C_AUTO_SIDE);
- first_try = 0;
-
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, 300);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_FAST);
- err = strat_calib(600, TRAJ_FLAGS_SMALL_DIST);
-
- trajectory_d_rel(&mainboard.traj, -DIST_BACK_DISPENSER);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (!TRAJ_SUCCESS(err))
- goto fail;
-
- position_set(&mainboard.pos, 0, 0, 0);
- if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
- strat_eject_col(90, 0);
- goto retry;
- }
-
- /* start to pickup with finger / arms */
-
- printf_P(PSTR("%s pickup now\r\n"), __FUNCTION__);
- i2c_mechboard_mode_pickup();
- WAIT_COND_OR_TIMEOUT(get_mechboard_mode() ==
- I2C_MECHBOARD_MODE_PICKUP, 100);
- us = time_get_us2();
- cols = get_column_count();
- while (get_mechboard_mode() == I2C_MECHBOARD_MODE_PICKUP) {
- if (get_column_count() != cols) {
- cols = get_column_count();
- us = time_get_us2();
- }
- if ((get_column_count() - cols_count_before) >= 4) {
- printf_P(PSTR("%s no more cols in disp\r\n"), __FUNCTION__);
- break;
- }
- /* 1 second timeout */
- if (time_get_us2() - us > 1500000L) {
- printf_P(PSTR("%s timeout\r\n"), __FUNCTION__);
- timeout = 1;
- break;
- }
- }
-
- /* eject if we found a bad color column */
-
- if (get_mechboard_mode() == I2C_MECHBOARD_MODE_PREPARE_EJECT) {
- strat_eject_col(90, 0);
- goto retry;
- }
-
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
- trajectory_d_rel(&mainboard.traj, -250);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST | END_NEAR);
-
- cols_count_after = get_column_count();
- cols = cols_count_after - cols_count_before;
- DEBUG(E_USER_STRAT, "%s we got %d cols", __FUNCTION__, cols);
git.droids-corp.org / aversive.git / commitdiff