- fixed_string_t arg1;
- int16_t start_dist;
- int16_t scan_dist;
- int16_t scan_speed;
- int16_t center_x;
- int16_t center_y;
- uint8_t level;
-};
-
-#define SCAN_MODE_CHECK_TEMPLE 0
-#define SCAN_MODE_SCAN_COL 1
-#define SCAN_MODE_SCAN_TEMPLE 2
-#define SCAN_MODE_TRAJ_ONLY 3
-
-/* function called when cmd_scan_test is parsed successfully */
-static void cmd_scan_test_parsed(void *parsed_result, void *data)
-{
- 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);
- }
-}
-
-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);
-
-prog_char help_scan_test2[] = "Scan_Test function (start_dist, scan_dist, speed_dist, templex, templey, level)";
-parse_pgm_inst_t cmd_scan_test2 = {
- .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_arg1b,
- (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,
- (prog_void *)&cmd_scan_test_arg7,
- NULL,
- },