summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_loader_cb.c24
-rw-r--r--digital/io/src/loader.fsm11
-rw-r--r--digital/io/src/main.c4
3 files changed, 30 insertions, 9 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index fa85c4e7..46e9b60c 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -252,7 +252,7 @@ ai__LOADER_LOAD_CLOSING__clamp_succeed (void)
- mimot_get_motor1_position ();
if (tickness > BOT_CLAMP_EMPTY_STEP)
{
- asserv_move_motor0_absolute (BOT_ELEVATOR_UNLOAD_STEP,
+ asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
BOT_ELEVATOR_SPEED);
return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full);
}
@@ -265,28 +265,38 @@ ai__LOADER_LOAD_CLOSING__clamp_succeed (void)
}
/*
- * LOADER_LOAD_UPING =elevator_succeed=>
+ * LOADER_LOAD_UPING =elevator_unload_position=>
* => LOADER_LOAD_UNLOADING
* open clamp
*/
fsm_branch_t
-ai__LOADER_LOAD_UPING__elevator_succeed (void)
+ai__LOADER_LOAD_UPING__elevator_unload_position (void)
{
mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
mimot_move_motor1_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
- return ai_next (LOADER_LOAD_UPING, elevator_succeed);
+ return ai_next (LOADER_LOAD_UPING, elevator_unload_position);
}
/*
- * LOADER_LOAD_UNLOADING =clamp_succeed=>
+ * LOADER_LOAD_UNLOADING =elevator_succeed=>
+ * => LOADER_LOAD_UNLOADING_OPEN
+ */
+fsm_branch_t
+ai__LOADER_LOAD_UNLOADING__elevator_succeed (void)
+{
+ return ai_next (LOADER_LOAD_UNLOADING, elevator_succeed);
+}
+
+/*
+ * LOADER_LOAD_UNLOADING_OPEN =clamp_succeed=>
* => LOADER_DOWNING
* move down
*/
fsm_branch_t
-ai__LOADER_LOAD_UNLOADING__clamp_succeed (void)
+ai__LOADER_LOAD_UNLOADING_OPEN__clamp_succeed (void)
{
asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_LOAD_UNLOADING, clamp_succeed);
+ return ai_next (LOADER_LOAD_UNLOADING_OPEN, clamp_succeed);
}
/*
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index 4ffafd26..c97941b9 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -32,7 +32,9 @@ States:
LOADER_LOAD_UPING
move load up to the unload position
LOADER_LOAD_UNLOADING
- open clamp to unload
+ open clamp to unload, wait until unload position
+ LOADER_LOAD_UNLOADING_OPEN
+ check clamp is open
LOADER_LOAD_EMPTY_OPEN
open clamp due to no elements to pick
@@ -41,6 +43,8 @@ Events:
asserv success result
elevator_failed
asserv failure result
+ elevator_unload_position
+ elevator above unloading position
clamp_succeed
asserv success result, clamp movement cannot fail
loader_element
@@ -118,10 +122,13 @@ LOADER_LOAD_CLOSING:
open clamp
LOADER_LOAD_UPING:
- elevator_succeed -> LOADER_LOAD_UNLOADING
+ elevator_unload_position -> LOADER_LOAD_UNLOADING
open clamp
LOADER_LOAD_UNLOADING:
+ elevator_succeed -> LOADER_LOAD_UNLOADING_OPEN
+
+LOADER_LOAD_UNLOADING_OPEN:
clamp_succeed -> LOADER_DOWNING
move down
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 8ad7c533..fdd98355 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -153,6 +153,10 @@ main_event_to_fsm (void)
if (motorm0_status == success && motorm1_status == success)
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed);
+ /* Check positions. */
+ if (asserv_get_motor0_position () > BOT_ELEVATOR_UNLOAD_STEP)
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_elevator_unload_position);
+
/* Contacts. */
if (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1))
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element);