summaryrefslogtreecommitdiff
path: root/digital/io
diff options
context:
space:
mode:
authorNicolas Schodet2008-05-02 07:14:41 +0200
committerNicolas Schodet2008-05-02 07:14:41 +0200
commit6980d8540e567a0955dc17d84c9d2e2166550a2b (patch)
treedffd83ab09781574c4c047443b8f3caeed15d84e /digital/io
parentab317d655a86a0051800e4122d051bfe9ab7de28 (diff)
* digital/io/src:
- added arm close timeout.
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/getsamples.fsm5
-rw-r--r--digital/io/src/getsamples_cb.c21
-rw-r--r--digital/io/src/main.c14
-rw-r--r--digital/io/src/main.h8
4 files changed, 48 insertions, 0 deletions
diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm
index 2f495626..0628448b 100644
--- a/digital/io/src/getsamples.fsm
+++ b/digital/io/src/getsamples.fsm
@@ -39,6 +39,8 @@ Events:
the arm has reached the desired position
arm_pass_noted_position
the arm has just passed the 'noted' position
+ wait_finished
+ we have wait the desired time
IDLE:
start -> FACE_DISTRIBUTOR
@@ -70,7 +72,10 @@ TAKE_SAMPLES:
MOVE_AWAY_FROM_DISTRIBUTOR:
bot_move_succeed -> CLOSE_INPUT_HOLE
close input hole
+ setup a close timeout
CLOSE_INPUT_HOLE:
arm_move_succeed -> IDLE
tell the top FSM we have finished
+ wait_finished -> IDLE
+ timed out, give up
diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c
index 3099452f..49eda8d1 100644
--- a/digital/io/src/getsamples_cb.c
+++ b/digital/io/src/getsamples_cb.c
@@ -42,6 +42,11 @@
#define GET_SAMPLES_MOVE_BACKWARD_DISTANCE 7
/**
+ * Arm time out.
+ */
+#define GET_SAMPLES_ARM_TIMEOUT (12 * 225 + 225 / 2)
+
+/**
* 'Private' get samples data used internaly by the FSM.
*/
extern struct getsamples_data_t getsamples_data_;
@@ -130,6 +135,19 @@ getsamples__OPEN_INPUT_HOLE__arm_move_succeed (void)
}
/*
+ * CLOSE_INPUT_HOLE =wait_finished=>
+ * => IDLE
+ * timed out, give up
+ */
+fsm_branch_t
+getsamples__CLOSE_INPUT_HOLE__wait_finished (void)
+{
+ /* Give up, tell the top FSM we have finished */
+ main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished;
+ return getsamples_next (CLOSE_INPUT_HOLE, wait_finished);
+}
+
+/*
* CLOSE_INPUT_HOLE =arm_move_succeed=>
* => IDLE
* tell the top FSM we have finished
@@ -189,12 +207,15 @@ getsamples__IDLE__start (void)
* MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=>
* => CLOSE_INPUT_HOLE
* close input hole
+ * setup a close timeout
*/
fsm_branch_t
getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void)
{
/* Move the arm to close the input hole */
asserv_move_arm (BOT_ARM_MIN_TO_OPEN + BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED);
+ /* Post an event for the top FSM to be waked up later */
+ main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT;
return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_succeed);
}
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 0866642b..3c045368 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -88,6 +88,11 @@ uint8_t main_always_stop_for_obstacle = 1;
uint16_t main_move_wait_cycle;
/**
+ * Get samples timeout.
+ */
+uint16_t main_getsamples_wait_cycle;
+
+/**
* Sharps stats counters.
*/
uint8_t main_stats_sharps, main_stats_sharps_cpt;
@@ -206,6 +211,9 @@ main_loop (void)
/* Update wait flag for move FSM */
if (main_move_wait_cycle)
main_move_wait_cycle--;
+ /* Update wait flag for getsamples FSM */
+ if (main_getsamples_wait_cycle)
+ main_getsamples_wait_cycle--;
/* Update sharp module if required and only every
* MAIN_SHARP_UPDATE_FREQ cycles */
if (++main_sharp_freq_counter_ == MAIN_SHARP_UPDATE_FREQ)
@@ -313,6 +321,12 @@ main_loop (void)
{
FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_wait_finished);
}
+ /* Wait flag for getsamples FSM */
+ if (!main_getsamples_wait_cycle)
+ {
+ FSM_HANDLE_EVENT (&getsamples_fsm,
+ GETSAMPLES_EVENT_wait_finished);
+ }
/* TODO: Check other sensors */
}
diff --git a/digital/io/src/main.h b/digital/io/src/main.h
index 51ab2c9a..1b09a9f8 100644
--- a/digital/io/src/main.h
+++ b/digital/io/src/main.h
@@ -49,6 +49,14 @@ extern uint16_t main_sharp_ignore_event;
extern uint16_t main_move_wait_cycle;
/**
+ * Post an event for the main loop to wake up the getsamples FSM in a certain
+ * count of cycles.
+ * This is used by the getsamples FSM to timeout if the arm is completely
+ * blocked.
+ */
+extern uint16_t main_getsamples_wait_cycle;
+
+/**
* Flag for homologation, to disable the path finding and always stop in front
* of an obstacle and wait.
*/