summaryrefslogtreecommitdiffhomepage
path: root/digital/io/src
diff options
context:
space:
mode:
authorNicolas Schodet2010-05-04 02:25:16 +0200
committerNicolas Schodet2010-05-04 02:25:16 +0200
commit7c593a7f73a4d56a7eb1e6727a2f5b2007133a27 (patch)
treebf4b3be32bc0486d87f6841d46a4b5bdcb549783 /digital/io/src
parentfd9ce0cc3d9b24ec7661876d2a72c37a808b9eab (diff)
digital/io/src: add up and down clamp positions
Diffstat (limited to 'digital/io/src')
-rw-r--r--digital/io/src/ai_loader_cb.c101
-rw-r--r--digital/io/src/ai_top_cb.c4
-rw-r--r--digital/io/src/bot.h6
-rw-r--r--digital/io/src/loader.c14
-rw-r--r--digital/io/src/loader.fsm48
-rw-r--r--digital/io/src/loader.h8
6 files changed, 177 insertions, 4 deletions
diff --git a/digital/io/src/ai_loader_cb.c b/digital/io/src/ai_loader_cb.c
index 1d4f816e..10e32211 100644
--- a/digital/io/src/ai_loader_cb.c
+++ b/digital/io/src/ai_loader_cb.c
@@ -28,6 +28,7 @@
#include "asserv.h"
#include "mimot.h"
#include "bot.h"
+#include "main.h"
/*
* LOADER_IDLE =start=>
@@ -85,7 +86,7 @@ ai__LOADER_INIT_ELEVATOR_ZERO__elevator_succeed (void)
fsm_branch_t
ai__LOADER_INIT_CLAMP_CLOSE__clamp_succeed (void)
{
- asserv_move_motor0_absolute (BOT_ELEVATOR_STROKE_STEP,
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP,
BOT_ELEVATOR_SPEED);
mimot_motor0_zero_position (-BOT_CLAMP_ZERO_SPEED);
mimot_motor1_zero_position (-BOT_CLAMP_ZERO_SPEED);
@@ -104,7 +105,7 @@ ai__LOADER_INIT_CLAMP_ZERO__clamp_succeed (void)
/*
* LOADER_INIT_ELEVATOR_UP =elevator_succeed=>
- * => LOADER_IDLE
+ * => LOADER_UP
*/
fsm_branch_t
ai__LOADER_INIT_ELEVATOR_UP__elevator_succeed (void)
@@ -126,3 +127,99 @@ ai__LOADER_INIT_ELEVATOR_UP__elevator_failed (void)
return ai_next (LOADER_INIT_ELEVATOR_UP, elevator_failed);
}
+/*
+ * LOADER_UP =loader_down=>
+ * => LOADER_DOWNING
+ * move down
+ */
+fsm_branch_t
+ai__LOADER_UP__loader_down (void)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return ai_next (LOADER_UP, loader_down);
+}
+
+/*
+ * LOADER_DOWN =loader_up=>
+ * => LOADER_UPING
+ * move up
+ */
+fsm_branch_t
+ai__LOADER_DOWN__loader_up (void)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return ai_next (LOADER_DOWN, loader_up);
+}
+
+/*
+ * LOADER_UPING =elevator_succeed=>
+ * => LOADER_UP
+ * post loader_uped event
+ */
+fsm_branch_t
+ai__LOADER_UPING__elevator_succeed (void)
+{
+ main_post_event_for_top_fsm = AI_EVENT_loader_uped;
+ return ai_next (LOADER_UPING, elevator_succeed);
+}
+
+/*
+ * LOADER_UPING =elevator_failed=>
+ * => LOADER_ERROR
+ * post loader_errored event
+ */
+fsm_branch_t
+ai__LOADER_UPING__elevator_failed (void)
+{
+ main_post_event_for_top_fsm = AI_EVENT_loader_errored;
+ return ai_next (LOADER_UPING, elevator_failed);
+}
+
+/*
+ * LOADER_DOWNING =elevator_succeed=>
+ * => LOADER_DOWN
+ * post loader_downed event
+ */
+fsm_branch_t
+ai__LOADER_DOWNING__elevator_succeed (void)
+{
+ main_post_event_for_top_fsm = AI_EVENT_loader_downed;
+ return ai_next (LOADER_DOWNING, elevator_succeed);
+}
+
+/*
+ * LOADER_DOWNING =elevator_failed=>
+ * => LOADER_UPING
+ * something is blocking, move it up
+ */
+fsm_branch_t
+ai__LOADER_DOWNING__elevator_failed (void)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return ai_next (LOADER_DOWNING, elevator_failed);
+}
+
+/*
+ * LOADER_ERROR =loader_down=>
+ * => LOADER_DOWNING
+ * move down
+ */
+fsm_branch_t
+ai__LOADER_ERROR__loader_down (void)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_DOWN_STEP, BOT_ELEVATOR_SPEED);
+ return ai_next (LOADER_ERROR, loader_down);
+}
+
+/*
+ * LOADER_ERROR =loader_up=>
+ * => LOADER_UPING
+ * move up
+ */
+fsm_branch_t
+ai__LOADER_ERROR__loader_up (void)
+{
+ asserv_move_motor0_absolute (BOT_ELEVATOR_REST_STEP, BOT_ELEVATOR_SPEED);
+ return ai_next (LOADER_ERROR, loader_up);
+}
+
diff --git a/digital/io/src/ai_top_cb.c b/digital/io/src/ai_top_cb.c
index 882fb84f..4f349ead 100644
--- a/digital/io/src/ai_top_cb.c
+++ b/digital/io/src/ai_top_cb.c
@@ -31,7 +31,7 @@
#include "move.h"
#include "chrono.h"
#include "playground.h"
-#include "asserv.h"
+#include "loader.h"
/**
* Time limit.
@@ -73,6 +73,7 @@ ai__GO_FAR__move_fsm_succeed (void)
{
position_t pos = PG_POSITION_DEG (2625, 253, -90);
move_start (pos, 0);
+ loader_down ();
return ai_next (GO_FAR, move_fsm_succeed);
}
@@ -98,6 +99,7 @@ ai__GO_NEAR__move_fsm_succeed (void)
{
position_t pos = PG_POSITION_DEG (825, 1253, -29);
move_start (pos, 0);
+ loader_up ();
return ai_next (GO_NEAR, move_fsm_succeed);
}
diff --git a/digital/io/src/bot.h b/digital/io/src/bot.h
index aa889970..39467252 100644
--- a/digital/io/src/bot.h
+++ b/digital/io/src/bot.h
@@ -62,6 +62,12 @@
/** Elevator stroke in steps. */
#define BOT_ELEVATOR_STROKE_STEP 7089
+/** Elevator resting up position in steps. */
+#define BOT_ELEVATOR_REST_STEP 4111
+
+/** Elevator down position in steps. */
+#define BOT_ELEVATOR_DOWN_STEP 30
+
/** Elevator work speed. */
#define BOT_ELEVATOR_SPEED 0x60
diff --git a/digital/io/src/loader.c b/digital/io/src/loader.c
index bf297161..019ec41b 100644
--- a/digital/io/src/loader.c
+++ b/digital/io/src/loader.c
@@ -25,3 +25,17 @@
#include "common.h"
#include "loader.h"
+#include "fsm.h"
+
+void
+loader_up (void)
+{
+ fsm_handle_event (&ai_fsm, AI_EVENT_loader_up);
+}
+
+void
+loader_down (void)
+{
+ fsm_handle_event (&ai_fsm, AI_EVENT_loader_down);
+}
+
diff --git a/digital/io/src/loader.fsm b/digital/io/src/loader.fsm
index 554ff08a..05918468 100644
--- a/digital/io/src/loader.fsm
+++ b/digital/io/src/loader.fsm
@@ -17,6 +17,16 @@ States:
find clamp zero position
LOADER_INIT_ELEVATOR_UP
move the elevator up so that the robot can go to the wall
+ LOADER_UP
+ up position, clamp is protected
+ LOADER_DOWN
+ down position, ready to take an element
+ LOADER_UPING
+ moving to the up position
+ LOADER_DOWNING
+ moving to the down position
+ LOADER_ERROR
+ error while moving up, stop in an unknown state
Events:
elevator_succeed
@@ -25,6 +35,16 @@ Events:
asserv failure result
clamp_succeed
asserv success result, clamp movement cannot fail
+ loader_down
+ make the loader ready to load an element
+ loader_up
+ protect the clamp by moving it up
+ loader_downed
+ posted when loader successfully moved down
+ loader_uped
+ posted when loader successfully moved up
+ loader_errored
+ posted when loader in an unknown state due to error while moving up
LOADER_IDLE:
start -> LOADER_WAIT_JACK_IN
@@ -49,7 +69,33 @@ LOADER_INIT_CLAMP_ZERO:
clamp_succeed -> LOADER_INIT_ELEVATOR_UP
LOADER_INIT_ELEVATOR_UP:
- elevator_succeed -> LOADER_IDLE
+ elevator_succeed -> LOADER_UP
elevator_failed -> LOADER_IDLE
initialisation failure
+LOADER_UP:
+ loader_down -> LOADER_DOWNING
+ move down
+
+LOADER_DOWN:
+ loader_up -> LOADER_UPING
+ move up
+
+LOADER_ERROR:
+ loader_down -> LOADER_DOWNING
+ move down
+ loader_up -> LOADER_UPING
+ move up
+
+LOADER_DOWNING:
+ elevator_succeed -> LOADER_DOWN
+ post loader_downed event
+ elevator_failed -> LOADER_UPING
+ something is blocking, move it up
+
+LOADER_UPING:
+ elevator_succeed -> LOADER_UP
+ post loader_uped event
+ elevator_failed -> LOADER_ERROR
+ post loader_errored event
+
diff --git a/digital/io/src/loader.h b/digital/io/src/loader.h
index 897763ab..6ca7459f 100644
--- a/digital/io/src/loader.h
+++ b/digital/io/src/loader.h
@@ -25,4 +25,12 @@
*
* }}} */
+/** Move loader up, in a rest position. */
+void
+loader_up (void);
+
+/** Move loader down, ready to take elements. */
+void
+loader_down (void);
+
#endif /* loader_h */