summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-02 11:06:14 +0200
committerNicolas Schodet2011-06-02 13:39:30 +0200
commit63407d7ed7b959e883c67107b3a80e859144e01d (patch)
tree70eaa60a1779b7a4a1d82e7059c55d5fdce4164e
parenta7015e8589617e8c0d908bc34c5a7d12b829074c (diff)
digital/io-hub: handle clamp blocking
-rw-r--r--digital/io-hub/src/robospierre/clamp.c101
-rw-r--r--digital/io-hub/src/robospierre/logistic.c8
-rw-r--r--digital/io-hub/src/robospierre/logistic.h4
-rw-r--r--digital/io-hub/src/robospierre/main.c3
-rw-r--r--digital/io-hub/src/robospierre/top.c65
5 files changed, 169 insertions, 12 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 812d4890..e05f79d7 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -78,6 +78,8 @@ FSM_STATES (
CLAMP_DROPING_WAITING_ROBOT,
/* Clamp locked in a bay. */
CLAMP_LOCKED,
+ /* Clamp blocked. */
+ CLAMP_BLOCKED,
/* Waiting movement order. */
CLAMP_MOVE_IDLE,
@@ -105,6 +107,8 @@ FSM_EVENTS (
clamp_working,
/* Sent when clamp return to idle state. */
clamp_done,
+ /* Sent when clamp is blocked. */
+ clamp_blocked,
/* Order to drop elements. */
clamp_drop,
/* Sent once drop is done, but robot should advance to completely
@@ -116,8 +120,12 @@ FSM_EVENTS (
clamp_move,
/* Clamp movement success. */
clamp_move_success,
+ /* Clamp movement failure. */
+ clamp_move_failure,
/* Elevation and elevation motor success. */
clamp_elevation_rotation_success,
+ /* Elevation or elevation motor failure. */
+ clamp_elevation_or_rotation_failure,
/* Elevation motor success. */
clamp_elevation_success,
/* Elevation motor failure. */
@@ -248,7 +256,7 @@ clamp_new_element (uint8_t pos, uint8_t element_type)
void
clamp_prepare (uint8_t prepare)
{
- logistic_global.prepare = 1;
+ logistic_global.prepare = prepare;
FSM_HANDLE (AI, clamp_prepare);
}
@@ -409,6 +417,22 @@ clamp_route (void)
ctx.pos_current = pos_new;
}
+static void
+clamp_blocked (void)
+{
+ /* Free everything. */
+ clamp_openclose (1);
+ clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1);
+ clamp_door (CLAMP_SLOT_FRONT_TOP, 1);
+ clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1);
+ clamp_door (CLAMP_SLOT_BACK_TOP, 1);
+ mimot_motor0_free ();
+ mimot_motor1_free ();
+ logistic_dump ();
+ /* Signal problem. */
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure));
+}
+
/* CLAMP FSM */
FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING)
@@ -488,6 +512,12 @@ FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE)
return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success);
}
+FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_failure, CLAMP_BLOCKED)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_failure);
+}
+
FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING)
{
ctx.working = 1;
@@ -510,7 +540,8 @@ FSM_TRANS (CLAMP_IDLE, clamp_prepare,
logistic_global.moving_to);
return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element);
}
- else if (logistic_global.clamp_pos_idle != ctx.pos_current)
+ else if (logistic_global.prepare != 3
+ && logistic_global.clamp_pos_idle != ctx.pos_current)
{
if (logistic_path_clear (ctx.pos_current,
logistic_global.clamp_pos_idle))
@@ -556,7 +587,8 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
logistic_global.moving_to);
return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element);
}
- else if (logistic_global.clamp_pos_idle != ctx.pos_current)
+ else if (logistic_global.prepare != 3
+ && logistic_global.clamp_pos_idle != ctx.pos_current)
{
if (logistic_path_clear (ctx.pos_current,
logistic_global.clamp_pos_idle))
@@ -593,7 +625,8 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
move_element);
}
- else if (logistic_global.clamp_pos_idle != ctx.pos_current)
+ else if (logistic_global.prepare != 3
+ && logistic_global.clamp_pos_idle != ctx.pos_current)
{
if (logistic_path_clear (ctx.pos_current,
logistic_global.clamp_pos_idle))
@@ -619,6 +652,12 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
}
}
+FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_failure, CLAMP_BLOCKED)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked));
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_failure);
+}
+
FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
CLAMP_DROPING_WAITING_ROBOT)
{
@@ -640,7 +679,8 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
move_element);
}
- else if (logistic_global.clamp_pos_idle != ctx.pos_current)
+ else if (logistic_global.prepare != 3
+ && logistic_global.clamp_pos_idle != ctx.pos_current)
{
if (logistic_path_clear (ctx.pos_current,
logistic_global.clamp_pos_idle))
@@ -684,6 +724,34 @@ FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
return FSM_NEXT (CLAMP_LOCKED, clamp_drop);
}
+FSM_TRANS (CLAMP_BLOCKED, clamp_prepare,
+ move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
+ done, CLAMP_IDLE)
+{
+ if (logistic_global.prepare != 3)
+ {
+ if (logistic_path_clear (ctx.pos_current,
+ logistic_global.clamp_pos_idle))
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle);
+ }
+ else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked);
+ }
+ }
+ else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, done);
+ }
+}
+
/* CLAMP_MOVE FSM */
FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move,
@@ -729,6 +797,13 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
}
}
+FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure,
+ CLAMP_MOVE_IDLE)
+{
+ clamp_blocked ();
+ return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure);
+}
+
FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
done, CLAMP_MOVE_SRC_CLAMP_CLOSING,
next, CLAMP_MOVE_SRC_ROUTING)
@@ -747,6 +822,14 @@ FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
}
}
+FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_or_rotation_failure,
+ CLAMP_MOVE_IDLE)
+{
+ clamp_blocked ();
+ return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
+ clamp_elevation_or_rotation_failure);
+}
+
FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME,
open_door, CLAMP_MOVE_SRC_DOOR_OPENDING,
move, CLAMP_MOVE_DST_ROUTING)
@@ -804,6 +887,14 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success,
}
}
+FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_or_rotation_failure,
+ CLAMP_MOVE_IDLE)
+{
+ clamp_blocked ();
+ return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
+ clamp_elevation_or_rotation_failure);
+}
+
FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
CLAMP_MOVE_DST_CLAMP_OPENING)
{
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
index 4b0536f2..afa625bc 100644
--- a/digital/io-hub/src/robospierre/logistic.c
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -518,6 +518,14 @@ logistic_drop (uint8_t direction)
logistic_decision ();
}
+void
+logistic_dump (void)
+{
+ uint8_t i;
+ for (i = 0; i < CLAMP_SLOT_NB; i++)
+ ctx.slots[i] = 0;
+}
+
static uint8_t
logistic_slot_clear (uint8_t slot)
{
diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h
index 9962874b..6acb681e 100644
--- a/digital/io-hub/src/robospierre/logistic.h
+++ b/digital/io-hub/src/robospierre/logistic.h
@@ -124,6 +124,10 @@ logistic_element_move_done (void);
void
logistic_drop (uint8_t direction);
+/** Dump every element. */
+void
+logistic_dump (void);
+
/** Is path clear between two positions? */
uint8_t
logistic_path_clear (uint8_t slot1, uint8_t slot2);
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index 6b2c8d31..1e12c427 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -144,6 +144,9 @@ main_event_to_fsm (void)
if (mimot_motor0_status == success
&& mimot_motor1_status == success)
FSM_HANDLE_E (AI, clamp_elevation_rotation_success);
+ if (mimot_motor0_status == failure
+ || mimot_motor1_status == failure)
+ FSM_HANDLE_E (AI, clamp_elevation_or_rotation_failure);
if (mimot_motor0_status == success)
FSM_HANDLE_E (AI, clamp_elevation_success);
else if (mimot_motor0_status == failure)
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index c9362203..8ff69ee6 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -56,6 +56,10 @@ FSM_STATES (
TOP_GOING_TO_ELEMENT,
/* Waiting clamp has finished its work. */
TOP_WAITING_CLAMP,
+ /* Unblocking: waiting a little bit. */
+ TOP_UNBLOCKING_SHAKE_WAIT,
+ /* Unblocking: shaking. */
+ TOP_UNBLOCKING_SHAKE,
/* Waiting construction is ready to drop. */
TOP_WAITING_READY,
/* Dropping, opening the doors. */
@@ -72,6 +76,8 @@ struct top_t
uint8_t target_element_id;
/** Chaos counter. */
uint8_t chaos;
+ /** Broken clamp. */
+ uint8_t broken;
};
/** Global context. */
@@ -111,10 +117,13 @@ top_go_element (void)
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;
+ if (!ctx.broken)
+ {
+ 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;
@@ -154,11 +163,12 @@ static uint8_t
top_decision (void)
{
/* If we can make a tower. */
- if (logistic_global.construct_possible == 1)
+ if (logistic_global.construct_possible == 1
+ || (ctx.broken && logistic_global.construct_possible == 2))
return top_go_drop ();
if (logistic_global.need_prepare)
{
- clamp_prepare (2);
+ clamp_prepare (ctx.broken ? 3 : 2);
return top_go_drop ();
}
else
@@ -217,7 +227,7 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success,
}
else
{
- clamp_prepare (1);
+ clamp_prepare (ctx.broken ? 3 : 1);
return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
}
}
@@ -292,12 +302,53 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
}
}
+FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT)
+{
+ return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked);
+}
+
+FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250,
+ try_again, TOP_UNBLOCKING_SHAKE,
+ tryout, TOP_WAITING_CLAMP)
+{
+ if (ctx.chaos < 4)
+ {
+ int16_t dist = logistic_global.collect_direction
+ == DIRECTION_FORWARD ? 100 : -100;
+ asserv_move_linearly (++ctx.chaos % 2 ? -dist : dist);
+ return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, try_again);
+ }
+ else
+ {
+ ctx.broken = 1;
+ clamp_prepare (3);
+ return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, tryout);
+ }
+}
+
+FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_success, TOP_WAITING_CLAMP)
+{
+ clamp_prepare (0);
+ return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_success);
+}
+
+FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_failure,
+ TOP_UNBLOCKING_SHAKE_WAIT)
+{
+ return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_failure);
+}
+
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_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT)
+{
+ return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked);
+}
+
FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING)
{
element_down (ctx.target_element_id, ELEMENT_TOWER);