summaryrefslogtreecommitdiff
path: root/digital/io-hub
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-22 20:42:57 +0200
committerNicolas Schodet2011-05-22 20:53:13 +0200
commit4232368383074bfa9fcb98d96b6a20f0a4ef2736 (patch)
tree9db180ecaeeda6033ac9d8285bc02f56d9516cf4 /digital/io-hub
parent27700ff4b67d9e8351da875c6f4d58a8f1311e5d (diff)
digital/io-hub: handle clamp locking
When the clamp is in a bay with an element at the middle level, it can no longer move.
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/robospierre/clamp.c51
-rw-r--r--digital/io-hub/src/robospierre/logistic.c18
-rw-r--r--digital/io-hub/src/robospierre/logistic.h4
3 files changed, 65 insertions, 8 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 50fcf041..70834bfe 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -64,6 +64,8 @@ FSM_STATES (
CLAMP_DROPING_DOOR_OPENING,
/* Droping a tower, waiting for robot to advance. */
CLAMP_DROPING_WAITING_ROBOT,
+ /* Clamp locked in a bay. */
+ CLAMP_LOCKED,
/* Waiting movement order. */
CLAMP_MOVE_IDLE,
@@ -354,6 +356,7 @@ FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
move_element, CLAMP_MOVING_ELEMENT,
move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
done, CLAMP_IDLE)
{
logistic_element_new (ctx.pos_new, ctx.new_element_type);
@@ -365,8 +368,14 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
}
else if (logistic_global.clamp_pos_idle != ctx.pos_current)
{
- clamp_move (logistic_global.clamp_pos_idle);
- return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle);
+ if (logistic_path_clear (ctx.pos_current,
+ logistic_global.clamp_pos_idle))
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle);
+ }
+ else
+ return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked);
}
else
return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done);
@@ -375,6 +384,7 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
move_element, CLAMP_MOVING_ELEMENT,
move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
done, CLAMP_IDLE)
{
logistic_element_move_done ();
@@ -387,9 +397,16 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
}
else if (logistic_global.clamp_pos_idle != ctx.pos_current)
{
- clamp_move (logistic_global.clamp_pos_idle);
- return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
- move_to_idle);
+ if (logistic_path_clear (ctx.pos_current,
+ logistic_global.clamp_pos_idle))
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ move_to_idle);
+ }
+ else
+ return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
+ clamp_locked);
}
else
return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
@@ -406,6 +423,7 @@ FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
move_element, CLAMP_MOVING_ELEMENT,
move_to_idle, CLAMP_GOING_IDLE,
+ clamp_locked, CLAMP_LOCKED,
done, CLAMP_IDLE)
{
logistic_drop (ctx.drop_direction);
@@ -418,15 +436,32 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
}
else if (logistic_global.clamp_pos_idle != ctx.pos_current)
{
- clamp_move (logistic_global.clamp_pos_idle);
- return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
- move_to_idle);
+ if (logistic_path_clear (ctx.pos_current,
+ logistic_global.clamp_pos_idle))
+ {
+ clamp_move (logistic_global.clamp_pos_idle);
+ return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ move_to_idle);
+ }
+ else
+ return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ clamp_locked);
}
else
return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
done);
}
+FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
+{
+ /* If going forward, drop at back. */
+ uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM;
+ pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN);
+ pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN);
+ return FSM_NEXT (CLAMP_LOCKED, clamp_drop);
+}
+
/* CLAMP_MOVE FSM */
FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move,
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
index 8c2a759f..0dbbaa6c 100644
--- a/digital/io-hub/src/robospierre/logistic.c
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -221,3 +221,21 @@ logistic_drop (uint8_t direction)
logistic_decision ();
}
+static uint8_t
+logistic_slot_clear (uint8_t slot)
+{
+ if (CLAMP_IS_SLOT_IN_FRONT_BAY (slot)
+ && ctx.slots[CLAMP_SLOT_FRONT_MIDDLE])
+ return 0;
+ if (CLAMP_IS_SLOT_IN_BACK_BAY (slot)
+ && ctx.slots[CLAMP_SLOT_BACK_MIDDLE])
+ return 0;
+ return 1;
+}
+
+uint8_t
+logistic_path_clear (uint8_t slot1, uint8_t slot2)
+{
+ return logistic_slot_clear (slot1) && logistic_slot_clear (slot2);
+}
+
diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h
index fd47bd1e..7a742957 100644
--- a/digital/io-hub/src/robospierre/logistic.h
+++ b/digital/io-hub/src/robospierre/logistic.h
@@ -70,4 +70,8 @@ logistic_element_move_done (void);
void
logistic_drop (uint8_t direction);
+/** Is path clear between two positions? */
+uint8_t
+logistic_path_clear (uint8_t slot1, uint8_t slot2);
+
#endif /* logistic_h */