- uint8_t err, mode=0, c;
- int16_t pos1x, pos1y, dist;
- struct cmd_scan_test_result *res = parsed_result;
- int16_t back_mm = 0;
-
- int16_t ckpt_rel_x = 0, ckpt_rel_y = 0;
-
- double center_abs_x, center_abs_y;
- double ckpt_rel_d, ckpt_rel_a;
- double ckpt_abs_x, ckpt_abs_y;
-
- if (!strcmp_P(res->arg1, PSTR("traj_only")))
- mode = SCAN_MODE_TRAJ_ONLY;
- else if (!strcmp_P(res->arg1, PSTR("check_temple")))
- mode = SCAN_MODE_CHECK_TEMPLE;
- else if (!strcmp_P(res->arg1, PSTR("scan_col")))
- mode = SCAN_MODE_SCAN_COL;
- else if (!strcmp_P(res->arg1, PSTR("scan_temple")))
- mode = SCAN_MODE_SCAN_TEMPLE;
-
- /* go to disc */
- 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;
-
- /* save absolute position of disc */
- rel_da_to_abs_xy(265, 0, ¢er_abs_x, ¢er_abs_y);
-
- /* go back and prepare to scan */
- strat_set_speed(1000, 1000);
- trajectory_d_a_rel(&mainboard.traj, -140, 130);
- err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (!TRAJ_SUCCESS(err))
- return;
-
- /* prepare scanner arm */
- if (mode != SCAN_MODE_TRAJ_ONLY)
- i2c_sensorboard_scanner_prepare();
- time_wait_ms(250);
-
- strat_set_speed(res->scan_speed, 1000);
-
- pos1x = position_get_x_s16(&mainboard.pos);
- pos1y = position_get_y_s16(&mainboard.pos);
- trajectory_d_rel(&mainboard.traj, -res->scan_dist);
-
- while (1) {
- err = test_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (err != 0)
- break;
-
- dist = distance_from_robot(pos1x, pos1y);
-
- if (dist > res->start_dist)
- break;
-
- if (get_scanner_status() & I2C_SCAN_MAX_COLUMN) {
- err = END_ERROR;
- break;
- }
- }
-
- if (err) {
- if (TRAJ_SUCCESS(err))
- err = END_ERROR; /* should not reach end */
- strat_hardstop();
- trajectory_goto_xy_abs(&mainboard.traj, pos1x, pos1y);
- wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
- if (mode != SCAN_MODE_TRAJ_ONLY)
- i2c_sensorboard_scanner_stop();
- return;
- }
-
- /* start the scanner */
-
- if (mode != SCAN_MODE_TRAJ_ONLY)
- i2c_sensorboard_scanner_start();
-
- err = WAIT_COND_OR_TRAJ_END(get_scanner_status() & I2C_SCAN_MAX_COLUMN,
- TRAJ_FLAGS_NO_NEAR);
- 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);
- }