summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/robospierre/clamp.c45
-rw-r--r--digital/io-hub/src/robospierre/clamp.h4
-rw-r--r--digital/io-hub/src/robospierre/element.c7
-rw-r--r--digital/io-hub/src/robospierre/element.h6
-rw-r--r--digital/io-hub/src/robospierre/logistic.c3
-rw-r--r--digital/io-hub/src/robospierre/logistic.h4
-rw-r--r--digital/io-hub/src/robospierre/top.c54
7 files changed, 107 insertions, 16 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index e3e04cf6..3e7fa9fd 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -99,6 +99,8 @@ FSM_STATES (
FSM_EVENTS (
/* New element inside bottom slot. */
clamp_new_element,
+ /* Order to prepare tower. */
+ clamp_prepare,
/* Sent when clamp is working. */
clamp_working,
/* Sent when clamp return to idle state. */
@@ -243,6 +245,13 @@ clamp_new_element (uint8_t pos, uint8_t element_type)
FSM_HANDLE (AI, clamp_new_element);
}
+void
+clamp_prepare (uint8_t prepare)
+{
+ logistic_global.prepare = 1;
+ FSM_HANDLE (AI, clamp_prepare);
+}
+
uint8_t
clamp_drop (uint8_t drop_direction)
{
@@ -477,6 +486,42 @@ FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING)
return FSM_NEXT (CLAMP_IDLE, clamp_new_element);
}
+FSM_TRANS (CLAMP_IDLE, clamp_prepare,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
+ done, CLAMP_IDLE)
+{
+ logistic_decision ();
+ if (logistic_global.moving_from != CLAMP_SLOT_NB)
+ {
+ clamp_move_element (logistic_global.moving_from,
+ logistic_global.moving_to);
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element);
+ }
+ else if (logistic_global.clamp_pos_idle != ctx.pos_current)
+ {
+ if (logistic_path_clear (ctx.pos_current,
+ logistic_global.clamp_pos_idle))
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_to_idle);
+ }
+ else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, clamp_locked);
+ }
+ }
+ else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return FSM_NEXT (CLAMP_IDLE, clamp_prepare, done);
+ }
+}
+
FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
{
/* If going forward, drop at back. */
diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h
index 60da1615..76dcc34b 100644
--- a/digital/io-hub/src/robospierre/clamp.h
+++ b/digital/io-hub/src/robospierre/clamp.h
@@ -80,6 +80,10 @@ clamp_move_element (uint8_t from, uint8_t to);
void
clamp_new_element (uint8_t pos, uint8_t element_type);
+/** Change logisitic preparation level and update clamp state. */
+void
+clamp_prepare (uint8_t prepare);
+
/** Drop an element tower. Return 0 if not currently possible. If
* drop_direction is forward, drop at the back. */
uint8_t
diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c
index 71f754e0..73b165cc 100644
--- a/digital/io-hub/src/robospierre/element.c
+++ b/digital/io-hub/src/robospierre/element.c
@@ -131,13 +131,6 @@ struct element_t element_table[] =
{ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT} /* right blue */
};
-inline element_t
-element_get (uint8_t element_id)
-{
- assert (element_id < UTILS_COUNT (element_table));
- return element_table[element_id];
-}
-
inline void
element_set (uint8_t element_id, element_t element)
{
diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h
index 3ac1f9ba..bcc40f43 100644
--- a/digital/io-hub/src/robospierre/element.h
+++ b/digital/io-hub/src/robospierre/element.h
@@ -171,4 +171,10 @@ element_nearest_element_id (position_t robot_pos);
vect_t
element_get_pos (uint8_t element_id);
+extern inline element_t
+element_get (uint8_t element_id)
+{
+ return element_table[element_id];
+}
+
#endif /* element_h */
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
index fea121cf..32b9fdb0 100644
--- a/digital/io-hub/src/robospierre/logistic.c
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -365,8 +365,7 @@ logisitic_make_switches ()
D, _, RIGHT, 0);
}
-/** Examine current state and take a decision. */
-static void
+void
logistic_decision (void)
{
/* Reset. */
diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h
index 13c849a5..334b9db4 100644
--- a/digital/io-hub/src/robospierre/logistic.h
+++ b/digital/io-hub/src/robospierre/logistic.h
@@ -97,6 +97,10 @@ extern struct logistic_t logistic_global;
void
logistic_init (void);
+/** Examine current state and take a decision. */
+void
+logistic_decision (void);
+
/** To be called at regular interval to check for bad robot state. */
void
logistic_update (void);
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index b99e3baf..0af8ce80 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -51,7 +51,13 @@ FSM_STATES (
TOP_GOING_TO_DROP,
TOP_GOING_TO_ELEMENT,
/* Waiting clamp has finished its work. */
- TOP_WAITING_CLAMP)
+ TOP_WAITING_CLAMP,
+ /* Waiting construction is ready to drop. */
+ TOP_WAITING_READY,
+ /* Dropping, opening the doors. */
+ TOP_DROP_DROPPING,
+ /* Dropping, clearing so that doors can be closed. */
+ TOP_DROP_CLEARING)
FSM_START_WITH (TOP_START)
@@ -85,6 +91,11 @@ top_go_element (void)
position_t robot_pos;
asserv_get_position (&robot_pos);
ctx.target_element_id = element_best (robot_pos);
+ element_t e = element_get (ctx.target_element_id);
+ if (e.attr & ELEMENT_GREEN)
+ logistic_global.prepare = 0;
+ else
+ logistic_global.prepare = 1;
vect_t element_pos = element_get_pos (ctx.target_element_id);
uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
? 0 : ASSERV_BACKWARD;
@@ -130,14 +141,18 @@ FSM_TRANS (TOP_GOING_OUT2, robot_move_success,
}
FSM_TRANS (TOP_GOING_TO_DROP, move_success,
- drop, TOP_GOING_TO_DROP,
- element, TOP_GOING_TO_ELEMENT)
+ ready, TOP_DROP_DROPPING,
+ wait_clamp, TOP_WAITING_READY)
{
- clamp_drop (logistic_global.collect_direction);
- switch (top_decision ())
+ if (logistic_global.ready)
+ {
+ clamp_drop (logistic_global.collect_direction);
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready);
+ }
+ else
{
- default: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, drop);
- case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, element);
+ clamp_prepare (1);
+ return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
}
}
@@ -166,3 +181,28 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element);
}
}
+
+FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING)
+{
+ clamp_drop (logistic_global.collect_direction);
+ return FSM_NEXT (TOP_WAITING_READY, clamp_done);
+}
+
+FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING)
+{
+ asserv_move_linearly (200);
+ return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting);
+}
+
+FSM_TRANS (TOP_DROP_CLEARING, robot_move_success,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ clamp_drop_clear ();
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop);
+ case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, element);
+ }
+}
+