summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-10 23:54:29 +0200
committerNicolas Schodet2010-05-10 23:54:29 +0200
commit5c206451fabc49b614b09f416b373288f3cfe39d (patch)
tree36f9eb5decc017af0358d233d58e51a66aa0401d /digital
parentf12ffb8014ba12e09b09e005c664a625bb5fe6c5 (diff)
digital/io/src: handle empty clamp
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/ai_loader_cb.c35
-rw-r--r--digital/io/src/bot.h6
-rw-r--r--digital/io/src/loader.fsm11
3 files changed, 45 insertions, 7 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index a02f8b22..fa85c4e7 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -154,7 +154,7 @@ ai__LOADER_DOWN__loader_up (void)
/*
* LOADER_DOWN =loader_element=>
* => LOADER_LOAD_CLOSING
- * close clamp
+ * clamp
*/
fsm_branch_t
ai__LOADER_DOWN__loader_element (void)
@@ -238,15 +238,30 @@ ai__LOADER_ERROR__loader_up (void)
/*
* LOADER_LOAD_CLOSING =clamp_succeed=>
- * => LOADER_LOAD_UPING
+ * full => LOADER_LOAD_UPING
* move up
+ * empty => LOADER_LOAD_EMPTY_OPEN
+ * open clamp
*/
fsm_branch_t
ai__LOADER_LOAD_CLOSING__clamp_succeed (void)
{
- asserv_move_motor0_absolute (BOT_ELEVATOR_UNLOAD_STEP,
- BOT_ELEVATOR_SPEED);
- return ai_next (LOADER_LOAD_CLOSING, clamp_succeed);
+ /* Measure load using clamp position. */
+ uint16_t tickness = BOT_CLAMP_WIDTH_STEP
+ - mimot_get_motor0_position ()
+ - mimot_get_motor1_position ();
+ if (tickness > BOT_CLAMP_EMPTY_STEP)
+ {
+ asserv_move_motor0_absolute (BOT_ELEVATOR_UNLOAD_STEP,
+ BOT_ELEVATOR_SPEED);
+ return ai_next_branch (LOADER_LOAD_CLOSING, clamp_succeed, full);
+ }
+ else
+ {
+ 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_branch (LOADER_LOAD_CLOSING, clamp_succeed, empty);
+ }
}
/*
@@ -274,3 +289,13 @@ ai__LOADER_LOAD_UNLOADING__clamp_succeed (void)
return ai_next (LOADER_LOAD_UNLOADING, clamp_succeed);
}
+/*
+ * LOADER_LOAD_EMPTY_OPEN =clamp_succeed=>
+ * => LOADER_DOWN
+ */
+fsm_branch_t
+ai__LOADER_LOAD_EMPTY_OPEN__clamp_succeed (void)
+{
+ return ai_next (LOADER_LOAD_EMPTY_OPEN, clamp_succeed);
+}
+
diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h
index f4dfc00a..fef715de 100644
--- a/digital/io/src/bot.h
+++ b/digital/io/src/bot.h
@@ -83,6 +83,12 @@
/** Clamp open position in steps. */
#define BOT_CLAMP_OPEN_STEP 15
+/** Clamp width in steps. */
+#define BOT_CLAMP_WIDTH_STEP 4604
+
+/** Distance under which the clamp is considered empty. */
+#define BOT_CLAMP_EMPTY_STEP 800
+
/** Clamp work speed. */
#define BOT_CLAMP_SPEED 0x60
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index 747bb51e..4ffafd26 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -33,6 +33,8 @@ States:
move load up to the unload position
LOADER_LOAD_UNLOADING
open clamp to unload
+ LOADER_LOAD_EMPTY_OPEN
+ open clamp due to no elements to pick
Events:
elevator_succeed
@@ -89,7 +91,7 @@ LOADER_DOWN:
loader_up -> LOADER_UPING
move up
loader_element -> LOADER_LOAD_CLOSING
- close clamp
+ clamp
LOADER_ERROR:
loader_down -> LOADER_DOWNING
@@ -110,8 +112,10 @@ LOADER_UPING:
post loader_errored event
LOADER_LOAD_CLOSING:
- clamp_succeed -> LOADER_LOAD_UPING
+ clamp_succeed: full -> LOADER_LOAD_UPING
move up
+ clamp_succeed: empty -> LOADER_LOAD_EMPTY_OPEN
+ open clamp
LOADER_LOAD_UPING:
elevator_succeed -> LOADER_LOAD_UNLOADING
@@ -121,3 +125,6 @@ LOADER_LOAD_UNLOADING:
clamp_succeed -> LOADER_DOWNING
move down
+LOADER_LOAD_EMPTY_OPEN:
+ clamp_succeed -> LOADER_DOWN
+