summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/robospierre/clamp.c40
-rw-r--r--digital/io-hub/src/robospierre/clamp.h4
-rw-r--r--digital/io-hub/src/robospierre/top.c17
3 files changed, 60 insertions, 1 deletions
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index b2a6bf76..0a7654f3 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -95,6 +95,10 @@ FSM_STATES (
FSM_EVENTS (
/* New element inside bottom slot. */
clamp_new_element,
+ /* Sent when clamp is working. */
+ clamp_working,
+ /* Sent when clamp return to idle state. */
+ clamp_done,
/* Order to drop elements. */
clamp_drop,
/* Sent once drop is done, but robot should advance to completely
@@ -123,6 +127,8 @@ FSM_START_WITH (CLAMP_MOVE_IDLE)
/** Clamp context. */
struct clamp_t
{
+ /** True if clamp is working. */
+ uint8_t working;
/** Current position. */
uint8_t pos_current;
/** Requested position. */
@@ -196,6 +202,12 @@ clamp_init (void)
ctx.open = 1;
}
+uint8_t
+clamp_working (void)
+{
+ return ctx.working;
+}
+
void
clamp_move (uint8_t pos)
{
@@ -428,11 +440,15 @@ FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE)
{
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success);
}
FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING)
{
+ ctx.working = 1;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_working));
pwm_set_timed (clamp_slot_door[ctx.pos_new],
BOT_PWM_DOOR_CLOSE (ctx.pos_new));
return FSM_NEXT (CLAMP_IDLE, clamp_new_element);
@@ -470,10 +486,18 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle);
}
else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked);
+ }
}
else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done);
+ }
}
FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
@@ -500,12 +524,20 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
move_to_idle);
}
else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
clamp_locked);
+ }
}
else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
done);
+ }
}
FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
@@ -539,12 +571,20 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
move_to_idle);
}
else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
clamp_locked);
+ }
}
else
+ {
+ ctx.working = 0;
+ fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
done);
+ }
}
FSM_TRANS (CLAMP_LOCKED, clamp_new_element, CLAMP_LOCKED)
diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h
index 09233cce..60da1615 100644
--- a/digital/io-hub/src/robospierre/clamp.h
+++ b/digital/io-hub/src/robospierre/clamp.h
@@ -64,6 +64,10 @@ enum {
void
clamp_init (void);
+/** Is clamp working? */
+uint8_t
+clamp_working (void);
+
/** Move clamp to given position. */
void
clamp_move (uint8_t pos);
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index a3ad4c72..92e42b1c 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -49,7 +49,9 @@ FSM_STATES (
TOP_GOING_OUT2,
TOP_GOING_TO_DROP,
- TOP_GOING_TO_ELEMENT)
+ TOP_GOING_TO_ELEMENT,
+ /* Waiting clamp has finished its work. */
+ TOP_WAITING_CLAMP)
FSM_START_WITH (TOP_START)
@@ -137,10 +139,13 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success,
}
FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
+ clamp_working, TOP_WAITING_CLAMP,
drop, TOP_GOING_TO_DROP,
element, TOP_GOING_TO_ELEMENT)
{
element_taken (ctx.target_element_id, ELEMENT_PAWN);
+ if (clamp_working ())
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working);
switch (top_decision ())
{
default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop);
@@ -148,3 +153,13 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
}
}
+FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
+ drop, TOP_GOING_TO_DROP,
+ element, TOP_GOING_TO_ELEMENT)
+{
+ switch (top_decision ())
+ {
+ default: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, drop);
+ case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element);
+ }
+}