summaryrefslogtreecommitdiffhomepage
path: root/digital/io
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/main.c18
-rw-r--r--digital/io/src/top.fsm23
-rw-r--r--digital/io/src/top_cb.c81
3 files changed, 94 insertions, 28 deletions
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 3003d3c4..2c11ba89 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -187,12 +187,16 @@ main_event_to_fsm (void)
MOVE_EVENT_bot_move_succeed);
FSM_HANDLE_EVENT (&init_fsm,
INIT_EVENT_bot_move_succeed);
+ FSM_HANDLE_EVENT (&top_fsm,
+ TOP_EVENT_bot_move_succeed);
}
else if (move_status == failure)
{
/* Move failed. */
FSM_HANDLE_EVENT (&move_fsm,
MOVE_EVENT_bot_move_failed);
+ FSM_HANDLE_EVENT (&top_fsm,
+ TOP_EVENT_bot_move_failed);
}
/* Check elevator status. */
@@ -212,16 +216,15 @@ main_event_to_fsm (void)
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_move_done);
}
- else if (elevator_status == failure)
- {
- /* TODO: */
- }
/* FIXME: use general setting ack. */
/* send event if elevator received an order */
- if(elvt_order)
+ if (elvt_order)
FSM_HANDLE_EVENT (&elevator_fsm,
ELEVATOR_EVENT_order_received);
+ else
+ FSM_HANDLE_EVENT (&top_fsm,
+ TOP_EVENT_elevator_order_done);
/* FIXME: use general setting ack or not? */
@@ -328,6 +331,11 @@ main_event_to_fsm (void)
CYLINDER_EVENT_new_puck);
}
+ if (top_puck_inside_bot >= 4)
+ {
+ FSM_HANDLE_EVENT (&top_fsm,
+ TOP_EVENT_bot_is_full_of_pucks);
+ }
}
/**
diff --git a/digital/io/src/top.fsm b/digital/io/src/top.fsm
index 72244edf..2c286ee5 100644
--- a/digital/io/src/top.fsm
+++ b/digital/io/src/top.fsm
@@ -7,8 +7,8 @@ States:
waiting for the beginning of the top FSM.
WAIT_INIT_TO_FINISH
waiting for the end of init.
- GET_PUCK_FROM_THE_GROUND [timeout=1590]
- get some pucks from the ground (with a timeout set to 7 seconds).
+ GET_PUCK_FROM_THE_GROUND [timeout=1575]
+ get some pucks from the ground.
GET_PUCK_FROM_DISTRIBUTOR
get some pucks from the distributor.
STOP_TO_GO_TO_UNLOAD_AREA
@@ -21,7 +21,7 @@ States:
when going backward in position for unload.
UNLOAD_PUCKS [timeout=1125]
when unloading all pucks from the bot.
- ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD [timeout=1275]
+ ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD [timeout=1125]
wait for the elevator to be ready for a bot movement.
GO_AWAY_TO_RETRY_UNLOAD
when unload failed, go away from the unload area to try a new one.
@@ -29,7 +29,7 @@ States:
try again to go away to retry unloading pucks.
FUCK_THE_DISTRIBUTOR
fuck the distributor.
- WAIT_FOR_PUCKS
+ WAIT_FOR_PUCKS [timeout=1575]
wait until no more pucks or already full.
GO_AWAY_FROM_UNLOAD_AREA
go away from the unload area to turn freely.
@@ -61,8 +61,8 @@ Events:
the bot has four pucks inside.
empty_distributor
the cylinder has tried to get some pucks but none has been seen.
- elevator_is_closed
- the elevator has finished to unload the pucks.
+ elevator_order_done
+ the elevator has executed my order (go up, open door, close door).
IDLE:
start -> WAIT_INIT_TO_FINISH
@@ -74,15 +74,18 @@ WAIT_INIT_TO_FINISH:
GET_PUCK_FROM_THE_GROUND:
move_fsm_succeed: already_six_pucks_or_no_next_position -> GET_PUCK_FROM_DISTRIBUTOR
+ close cylinder.
get the next distributor position and launch move FSM to go there.
move_fsm_succeed: next_position_exists -> .
go to the next position using move FSM.
move_fsm_failed -> GET_PUCK_FROM_DISTRIBUTOR
+ close cylinder.
we have failed to do a move, go the distributor (get next distributor and go
there with move FSM).
bot_is_full_of_pucks -> STOP_TO_GO_TO_UNLOAD_AREA
stop move FSM.
state_timeout -> STOP_TO_GET_PUCK_FROM_DISTRIBUTOR
+ close cylinder.
too much time lost to get puck from the ground, stop move FSM.
STOP_TO_GET_PUCK_FROM_DISTRIBUTOR:
@@ -160,9 +163,13 @@ WAIT_FOR_PUCKS:
get a new distributor position and go there.
empty_distributor: some_pucks_and_no_more_time -> GO_TO_UNLOAD_AREA
compute an unload area and go there.
+ state_timeout: no_puck_or_still_time -> GET_PUCK_FROM_DISTRIBUTOR
+ do the same as empty distributor.
+ state_timeout: some_pucks_and_no_more_time -> GO_TO_UNLOAD_AREA
+ do the same as empty distributor.
UNLOAD_PUCKS:
- elevator_is_closed -> GO_AWAY_FROM_UNLOAD_AREA
+ elevator_order_done -> GO_AWAY_FROM_UNLOAD_AREA
linear move.
state_timeout -> ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD
we have failed, ask the elevator to put itself in a state where bot movement
@@ -188,7 +195,7 @@ TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA:
do the same as succeed.
ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD:
- elevator_is_closed -> GO_AWAY_TO_RETRY_UNLOAD
+ elevator_order_done -> GO_AWAY_TO_RETRY_UNLOAD
move backward from the unload area using linear move.
state_timeout -> GO_AWAY_TO_RETRY_UNLOAD
do the same as elevator_ready.
diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c
index f91398a5..524385bb 100644
--- a/digital/io/src/top_cb.c
+++ b/digital/io/src/top_cb.c
@@ -32,6 +32,8 @@
#include "chrono.h"
#include "playground.h"
#include "asserv.h"
+#include "cylinder.h"
+#include "elevator.h"
/**
* Internal data.
@@ -85,6 +87,8 @@ top__GET_PUCK_FROM_THE_GROUND__move_fsm_succeed (void)
if (!top_get_next_position_to_get_puck_on_the_ground (&position, 0)
|| top_total_puck_taken >= 6)
{
+ /* Ensure cylinder is close. */
+ cylinder_close_order = 1;
/* Go to distributor. */
top_get_next_position_to_get_distributor (&position, &front_position);
/* Go there. */
@@ -110,6 +114,8 @@ top__GET_PUCK_FROM_THE_GROUND__move_fsm_failed (void)
{
/* Get next position. */
asserv_position_t position;
+ /* Close the cylinder. */
+ cylinder_close_order = 1;
/* Go to distributor. */
top_get_next_position_to_get_distributor (&position, &front_position);
/* Go there. */
@@ -140,6 +146,8 @@ top__GET_PUCK_FROM_THE_GROUND__state_timeout (void)
{
/* Stop move FSM. */
move_stop ();
+ /* Close cylinder. */
+ cylinder_close_order = 1;
return top_next (GET_PUCK_FROM_THE_GROUND, state_timeout);
}
@@ -179,6 +187,7 @@ top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_failed (void)
}
else
{
+ /* Ensure cylinder is closed. */
asserv_position_t position;
/* Go to unload area. */
top_get_next_position_to_unload_puck (&position);
@@ -197,6 +206,8 @@ top__GET_PUCK_FROM_DISTRIBUTOR__move_fsm_failed (void)
fsm_branch_t
top__STOP_TO_GO_TO_UNLOAD_AREA__move_fsm_stopped (void)
{
+ /* Ensure cylinder is closed. */
+ cylinder_close_order = 1;
asserv_position_t position;
/* Go to unload area. */
top_get_next_position_to_unload_puck (&position);
@@ -239,6 +250,8 @@ top__STOP_TO_GO_TO_UNLOAD_AREA__bot_move_failed (void)
fsm_branch_t
top__STOP_TO_GET_PUCK_FROM_DISTRIBUTOR__move_fsm_stopped (void)
{
+ /* Ensure cylinder is closed. */
+ cylinder_close_order = 1;
/* Get next position. */
asserv_position_t position;
/* Go to distributor. */
@@ -286,7 +299,8 @@ top__GO_TO_UNLOAD_AREA__move_fsm_failed (void)
fsm_branch_t
top__FUCK_UNLOAD_AREA__bot_move_succeed (void)
{
- /* TODO: elevator order to unload. */
+ /* Close elevator. */
+ elvt_order = 3;
return top_next (FUCK_UNLOAD_AREA, bot_move_succeed);
}
@@ -304,16 +318,16 @@ top__FUCK_UNLOAD_AREA__bot_move_failed (void)
}
/*
- * UNLOAD_PUCKS =elevator_is_closed=>
+ * UNLOAD_PUCKS =elevator_order_done=>
* => GO_AWAY_FROM_UNLOAD_AREA
* linear move.
*/
fsm_branch_t
-top__UNLOAD_PUCKS__elevator_is_closed (void)
+top__UNLOAD_PUCKS__elevator_order_done (void)
{
/* Move forward. */
asserv_move_linearly (PG_BORDER_DISTANCE);
- return top_next (UNLOAD_PUCKS, elevator_is_closed);
+ return top_next (UNLOAD_PUCKS, elevator_order_done);
}
/*
@@ -325,21 +339,22 @@ top__UNLOAD_PUCKS__elevator_is_closed (void)
fsm_branch_t
top__UNLOAD_PUCKS__state_timeout (void)
{
- /* TODO: elevator close. */
+ /* Elevator close. */
+ elvt_order = 3;
return top_next (UNLOAD_PUCKS, state_timeout);
}
/*
- * ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD =elevator_is_closed=>
+ * ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD =elevator_order_done=>
* => GO_AWAY_TO_RETRY_UNLOAD
* move backward from the unload area using linear move.
*/
fsm_branch_t
-top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_is_closed (void)
+top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_order_done (void)
{
/* Move forward. */
asserv_move_linearly (PG_BORDER_DISTANCE);
- return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, elevator_is_closed);
+ return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, elevator_order_done);
}
/*
@@ -351,7 +366,7 @@ fsm_branch_t
top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__state_timeout (void)
{
/* Yerk. */
- top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_is_closed ();
+ top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__elevator_order_done ();
return top_next (ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD, state_timeout);
}
@@ -359,7 +374,7 @@ top__ELEVATOR_READY_TO_GO_AWAY_TO_RETRY_UNLOAD__state_timeout (void)
* GO_AWAY_TO_RETRY_UNLOAD =bot_move_succeed=>
* => GO_TO_UNLOAD_AREA
* compute a new unload area.
- * ask move FSM to go there using forward move only.
+ * ask move FSM to go there using backward move only.
*/
fsm_branch_t
top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void)
@@ -376,7 +391,7 @@ top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void)
* GO_AWAY_TO_RETRY_UNLOAD =bot_move_failed=>
* niceness => TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD
* decrement niceness.
- * move backward using linear move.
+ * move forward using linear move.
* no_more_niceness => GO_TO_UNLOAD_AREA
* do the same as move succeed.
*/
@@ -399,7 +414,7 @@ top__GO_AWAY_TO_RETRY_UNLOAD__bot_move_failed (void)
/*
* TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD =bot_move_succeed=>
* => GO_AWAY_TO_RETRY_UNLOAD
- * move forward from the unload area using linear move.
+ * move backward from the unload area using linear move.
*/
fsm_branch_t
top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_succeed (void)
@@ -430,7 +445,7 @@ top__TRY_AGAIN_TO_GO_AWAY_TO_RETRY_UNLOAD__bot_move_failed (void)
fsm_branch_t
top__FUCK_THE_DISTRIBUTOR__bot_move_succeed (void)
{
- /* FIXME: nothing to do? */
+ /* TODO: tell it to cylinder. */
return top_next (FUCK_THE_DISTRIBUTOR, bot_move_succeed);
}
@@ -445,6 +460,8 @@ top__FUCK_THE_DISTRIBUTOR__bot_move_succeed (void)
fsm_branch_t
top__FUCK_THE_DISTRIBUTOR__bot_move_failed (void)
{
+ /* Close cylinder. */
+ cylinder_close_order = 1;
if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT)
{
/* Get next position. */
@@ -519,6 +536,37 @@ top__WAIT_FOR_PUCKS__empty_distributor (void)
}
/*
+ * WAIT_FOR_PUCKS =state_timeout=>
+ * no_puck_or_still_time => GET_PUCK_FROM_DISTRIBUTOR
+ * do the same as empty distributor.
+ * some_pucks_and_no_more_time => GO_TO_UNLOAD_AREA
+ * do the same as empty distributor.
+ */
+fsm_branch_t
+top__WAIT_FOR_PUCKS__state_timeout (void)
+{
+ if (!top_puck_inside_bot || chrono_remaining_time () > TOP_TIME_LIMIT)
+ {
+ /* Get next position. */
+ asserv_position_t position;
+ /* Go to distributor. */
+ top_get_next_position_to_get_distributor (&position, &front_position);
+ /* Go there. */
+ move_start (position, ASSERV_BACKWARD);
+ return top_next_branch (WAIT_FOR_PUCKS, state_timeout, no_puck_or_still_time);
+ }
+ else
+ {
+ asserv_position_t position;
+ /* Go to unload area. */
+ top_get_next_position_to_unload_puck (&position);
+ /* Go there. */
+ move_start (position, ASSERV_BACKWARD);
+ return top_next_branch (WAIT_FOR_PUCKS, state_timeout, some_pucks_and_no_more_time);
+ }
+}
+
+/*
* GO_AWAY_FROM_UNLOAD_AREA =bot_move_succeed=>
* more_than_six_pucks_or_no_next_position => GET_PUCK_FROM_DISTRIBUTOR
* get the next distributor position and launch move FSM to go there.
@@ -553,7 +601,7 @@ top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void)
* GO_AWAY_FROM_UNLOAD_AREA =bot_move_failed=>
* niceness => TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA
* decrement niceness.
- * move backward using linear move.
+ * move forward using linear move.
* no_more_niceness_and_more_than_six_pucks_or_no_next_position => GET_PUCK_FROM_DISTRIBUTOR
* go to the next position using move FSM.
* no_more_niceness_and_next_position_for_pucks_on_ground_exists => GET_PUCK_FROM_THE_GROUND
@@ -595,7 +643,7 @@ top__GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void)
/*
* TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA =bot_move_succeed=>
* => GO_AWAY_FROM_UNLOAD_AREA
- * move forward using linear move.
+ * move backward using linear move.
*/
fsm_branch_t
top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_succeed (void)
@@ -626,6 +674,9 @@ top__TRY_AGAIN_TO_GO_AWAY_FROM_UNLOAD_AREA__bot_move_failed (void)
fsm_branch_t
top__CLEAN_FRONT_OF_DISTRIBUTOR__move_fsm_succeed (void)
{
+ /* Open cylinder. */
+ cylinder_close_order = 0;
+ /* TODO: alete cylinder to open. */
/* Fuck the distributor. */
asserv_go_to_the_wall (0);
return top_next (CLEAN_FRONT_OF_DISTRIBUTOR, move_fsm_succeed);