summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_loader_cb.c5
-rw-r--r--digital/io/src/loader.fsm1
2 files changed, 6 insertions, 0 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index e4461948..9b4680a8 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -518,13 +518,18 @@ ai__LOADER_LOAD_UPING__elevator_unload_position (void)
* LOADER_LOAD_UPING =elevator_failed=>
* => LOADER_ERROR
* post loader_errored or loader_black event
+ * remove one element
* open clamp
*/
fsm_branch_t
ai__LOADER_LOAD_UPING__elevator_failed (void)
{
if (asserv_get_motor0_position () < BOT_ELEVATOR_BLACK_THRESHOLD_STEP)
+ {
+ if (loader_elements)
+ loader_elements--;
main_post_event (AI_EVENT_loader_black);
+ }
else
main_post_event (AI_EVENT_loader_errored);
mimot_move_motor0_absolute (BOT_CLAMP_OPEN_STEP, BOT_CLAMP_SPEED);
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index f2ee3e44..322c99f0 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -200,6 +200,7 @@ LOADER_LOAD_UPING:
open clamp
elevator_failed -> LOADER_ERROR
post loader_errored or loader_black event
+ remove one element
open clamp
LOADER_LOAD_UNLOADING: