summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-04 02:25:27 +0200
committerNicolas Schodet2010-05-04 02:25:27 +0200
commit6efb5213ba0113e92f168156c80cd748aea53745 (patch)
tree0018d92b91f3c1c7e72364899882e185788abd35
parentfd1c46e3bbb4a98b7a263c051bd8bda9797c508f (diff)
digital/io/src: first element pickup
-rw-r--r--digital/io/src/ai_loader_cb.c51
-rw-r--r--digital/io/src/bot.h6
-rw-r--r--digital/io/src/contact.c3
-rw-r--r--digital/io/src/contact.h3
-rw-r--r--digital/io/src/loader.fsm22
-rw-r--r--digital/io/src/main.c4
6 files changed, 87 insertions, 2 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index 10e32211..f759b417 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -152,6 +152,19 @@ ai__LOADER_DOWN__loader_up (void)
}
/*
+ * LOADER_DOWN =loader_element=>
+ * => LOADER_LOAD_CLOSING
+ * close clamp
+ */
+fsm_branch_t
+ai__LOADER_DOWN__loader_element (void)
+{
+ mimot_move_motor0_absolute (BOT_CLAMP_STROKE_STEP, BOT_CLAMP_SPEED);
+ mimot_move_motor1_absolute (BOT_CLAMP_STROKE_STEP, BOT_CLAMP_SPEED);
+ return ai_next (LOADER_DOWN, loader_element);
+}
+
+/*
* LOADER_UPING =elevator_succeed=>
* => LOADER_UP
* post loader_uped event
@@ -223,3 +236,41 @@ ai__LOADER_ERROR__loader_up (void)
return ai_next (LOADER_ERROR, loader_up);
}
+/*
+ * LOADER_LOAD_CLOSING =clamp_succeed=>
+ * => LOADER_LOAD_UPING
+ * move up
+ */
+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);
+}
+
+/*
+ * LOADER_LOAD_UPING =elevator_succeed=>
+ * => LOADER_LOAD_UNLOADING
+ * open clamp
+ */
+fsm_branch_t
+ai__LOADER_LOAD_UPING__elevator_succeed (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);
+}
+
+/*
+ * LOADER_LOAD_UNLOADING =clamp_succeed=>
+ * => LOADER_DOWNING
+ * move down
+ */
+fsm_branch_t
+ai__LOADER_LOAD_UNLOADING__clamp_succeed (void)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return ai_next (LOADER_LOAD_UNLOADING, clamp_succeed);
+}
+
diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h
index 39467252..e5f280bd 100644
--- a/digital/io/src/bot.h
+++ b/digital/io/src/bot.h
@@ -62,6 +62,9 @@
/** Elevator stroke in steps. */
#define BOT_ELEVATOR_STROKE_STEP 7089
+/** Elevator unloading up position in steps. */
+#define BOT_ELEVATOR_UNLOAD_STEP 5600
+
/** Elevator resting up position in steps. */
#define BOT_ELEVATOR_REST_STEP 4111
@@ -77,6 +80,9 @@
/** Clamp stroke in steps. */
#define BOT_CLAMP_STROKE_STEP 5627
+/** Clamp open position in steps. */
+#define BOT_CLAMP_OPEN_STEP 30
+
/** Clamp work speed. */
#define BOT_CLAMP_SPEED 0x60
diff --git a/digital/io/src/contact.c b/digital/io/src/contact.c
index 4483fef3..8bcb8910 100644
--- a/digital/io/src/contact.c
+++ b/digital/io/src/contact.c
@@ -30,5 +30,6 @@ void
contact_init (void)
{
/* Pull-up. */
- //IO_SET (CONTACT_EXAMPLE);
+ IO_SET (CONTACT_BUMPER0);
+ IO_SET (CONTACT_BUMPER1);
}
diff --git a/digital/io/src/contact.h b/digital/io/src/contact.h
index e50a0833..4fcfa05c 100644
--- a/digital/io/src/contact.h
+++ b/digital/io/src/contact.h
@@ -26,7 +26,8 @@
* }}} */
#include "simu.host.h"
-#define CONTACT_EXAMPLE C, 0
+#define CONTACT_BUMPER0 C, 0
+#define CONTACT_BUMPER1 C, 1
/**
* Initialize the contact module.
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index 05918468..747bb51e 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -27,6 +27,12 @@ States:
moving to the down position
LOADER_ERROR
error while moving up, stop in an unknown state
+ LOADER_LOAD_CLOSING
+ close clamp
+ LOADER_LOAD_UPING
+ move load up to the unload position
+ LOADER_LOAD_UNLOADING
+ open clamp to unload
Events:
elevator_succeed
@@ -35,6 +41,8 @@ Events:
asserv failure result
clamp_succeed
asserv success result, clamp movement cannot fail
+ loader_element
+ element sensed between clamp to be loaded
loader_down
make the loader ready to load an element
loader_up
@@ -80,6 +88,8 @@ LOADER_UP:
LOADER_DOWN:
loader_up -> LOADER_UPING
move up
+ loader_element -> LOADER_LOAD_CLOSING
+ close clamp
LOADER_ERROR:
loader_down -> LOADER_DOWNING
@@ -99,3 +109,15 @@ LOADER_UPING:
elevator_failed -> LOADER_ERROR
post loader_errored event
+LOADER_LOAD_CLOSING:
+ clamp_succeed -> LOADER_LOAD_UPING
+ move up
+
+LOADER_LOAD_UPING:
+ elevator_succeed -> LOADER_LOAD_UNLOADING
+ open clamp
+
+LOADER_LOAD_UNLOADING:
+ clamp_succeed -> LOADER_DOWNING
+ move down
+
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 21bfdff8..3a5e5901 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -152,6 +152,10 @@ main_event_to_fsm (void)
if (motorm0_status == success && motorm1_status == success)
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_clamp_succeed);
+ /* Contacts. */
+ if (!IO_GET (CONTACT_BUMPER0) || !IO_GET (CONTACT_BUMPER1))
+ FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_loader_element);
+
/* Jack */
if (switch_get_jack ())
FSM_HANDLE_EVENT (&ai_fsm, AI_EVENT_jack_removed_from_bot);