summaryrefslogtreecommitdiff
path: root/digital/io-hub
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-14 16:49:48 +0200
committerNicolas Schodet2011-05-14 16:49:48 +0200
commit5ba03e9744a8916730d9b8462cbfa6512d380ac6 (patch)
tree2bf4abd82f0881f7de77c250421f583e508ecbcc /digital/io-hub
parent3d91eb26478dd0801294ce0ead01605687687d5e (diff)
digital/io-hub: add drop
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/robospierre/clamp.c73
-rw-r--r--digital/io-hub/src/robospierre/clamp.h9
-rw-r--r--digital/io-hub/src/robospierre/logistic.c11
-rw-r--r--digital/io-hub/src/robospierre/logistic.h5
-rw-r--r--digital/io-hub/src/robospierre/main.c8
-rw-r--r--digital/io-hub/tools/io_hub/io_hub.py10
6 files changed, 116 insertions, 0 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 227579ff..6c063979 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -60,6 +60,10 @@ FSM_STATES (
CLAMP_TAKING_DOOR_CLOSING,
/* Moving elements around. */
CLAMP_MOVING_ELEMENT,
+ /* Droping a tower. */
+ CLAMP_DROPING_DOOR_OPENING,
+ /* Droping a tower, waiting for robot to advance. */
+ CLAMP_DROPING_WAITING_ROBOT,
/* Waiting movement order. */
CLAMP_MOVE_IDLE,
@@ -83,6 +87,13 @@ FSM_EVENTS (
start,
/* New element inside bottom slot. */
clamp_new_element,
+ /* Order to drop elements. */
+ clamp_drop,
+ /* Sent once drop is done, but robot should advance to completely
+ * free the dropped tower. */
+ clamp_drop_waiting,
+ /* Received when top FSM made the robot advance after a drop. */
+ clamp_drop_clear,
/* Order to move the clamp. */
clamp_move,
/* Clamp movement success. */
@@ -110,6 +121,8 @@ struct clamp_t
uint8_t pos_new;
/** New element kind. */
uint8_t new_element;
+ /** Drop direction, drop on the other side. */
+ uint8_t drop_direction;
};
/** Global context. */
@@ -190,6 +203,25 @@ clamp_new_element (uint8_t pos, uint8_t element)
}
uint8_t
+clamp_drop (uint8_t drop_direction)
+{
+ if (FSM_CAN_HANDLE (AI, clamp_drop))
+ {
+ ctx.drop_direction = drop_direction;
+ FSM_HANDLE (AI, clamp_drop);
+ return 1;
+ }
+ else
+ return 0;
+}
+
+void
+clamp_drop_clear (void)
+{
+ FSM_HANDLE (AI, clamp_drop_clear);
+}
+
+uint8_t
clamp_handle_event (void)
{
if (FSM_CAN_HANDLE (AI, clamp_new_element))
@@ -310,6 +342,16 @@ FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING)
return FSM_NEXT (CLAMP_IDLE, clamp_new_element);
}
+FSM_TRANS (CLAMP_IDLE, 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_IDLE, clamp_drop);
+}
+
FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
move_element, CLAMP_MOVING_ELEMENT,
move_to_idle, CLAMP_GOING_IDLE,
@@ -355,6 +397,37 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
done);
}
+FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
+ CLAMP_DROPING_WAITING_ROBOT)
+{
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_drop_waiting));
+ return FSM_NEXT_TIMEOUT (CLAMP_DROPING_DOOR_OPENING);
+}
+
+FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ move_element, CLAMP_MOVING_ELEMENT,
+ move_to_idle, CLAMP_GOING_IDLE,
+ done, CLAMP_IDLE)
+{
+ logistic_drop (ctx.drop_direction);
+ if (logistic_global.moving_from != CLAMP_SLOT_NB)
+ {
+ clamp_move_element (logistic_global.moving_from,
+ logistic_global.moving_to);
+ return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ move_element);
+ }
+ 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);
+ }
+ else
+ return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
+ done);
+}
+
/* CLAMP_MOVE FSM */
FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move,
diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h
index bb15b62e..a59e91ae 100644
--- a/digital/io-hub/src/robospierre/clamp.h
+++ b/digital/io-hub/src/robospierre/clamp.h
@@ -68,6 +68,15 @@ clamp_move (uint8_t pos);
void
clamp_move_element (uint8_t from, uint8_t to);
+/** Drop an element tower. Return 0 if not currently possible. If
+ * drop_direction is forward, drop at the back. */
+uint8_t
+clamp_drop (uint8_t drop_direction);
+
+/** Signal robot advanced, and drop is finished. */
+void
+clamp_drop_clear (void);
+
/** Examine sensors to generate new events, return non zero if an event was
* generated. */
uint8_t
diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c
index 534f1c46..945bd583 100644
--- a/digital/io-hub/src/robospierre/logistic.c
+++ b/digital/io-hub/src/robospierre/logistic.c
@@ -210,3 +210,14 @@ logistic_element_move_done (void)
logistic_decision ();
}
+void
+logistic_drop (uint8_t direction)
+{
+ uint8_t bay = direction == DIRECTION_FORWARD
+ ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM;
+ uint8_t i;
+ for (i = bay; i < bay + 3; i++)
+ ctx.slots[i] = 0;
+ logistic_decision ();
+}
+
diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h
index 75b49d53..c6aa4159 100644
--- a/digital/io-hub/src/robospierre/logistic.h
+++ b/digital/io-hub/src/robospierre/logistic.h
@@ -65,4 +65,9 @@ logistic_element_new (uint8_t pos, uint8_t element);
void
logistic_element_move_done (void);
+/** To be called when elements have been dropped on the opposite side of
+ * direction. */
+void
+logistic_drop (uint8_t direction);
+
#endif /* logistic_h */
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index ffb05a99..938703b6 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -222,6 +222,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_OPEN);
pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_OPEN);
break;
+ case c ('d', 1):
+ /* Drop elements.
+ * - 1b: 00: drop clear, 01: drop forward, 02: drop backward. */
+ if (args[0] == 0x00)
+ clamp_drop_clear ();
+ else
+ clamp_drop (args[0]);
+ break;
/* Stats commands.
* - b: interval between stats. */
case c ('A', 1):
diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py
index 1b25102c..d6ae8618 100644
--- a/digital/io-hub/tools/io_hub/io_hub.py
+++ b/digital/io-hub/tools/io_hub/io_hub.py
@@ -50,6 +50,16 @@ class Proto:
def clamp_move_element (self, from_, to):
self.proto.send ('c', 'BB', from_, to)
+ def drop (self, order):
+ if order == 'drop_clear':
+ self.proto.send ('d', 'B', 0x00)
+ elif order == 'drop_forward':
+ self.proto.send ('d', 'B', 0x01)
+ elif order == 'drop_backward':
+ self.proto.send ('d', 'B', 0x02)
+ else:
+ raise ValueError
+
def close (self):
self.reset ()
self.proto.wait (lambda: True)