-/**********************************************************/
-/* 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);