From 503e3511b2fa42263749c62a9625e6549ac2fe2d Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 20 May 2009 19:22:02 +0200 Subject: * digital/io/src: - support backward parameter for the asserv_goto, - enhance to avoid static obstacle, - add center circle of the playground table. --- digital/io/src/asserv.c | 4 +- digital/io/src/asserv.h | 7 ++- digital/io/src/init_cb.c | 4 ++ digital/io/src/main.c | 3 +- digital/io/src/move.fsm | 4 +- digital/io/src/move_cb.c | 121 +++++++++++++++++++++++++++++++++++------------ digital/io/src/top.c | 8 ++-- digital/io/src/top_cb.c | 35 +++++++------- 8 files changed, 128 insertions(+), 58 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index 062ca13a..d8fc2ea0 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -627,7 +627,7 @@ asserv_set_position (int32_t x, int32_t y, int16_t angle) /* Go to an absolute position in (X, Y). */ void -asserv_goto (uint32_t x, uint32_t y) +asserv_goto (uint32_t x, uint32_t y, uint8_t backward) { x = fixed_mul_f824 (x, asserv_scale_inv); y = fixed_mul_f824 (y, asserv_scale_inv); @@ -640,7 +640,7 @@ asserv_goto (uint32_t x, uint32_t y) asserv_twi_buffer_param[4] = v32_to_v8 (y, 1); asserv_twi_buffer_param[5] = v32_to_v8 (y, 0); /* No backward. */ - asserv_twi_buffer_param[6] = 0; + asserv_twi_buffer_param[6] = backward; /* Send the got to an absolute position command to the asserv board */ asserv_twi_send_command ('x', 7); } diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h index 82f405c0..06be268b 100644 --- a/digital/io/src/asserv.h +++ b/digital/io/src/asserv.h @@ -226,7 +226,8 @@ asserv_goto_angle (int16_t angle); * @param x the absolute position on the X axis. * @param y the absolute position on the Y axis. * @param a the absolute angle. - * @param backward 0 no backward, 1 backward possible, 2 backward compulsary. + * @param backward 0 no backward, ASSERV_BACKWARD backward compulsary, + * ASSERV_REVERT_OK backward allowed. */ void asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward); @@ -320,9 +321,11 @@ asserv_set_position (int32_t x, int32_t y, int16_t angle); * Go to an absolute position in (X, Y). * @param x the x position on the table. * @param y the y position on the table. + * @param backward 0 no backward, ASSERV_BACKWARD backward compulsary, + * ASSERV_REVERT_OK backward allowed. */ void -asserv_goto (uint32_t x, uint32_t y); +asserv_goto (uint32_t x, uint32_t y, uint8_t backward); /** * Go to an absolute position at (X, Y) with backward enabled. diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c index b74937b2..f79ca49a 100644 --- a/digital/io/src/init_cb.c +++ b/digital/io/src/init_cb.c @@ -32,7 +32,9 @@ #include "aquajim.h" #include "switch.h" #include "chrono.h" +#include "playground.h" #include "modules/trace/trace.h" +#include "modules/path/path.h" /* * IDLE =start=> @@ -238,6 +240,8 @@ init__WAIT_SECOND_JACK_OUT__jack_removed_from_bot (void) init_match_is_started = 1; /* Start the chrono. */ chrono_init (); + /* Set the central round to path module. */ + path_obstacle (0x01, PG_WIDTH / 2, PG_LENGTH / 2, 350, 0xFFFF); return init_next (WAIT_SECOND_JACK_OUT, jack_removed_from_bot); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index c84c7ca6..63dbfa03 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -753,7 +753,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 4b: y. */ asserv_goto (v8_to_v32 (args[1], args[2], args[3], args[4]), - v8_to_v32 (args[5], args[6], args[7], args[8])); + v8_to_v32 (args[5], args[6], args[7], args[8]), + 0); break; case 'r': /* Go to an absolute position (X,Y) in mm with backward diff --git a/digital/io/src/move.fsm b/digital/io/src/move.fsm index 581378ed..f5a56253 100644 --- a/digital/io/src/move.fsm +++ b/digital/io/src/move.fsm @@ -30,7 +30,9 @@ Events: state_timeout IDLE: - start -> MOVING_TO_FINAL_POSITION + start: intermediate_path_found -> MOVING_TO_INTERMEDIATE_POSITION + ask the asserv to go to the intermediate position + start: no_intermediate_path_found -> MOVING_TO_FINAL_POSITION ask the asserv to go to the final position MOVING_TO_FINAL_POSITION: diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index 98f609b6..95dc3df9 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -243,16 +243,64 @@ move_after_moving_backward (void) if (move_get_next_position (&move_data.intermediate)) { /* Go to the next position */ - asserv_goto_xya (move_data.intermediate.x, + if (move_data.intermediate.x == move_data.final.x + && move_data.intermediate.y == move_data.final.y) + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.final.a, + move_data.backward_movement_allowed); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y, - move_data.intermediate.a, move_data.backward_movement_allowed); + + return 1; } else return 0; } +/* + * IDLE =start=> + * intermediate_path_found => MOVING_TO_INTERMEDIATE_POSITION + * ask the asserv to go to the intermediate position + * no_intermediate_path_found => MOVING_TO_FINAL_POSITION + * ask the asserv to go to the final position + */ +fsm_branch_t +move__IDLE__start (void) +{ + if (!move_get_next_position (&move_data.intermediate)) + { + if (move_data.intermediate.x == move_data.final.x + && move_data.intermediate.y == move_data.final.y) + { + /* Go to the destination position */ + asserv_goto_xya (move_data.final.x, + move_data.final.y, + move_data.final.a, + move_data.backward_movement_allowed); + return move_next_branch (IDLE, start, no_intermediate_path_found); + } + else + { + asserv_goto (move_data.intermediate.x, + move_data.intermediate.y, + move_data.backward_movement_allowed); + return move_next_branch (IDLE, start, intermediate_path_found); + } + } + else + { + asserv_goto_xya (move_data.final.x, + move_data.final.y, + move_data.final.a, + move_data.backward_movement_allowed); + return move_next_branch (IDLE, start, no_intermediate_path_found); + } +} + /* * WAIT_FOR_CLEAR_PATH =wait_finished=> * no_obstacle => MOVING_TO_FINAL_POSITION @@ -287,11 +335,18 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void) { /* Ignore sharps */ main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT; - /* Go to the next intermediate position */ - asserv_goto_xya (move_data.intermediate.x, + if (move_data.intermediate.x == move_data.final.x + && move_data.intermediate.y == move_data.final.y) + /* Go to the next intermediate position */ + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.final.a, + move_data.backward_movement_allowed); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y, - move_data.intermediate.a, move_data.backward_movement_allowed); + return move_next_branch (WAIT_FOR_CLEAR_PATH, wait_finished, obstacle_and_intermediate_path_found); } else @@ -307,21 +362,6 @@ move__WAIT_FOR_CLEAR_PATH__wait_finished (void) } } -/* - * IDLE =start=> - * => MOVING_TO_FINAL_POSITION - * ask the asserv to go to the final position - */ -fsm_branch_t -move__IDLE__start (void) -{ - /* Go to the destination position */ - asserv_goto_xya (move_data.final.x, - move_data.final.y, - move_data.final.a, - move_data.backward_movement_allowed); - return move_next (IDLE, start); -} /* * MOVING_TO_INTERMEDIATE_POSITION =bot_move_succeed=> @@ -349,11 +389,18 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_succeed (void) /* Get next position */ if (move_get_next_position (&move_data.intermediate)) { - /* Go to the next intermediate position */ - asserv_goto_xya (move_data.intermediate.x, + if (move_data.intermediate.x == move_data.final.x + && move_data.intermediate.y == move_data.final.y) + /* Go to the next intermediate position */ + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.final.a, + move_data.backward_movement_allowed); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y, - move_data.intermediate.a, move_data.backward_movement_allowed); + return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_succeed, position_intermediary); } else @@ -390,11 +437,17 @@ move__MOVING_TO_INTERMEDIATE_POSITION__bot_move_obstacle (void) /* Get next position */ if (move_get_next_position (&move_data.intermediate)) { - /* Go to the next intermediate position */ - asserv_goto_xya (move_data.intermediate.x, - move_data.intermediate.y, - move_data.intermediate.a, - move_data.backward_movement_allowed); + if (move_data.intermediate.x == move_data.final.x + && move_data.intermediate.y == move_data.final.y) + /* Go to the next intermediate position */ + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.final.a, + move_data.backward_movement_allowed); + else + asserv_goto (move_data.intermediate.x, + move_data.intermediate.y, + move_data.backward_movement_allowed); return move_next_branch (MOVING_TO_INTERMEDIATE_POSITION, bot_move_obstacle, intermediate_path_found); } else @@ -537,10 +590,16 @@ move__MOVING_TO_FINAL_POSITION__bot_move_obstacle (void) /* Get next position */ if (move_get_next_position (&move_data.intermediate)) { - /* Go to the next intermediate position */ - asserv_goto_xya (move_data.intermediate.x, + if (move_data.intermediate.x == move_data.final.x + && move_data.intermediate.y == move_data.final.y) + /* Go to the next intermediate position */ + asserv_goto_xya (move_data.intermediate.x, + move_data.intermediate.y, + move_data.final.a, + move_data.backward_movement_allowed); + else + asserv_goto (move_data.intermediate.x, move_data.intermediate.y, - move_data.intermediate.a, move_data.backward_movement_allowed); return move_next_branch (MOVING_TO_FINAL_POSITION, bot_move_obstacle, intermediate_path_found); } diff --git a/digital/io/src/top.c b/digital/io/src/top.c index 40c6280b..718e1e74 100644 --- a/digital/io/src/top.c +++ b/digital/io/src/top.c @@ -82,16 +82,16 @@ top_get_next_position_to_get_distributor (asserv_position_t *clean_position, static const asserv_position_t distributor[DISTRIBUTOR_COUNT][2] = { /* First distributor. */ - { { 2500, 170, 0 }, { 2711, 170, 280 } }, + { { 2711, 270, 270 }, { 2711, 200, 270 } }, /* Second distributor. */ - { { 2830, 600, 180 }, { 2830, 800, 0 } }, + { { 2730, 600, 180 }, { 2730, 800, 0 } }, /* Third distributor. */ - { { 2830, 1100, 180 }, { 2830, 1300, 0 } }, + { { 2730, 1100, 180 }, { 2730, 1300, 0 } }, }; /* Maximum number of remaining puck by distributor. */ static uint8_t distributor_puck_count[DISTRIBUTOR_COUNT] = { 5, 5, 5 }; /* Current distributor. */ - static uint8_t current_position = DISTRIBUTOR_COUNT; + static uint8_t current_position = DISTRIBUTOR_COUNT ; /* Keep track of how many pucks we have get. This variable will be used to * know how many puck were taken for each distributor. */ static uint8_t previous_total_puck_taken = 0; diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index cd5d54a4..f91398a5 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -31,6 +31,7 @@ #include "move.h" #include "chrono.h" #include "playground.h" +#include "asserv.h" /** * Internal data. @@ -87,7 +88,7 @@ top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next_branch (GET_PUCK_FROM_THE_GROUND, move_fsm_succeed, already_six_pucks_or_no_next_position); } else @@ -112,7 +113,7 @@ top__GET_PUCK_FROM_THE_GROUND__move_fsm_failed (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next (GET_PUCK_FROM_THE_GROUND, move_fsm_failed); } @@ -151,7 +152,7 @@ fsm_branch_t top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_succeed (void) { /* Go in the front of the distributor. */ - move_start (front_position, 1); + move_start (front_position, ASSERV_REVERT_OK); return top_next (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_succeed); } @@ -173,7 +174,7 @@ top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_failed (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, no_puck_or_still_time); } else @@ -182,7 +183,7 @@ top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_failed (void) /* Go to unload area. */ top_get_next_position_to_unload_puck (&position); /* Go there. */ - move_start (position, 2); + move_start (position, ASSERV_BACKWARD); return top_next_branch (GET_PUCK_FROM_DISTRIBUTOR, move_fsm_failed, some_pucks_and_no_more_time); } } @@ -200,7 +201,7 @@ top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (void) /* Go to unload area. */ top_get_next_position_to_unload_puck (&position); /* Go there. */ - move_start (position, 2); + move_start (position, ASSERV_BACKWARD); return top_next (STOP_TO_GO_TO_UNLOAD_AREA, move_fsm_stopped); } @@ -243,7 +244,7 @@ top__STOP_TO_GET_PUCK_FROM_DISTRIBUTOR__move_fsm_stopped (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next (STOP_TO_GET_PUCK_FROM_DISTRIBUTOR, move_fsm_stopped); } @@ -273,7 +274,7 @@ top__GO_TO_UNLOAD_AREA__move_fsm_failed (void) /* Go to unload area. */ top_get_next_position_to_unload_puck (&position); /* Go there. */ - move_start (position, 2); + move_start (position, ASSERV_BACKWARD); return top_next (GO_TO_UNLOAD_AREA, move_fsm_failed); } @@ -367,7 +368,7 @@ top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void) /* Go to unload area. */ top_get_next_position_to_unload_puck (&position); /* Go there. */ - move_start (position, 2); + move_start (position, ASSERV_BACKWARD); return top_next (GO_AWAY_TO_RETRY_UNLOAD, bot_move_succeed); } @@ -451,7 +452,7 @@ top__FUCK_THE_DISTRIBUTOR__bot_move_failed (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, no_puck_or_still_time); } @@ -461,7 +462,7 @@ top__FUCK_THE_DISTRIBUTOR__bot_move_failed (void) /* Go to unload area. */ top_get_next_position_to_unload_puck (&position); /* Go there. */ - move_start (position, 2); + move_start (position, ASSERV_BACKWARD); return top_next_branch (FUCK_THE_DISTRIBUTOR, bot_move_failed, some_pucks_and_no_more_time); } @@ -480,7 +481,7 @@ top__WAIT_FOR_PUCKS__bot_is_full_of_pucks (void) /* Go to unload area. */ top_get_next_position_to_unload_puck (&position); /* Go there. */ - move_start (position, 2); + move_start (position, ASSERV_BACKWARD); return top_next (WAIT_FOR_PUCKS, bot_is_full_of_pucks); } @@ -501,7 +502,7 @@ top__WAIT_FOR_PUCKS__empty_distributor (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, no_puck_or_still_time); } @@ -511,7 +512,7 @@ top__WAIT_FOR_PUCKS__empty_distributor (void) /* Go to unload area. */ top_get_next_position_to_unload_puck (&position); /* Go there. */ - move_start (position, 2); + move_start (position, ASSERV_BACKWARD); return top_next_branch (WAIT_FOR_PUCKS, empty_distributor, some_pucks_and_no_more_time); } @@ -535,7 +536,7 @@ top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_succeed, more_than_six_pucks_or_no_next_position); } @@ -577,7 +578,7 @@ top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next_branch (GO_AWAY_FROM_UNLOAD_AREA, bot_move_failed, no_more_niceness_and_more_than_six_pucks_or_no_next_position); } @@ -643,7 +644,7 @@ top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_failed (void) /* Go to distributor. */ top_get_next_position_to_get_distributor (&position, &front_position); /* Go there. */ - move_start (position, 0); + move_start (position, ASSERV_BACKWARD); return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_failed); } -- cgit v1.2.3