summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/asserv.c4
-rw-r--r--digital/io/src/asserv.h7
-rw-r--r--digital/io/src/init_cb.c4
-rw-r--r--digital/io/src/main.c3
-rw-r--r--digital/io/src/move.fsm4
-rw-r--r--digital/io/src/move_cb.c121
-rw-r--r--digital/io/src/top.c8
-rw-r--r--digital/io/src/top_cb.c35
8 files changed, 128 insertions, 58 deletions
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,10 +243,18 @@ 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
@@ -254,6 +262,46 @@ move_after_moving_backward (void)
}
/*
+ * 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
* ask the asserv to go to the 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);
}