- if (err == 0)
- err = END_ERROR;
- if (!TRAJ_SUCCESS(err)) {
- strat_hardstop();
- trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- if (mode != SCAN_MODE_TRAJ_ONLY)
- i2c_sensorboard_scanner_stop();
- return;
- }
-
- if (mode == SCAN_MODE_TRAJ_ONLY)
- return;
-
- wait_scan_done(10000);
-
- i2c_sensorboard_scanner_stop();
-
- if (mode == SCAN_MODE_CHECK_TEMPLE) {
- i2c_sensorboard_scanner_algo_check(res->level,
- res->center_x, res->center_y);
- i2cproto_wait_update();
- wait_scan_done(10000);
- scanner_dump_state();
-
- if (sensorboard.dropzone_h == -1) {
- printf_P(PSTR("-- try to build a temple\r\n"));
- res->center_x = 15;
- res->center_y = 13;
- mode = SCAN_MODE_SCAN_TEMPLE;
- }
- }
-
- if (mode == SCAN_MODE_SCAN_TEMPLE) {
- i2c_sensorboard_scanner_algo_temple(I2C_SCANNER_ZONE_DISC,
- res->center_x,
- res->center_y);
- i2cproto_wait_update();
- wait_scan_done(10000);
- scanner_dump_state();
-
- if (sensorboard.dropzone_h == -1 ||
- strat_scan_get_checkpoint(mode, &ckpt_rel_x,
- &ckpt_rel_y, &back_mm)) {
- printf_P(PSTR("-- try to build a column\r\n"));
- mode = SCAN_MODE_SCAN_COL;
- }
- }
-
- if (mode == SCAN_MODE_SCAN_COL) {
- i2c_sensorboard_scanner_algo_column(I2C_SCANNER_ZONE_DISC,
- res->center_x, res->center_y);
- i2cproto_wait_update();
- wait_scan_done(10000);
- scanner_dump_state();
-
- if (sensorboard.dropzone_h == -1 ||
- strat_scan_get_checkpoint(mode, &ckpt_rel_x,
- &ckpt_rel_y, &back_mm)) {
- return;
- }
- }
-
- if (sensorboard.dropzone_h == -1)
- return;
-
- if (mode == SCAN_MODE_CHECK_TEMPLE) {
- ckpt_rel_x = 220;
- ckpt_rel_y = 100;
- }
-
-
- printf_P(PSTR("rel xy for ckpt is %d,%d\r\n"), ckpt_rel_x, ckpt_rel_y);
-
- rel_xy_to_abs_xy(ckpt_rel_x, ckpt_rel_y, &ckpt_abs_x, &ckpt_abs_y);
- abs_xy_to_rel_da(ckpt_abs_x, ckpt_abs_y, &ckpt_rel_d, &ckpt_rel_a);
-
- printf_P(PSTR("abs ckpt is %2.2f,%2.2f\r\n"), ckpt_abs_x, ckpt_abs_y);
-
- printf_P(PSTR("ok ? (y/n)\r\n"));
-
- c = cmdline_getchar_wait();
-
- if (c != 'y')
- return;
-
- strat_set_speed(SPEED_DIST_FAST, SPEED_ANGLE_FAST);
-
- /* intermediate checkpoint for some positions */
- if ( (DEG(ckpt_rel_a) < 0 && DEG(ckpt_rel_a) > -90) ) {
- trajectory_goto_xy_rel(&mainboard.traj, 200, 100);
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- if (!TRAJ_SUCCESS(err))
- return;
- }
-
- trajectory_goto_xy_abs(&mainboard.traj, ckpt_abs_x, ckpt_abs_y);
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- if (!TRAJ_SUCCESS(err))
- return;
-
- trajectory_turnto_xy(&mainboard.traj, center_abs_x, center_abs_y);
- err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
- if (!TRAJ_SUCCESS(err))
- return;
-
- c = cmdline_getchar_wait();
-
- pos1x = position_get_x_s16(&mainboard.pos);
- pos1y = position_get_y_s16(&mainboard.pos);
-
- strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW);
- trajectory_d_rel(&mainboard.traj, 200);
- err = WAIT_COND_OR_TRAJ_END(distance_from_robot(pos1x, pos1y) > 200,
- TRAJ_FLAGS_SMALL_DIST);
- if (err == 0) {
- strat_set_speed(SPEED_DIST_VERY_SLOW, SPEED_ANGLE_VERY_SLOW);
- trajectory_d_rel(&mainboard.traj, 400);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- }
- if (err != END_BLOCKING)
- return;
-
- if (back_mm) {
- trajectory_d_rel(&mainboard.traj, -back_mm);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- }
-}
-
-prog_char str_scan_test_arg0[] = "scan_test";
-parse_pgm_token_string_t cmd_scan_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg0, str_scan_test_arg0);
-prog_char str_scan_test_arg1[] = "traj_only#scan_col#scan_temple";
-parse_pgm_token_string_t cmd_scan_test_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1);
-parse_pgm_token_num_t cmd_scan_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, start_dist, INT16);
-parse_pgm_token_num_t cmd_scan_test_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_dist, INT16);
-parse_pgm_token_num_t cmd_scan_test_arg4 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, scan_speed, INT16);
-parse_pgm_token_num_t cmd_scan_test_arg5 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_x, INT16);
-parse_pgm_token_num_t cmd_scan_test_arg6 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, center_y, INT16);
-
-prog_char help_scan_test[] = "Scan_Test function (start_dist, scan_dist, speed_dist, centerx, centery)";
-parse_pgm_inst_t cmd_scan_test = {
- .f = cmd_scan_test_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_scan_test,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_scan_test_arg0,
- (prog_void *)&cmd_scan_test_arg1,
- (prog_void *)&cmd_scan_test_arg2,
- (prog_void *)&cmd_scan_test_arg3,
- (prog_void *)&cmd_scan_test_arg4,
- (prog_void *)&cmd_scan_test_arg5,
- (prog_void *)&cmd_scan_test_arg6,
- NULL,
- },
-};
-
-prog_char str_scan_test_arg1b[] = "check_temple";
-parse_pgm_token_string_t cmd_scan_test_arg1b = TOKEN_STRING_INITIALIZER(struct cmd_scan_test_result, arg1, str_scan_test_arg1b);
-parse_pgm_token_num_t cmd_scan_test_arg7 = TOKEN_NUM_INITIALIZER(struct cmd_scan_test_result, level, UINT8);