summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub/src/robospierre
diff options
context:
space:
mode:
authorJerome Jutteau2013-03-05 19:49:09 +0100
committerJerome Jutteau2013-03-17 15:10:13 +0100
commit47b8f6e963111adc81b2c41506acb231f35f416d (patch)
treec481b159a7f64602861a1f68ae3fcdd260dd8cdc /digital/io-hub/src/robospierre
parent4af6e816f0ca13d7ee85ce5892520e493782e33c (diff)
digital/io-hub/src/robospierre: update FSM
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/Makefile16
-rw-r--r--digital/io-hub/src/robospierre/clamp.c137
-rw-r--r--digital/io-hub/src/robospierre/main.c2
-rw-r--r--digital/io-hub/src/robospierre/move.c75
-rw-r--r--digital/io-hub/src/robospierre/pawn_sensor.c2
-rw-r--r--digital/io-hub/src/robospierre/top.c70
6 files changed, 124 insertions, 178 deletions
diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile
index 0f8d89c5..a187c457 100644
--- a/digital/io-hub/src/robospierre/Makefile
+++ b/digital/io-hub/src/robospierre/Makefile
@@ -33,7 +33,7 @@ vpath %.c $(AI_MODULES:%=../../../ai/src/%)
vpath %.h $(AI_MODULES:%=../../../ai/src/%)
INCLUDES += -I. -I../common $(AI_MODULES:%=-I../../../ai/src/%)
-EXTRA_CLEAN_FILES = angfsm_gen_avr_AI.h fsm_AI_gen.avr.c
+EXTRA_CLEAN_FILES = angfsm_gen_avr_AI.h fsm_AI_gen.avr.c AI.dot AI.png AI.svg
include $(BASE)/make/Makefile.gen
@@ -41,5 +41,17 @@ include $(BASE)/make/Makefile.gen
obj/main.avr.o: fsm_AI_gen.h
fsm_AI_gen.avr.c: fsm_AI_gen.h
fsm_AI_gen.h: io_hub.host
- ./$< --ang-gen avr
+ ./$< --ang-gen avr --ang-dot AI.dot
mv angfsm_gen_avr_AI.c fsm_AI_gen.avr.c
+
+# Image generation
+view: AI.view
+png: AI.png
+svg: AI.svg
+
+%.view: %.dot
+ dot -Txlib $<
+%.png: %.dot
+ dot -Tpng $< > $@
+%.svg: %.dot
+ dot -Tsvg $< > $@
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 641bf4de..5b801bb9 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -240,7 +240,7 @@ clamp_move (uint8_t pos)
FSM_HANDLE (AI, clamp_move);
}
else
- fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
+ fsm_queue_post_event (FSM_EVENT (clamp_move_success));
}
void
@@ -550,7 +550,7 @@ clamp_blocked (void)
mimot_motor_free (0, 1);
mimot_motor_free (1, 1);
/* Signal problem. */
- fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure));
+ fsm_queue_post_event (FSM_EVENT (clamp_move_failure));
}
#define CLAMP_DECISION_MOVE_ELEMENT 0
@@ -580,14 +580,14 @@ clamp_decision (uint8_t unblock)
else
{
ctx.working = 0;
- fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ fsm_queue_post_event (FSM_EVENT (clamp_done));
return CLAMP_DECISION_CLAMP_LOCKED;
}
}
else
{
ctx.working = 0;
- fsm_queue_post_event (FSM_EVENT (AI, clamp_done));
+ fsm_queue_post_event (FSM_EVENT (clamp_done));
return CLAMP_DECISION_DONE;
}
}
@@ -601,7 +601,6 @@ FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING)
pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT);
pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT);
pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_INIT);
- return FSM_NEXT (CLAMP_START, init_actuators);
}
FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT,
@@ -610,14 +609,12 @@ FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT,
mimot_move_motor0_absolute (mimot_get_motor0_position () +
BOT_CLAMP_INIT_ELEVATION_STEP,
BOT_CLAMP_INIT_ELEVATION_SPEED);
- return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPENING);
}
FSM_TRANS (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success,
CLAMP_INIT_FINDING_ROTATION_EDGE)
{
mimot_motor1_find_zero (BOT_CLAMP_INIT_ROTATION_SPEED, 0, 0);
- return FSM_NEXT (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success);
}
FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success,
@@ -625,8 +622,6 @@ FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success,
{
mimot_motor0_find_zero (BOT_CLAMP_INIT_ELEVATION_SPEED, 1,
BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP);
- return FSM_NEXT (CLAMP_INIT_FINDING_ROTATION_EDGE,
- clamp_rotation_success);
}
FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
@@ -636,14 +631,12 @@ FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
if (IO_GET (CONTACT_STRAT))
{
clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE);
- return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
- rest);
+ return FSM_BRANCH (rest);
}
else
{
clamp_move (logistic_global.clamp_pos_idle);
- return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success,
- demo);
+ return FSM_BRANCH (demo);
}
}
@@ -651,7 +644,6 @@ FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY)
{
mimot_motor_free (0, 1);
mimot_motor_free (1, 1);
- return FSM_NEXT (CLAMP_INIT_GOING_REST, clamp_move_success);
}
FSM_TRANS (CLAMP_INIT_READY, init_start_round, CLAMP_GOING_IDLE)
@@ -661,29 +653,25 @@ FSM_TRANS (CLAMP_INIT_READY, init_start_round, CLAMP_GOING_IDLE)
pwm_set (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT_START);
pwm_set (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT_START);
clamp_move (logistic_global.clamp_pos_idle);
- return FSM_NEXT (CLAMP_INIT_READY, init_start_round);
}
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_queue_post_event (FSM_EVENT (clamp_done));
}
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_queue_post_event (FSM_EVENT (clamp_blocked));
}
FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING)
{
ctx.working = 1;
- fsm_queue_post_event (FSM_EVENT (AI, clamp_working));
+ fsm_queue_post_event (FSM_EVENT (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);
}
FSM_TRANS (CLAMP_IDLE, clamp_prepare,
@@ -697,13 +685,13 @@ FSM_TRANS (CLAMP_IDLE, clamp_prepare,
{
default:
case CLAMP_DECISION_MOVE_ELEMENT:
- return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element);
+ return FSM_BRANCH (move_element);
case CLAMP_DECISION_MOVE_TO_IDLE:
- return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_to_idle);
+ return FSM_BRANCH (move_to_idle);
case CLAMP_DECISION_CLAMP_LOCKED:
- return FSM_NEXT (CLAMP_IDLE, clamp_prepare, clamp_locked);
+ return FSM_BRANCH (clamp_locked);
case CLAMP_DECISION_DONE:
- return FSM_NEXT (CLAMP_IDLE, clamp_prepare, done);
+ return FSM_BRANCH (done);
}
}
@@ -714,7 +702,6 @@ FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM;
pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN (bay + 0));
pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN (bay + 2));
- return FSM_NEXT (CLAMP_IDLE, clamp_drop);
}
FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
@@ -729,13 +716,13 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME,
{
default:
case CLAMP_DECISION_MOVE_ELEMENT:
- return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element);
+ return FSM_BRANCH (move_element);
case CLAMP_DECISION_MOVE_TO_IDLE:
- return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle);
+ return FSM_BRANCH (move_to_idle);
case CLAMP_DECISION_CLAMP_LOCKED:
- return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked);
+ return FSM_BRANCH (clamp_locked);
case CLAMP_DECISION_DONE:
- return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done);
+ return FSM_BRANCH (done);
}
}
@@ -750,31 +737,25 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success,
{
default:
case CLAMP_DECISION_MOVE_ELEMENT:
- return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
- move_element);
+ return FSM_BRANCH (move_element);
case CLAMP_DECISION_MOVE_TO_IDLE:
- return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
- move_to_idle);
+ return FSM_BRANCH (move_to_idle);
case CLAMP_DECISION_CLAMP_LOCKED:
- return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
- clamp_locked);
+ return FSM_BRANCH (clamp_locked);
case CLAMP_DECISION_DONE:
- return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success,
- done);
+ return FSM_BRANCH (done);
}
}
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_queue_post_event (FSM_EVENT (clamp_blocked));
}
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_queue_post_event (FSM_EVENT (clamp_drop_waiting));
}
FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
@@ -788,17 +769,13 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
{
default:
case CLAMP_DECISION_MOVE_ELEMENT:
- return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
- move_element);
+ return FSM_BRANCH (move_element);
case CLAMP_DECISION_MOVE_TO_IDLE:
- return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
- move_to_idle);
+ return FSM_BRANCH (move_to_idle);
case CLAMP_DECISION_CLAMP_LOCKED:
- return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
- clamp_locked);
+ return FSM_BRANCH (clamp_locked);
case CLAMP_DECISION_DONE:
- return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear,
- done);
+ return FSM_BRANCH (done);
}
}
@@ -808,7 +785,6 @@ FSM_TRANS (CLAMP_LOCKED, clamp_new_element, CLAMP_LOCKED)
BOT_PWM_DOOR_CLOSE (ctx.pos_new));
logistic_element_new (ctx.pos_new, ctx.new_element_type);
clamp_taken_pawn (ctx.new_element_type);
- return FSM_NEXT (CLAMP_LOCKED, clamp_new_element);
}
FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
@@ -818,7 +794,6 @@ FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING)
? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM;
pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN (bay + 0));
pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN (bay + 2));
- return FSM_NEXT (CLAMP_LOCKED, clamp_drop);
}
FSM_TRANS (CLAMP_BLOCKED, clamp_prepare,
@@ -830,11 +805,11 @@ FSM_TRANS (CLAMP_BLOCKED, clamp_prepare,
{
default:
case CLAMP_DECISION_MOVE_TO_IDLE:
- return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle);
+ return FSM_BRANCH (move_to_idle);
case CLAMP_DECISION_CLAMP_LOCKED:
- return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked);
+ return FSM_BRANCH (clamp_locked);
case CLAMP_DECISION_DONE:
- return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, done);
+ return FSM_BRANCH (done);
}
}
@@ -848,19 +823,19 @@ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move,
if (ctx.moving_to == CLAMP_POS_NB)
{
clamp_route ();
- return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move);
+ return FSM_BRANCH (move);
}
else
{
if (ctx.pos_current != ctx.pos_request)
{
clamp_route ();
- return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element);
+ return FSM_BRANCH (move_element);
}
else
{
clamp_openclose (0);
- return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element_here);
+ return FSM_BRANCH (move_element_here);
}
}
}
@@ -871,15 +846,13 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
{
if (ctx.pos_current == ctx.pos_request)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
- return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
- done);
+ fsm_queue_post_event (FSM_EVENT (clamp_move_success));
+ return FSM_BRANCH (done);
}
else
{
clamp_route ();
- return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success,
- next);
+ return FSM_BRANCH (next);
}
}
@@ -887,7 +860,6 @@ 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,
@@ -897,21 +869,18 @@ FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
{
if (clamp_tower_check ())
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
- return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success,
- cancel);
+ fsm_queue_post_event (FSM_EVENT (clamp_move_success));
+ return FSM_BRANCH (cancel);
}
else if (ctx.pos_current == ctx.pos_request)
{
clamp_openclose (0);
- return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
- clamp_elevation_rotation_success, done);
+ return FSM_BRANCH (done);
}
else
{
clamp_route ();
- return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING,
- clamp_elevation_rotation_success, next);
+ return FSM_BRANCH (next);
}
}
@@ -919,26 +888,24 @@ 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)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_taken));
+ fsm_queue_post_event (FSM_EVENT (clamp_taken));
if (clamp_slot_door[ctx.pos_current] != 0xff)
{
pwm_set_timed (clamp_slot_door[ctx.pos_current],
BOT_PWM_DOOR_OPEN (ctx.pos_current));
- return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, open_door);
+ return FSM_BRANCH (open_door);
}
else
{
ctx.pos_request = ctx.moving_to;
clamp_route ();
- return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, move);
+ return FSM_BRANCH (move);
}
}
@@ -947,7 +914,6 @@ FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING, BOT_PWM_DOOR_OPEN_TIME,
{
ctx.pos_request = ctx.moving_to;
clamp_route ();
- return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING);
}
FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success,
@@ -962,23 +928,18 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success,
{
pwm_set_timed (clamp_slot_door[ctx.pos_current],
BOT_PWM_DOOR_CLOSE (ctx.pos_current));
- return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
- clamp_elevation_rotation_success,
- done_close_door);
+ return FSM_BRANCH (done_close_door);
}
else
{
clamp_openclose (1);
- return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
- clamp_elevation_rotation_success,
- done_open_clamp);
+ return FSM_BRANCH (done_open_clamp);
}
}
else
{
clamp_route ();
- return FSM_NEXT (CLAMP_MOVE_DST_ROUTING,
- clamp_elevation_rotation_success, next);
+ return FSM_BRANCH (next);
}
}
@@ -986,21 +947,17 @@ 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)
{
clamp_openclose (1);
- return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING);
}
FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING, BOT_PWM_CLAMP_OPEN_TIME,
CLAMP_MOVE_IDLE)
{
- fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success));
- return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING);
+ fsm_queue_post_event (FSM_EVENT (clamp_move_success));
}
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index e812226d..2cc3d366 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -267,7 +267,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
case c ('j', 0):
/* Simulate jack insertion. */
- fsm_queue_post_event (FSM_EVENT (AI, jack_inserted));
+ fsm_queue_post_event (FSM_EVENT (jack_inserted));
break;
case c ('w', 3):
/* Set PWM.
diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c
index e3542e14..ea3f2654 100644
--- a/digital/io-hub/src/robospierre/move.c
+++ b/digital/io-hub/src/robospierre/move.c
@@ -83,7 +83,7 @@ move_start (position_t position, uint8_t backward)
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Start the FSM. */
- fsm_queue_post_event (FSM_EVENT (AI, move_start));
+ fsm_queue_post_event (FSM_EVENT (move_start));
}
void
@@ -98,14 +98,14 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten)
/* Reset try counter. */
move_data.try_again_counter = 3;
/* Start the FSM. */
- fsm_queue_post_event (FSM_EVENT (AI, move_start));
+ fsm_queue_post_event (FSM_EVENT (move_start));
}
void
move_stop (void)
{
/* Stop the FSM. */
- fsm_queue_post_event (FSM_EVENT (AI, move_stop));
+ fsm_queue_post_event (FSM_EVENT (move_stop));
}
void
@@ -318,14 +318,14 @@ FSM_TRANS (MOVE_IDLE, move_start,
if (next)
{
if (next == 2)
- return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT (MOVE_IDLE, move_start, path_found);
+ return FSM_BRANCH (path_found);
}
else
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_IDLE, move_start, no_path_found);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (no_path_found);
}
}
@@ -334,7 +334,6 @@ FSM_TRANS (MOVE_ROTATING,
MOVE_MOVING)
{
move_go ();
- return FSM_NEXT (MOVE_ROTATING, robot_move_success);
}
FSM_TRANS (MOVE_ROTATING,
@@ -342,20 +341,17 @@ FSM_TRANS (MOVE_ROTATING,
MOVE_MOVING)
{
move_go ();
- return FSM_NEXT (MOVE_ROTATING, robot_move_failure);
}
FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250,
MOVE_MOVING)
{
move_go ();
- return FSM_NEXT_TIMEOUT (MOVE_ROTATING);
}
FSM_TRANS (MOVE_ROTATING, move_stop, MOVE_IDLE)
{
asserv_stop_motor ();
- return FSM_NEXT (MOVE_ROTATING, move_stop);
}
FSM_TRANS (MOVE_MOVING, robot_move_success,
@@ -366,18 +362,18 @@ FSM_TRANS (MOVE_MOVING, robot_move_success,
{
if (move_data.final_move)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_success));
- return FSM_NEXT (MOVE_MOVING, robot_move_success, done);
+ fsm_queue_post_event (FSM_EVENT (move_success));
+ return FSM_BRANCH (done);
}
else
{
uint8_t next = move_path_next ();
if (next == 2)
- return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found);
+ return FSM_BRANCH (path_found);
}
- //return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found);
+ //return FSM_BRANCH (no_path_found);
}
static void
@@ -405,14 +401,12 @@ FSM_TRANS (MOVE_MOVING,
MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
{
move_moving_backward_to_turn_freely ();
- return FSM_NEXT (MOVE_MOVING, robot_move_failure);
}
FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500,
MOVE_MOVING_BACKWARD_TO_TURN_FREELY)
{
move_moving_backward_to_turn_freely ();
- return FSM_NEXT_TIMEOUT (MOVE_MOVING);
}
FSM_TRANS (MOVE_MOVING, obstacle_in_front,
@@ -423,17 +417,16 @@ FSM_TRANS (MOVE_MOVING, obstacle_in_front,
asserv_stop_motor ();
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (tryout);
}
else
- return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain);
+ return FSM_BRANCH (tryagain);
}
FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE)
{
asserv_stop_motor ();
- return FSM_NEXT (MOVE_MOVING, move_stop);
}
FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success,
@@ -444,8 +437,8 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success,
{
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (tryout);
}
else
{
@@ -453,14 +446,14 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success,
if (next)
{
if (next == 2)
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found);
+ return FSM_BRANCH (path_found);
}
else
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, no_path_found);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (no_path_found);
}
}
}
@@ -474,8 +467,8 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure,
{
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (tryout);
}
else
{
@@ -483,19 +476,19 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure,
if (next)
{
if (next == 2)
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found);
+ return FSM_BRANCH (path_found);
}
else
{
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (no_path_found_tryout);
}
else
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryagain);
+ return FSM_BRANCH (no_path_found_tryagain);
}
}
}
@@ -503,7 +496,6 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure,
FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop, MOVE_IDLE)
{
asserv_stop_motor ();
- return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop);
}
FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250,
@@ -517,25 +509,24 @@ FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250,
if (next)
{
if (next == 2)
- return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate);
+ return FSM_BRANCH (path_found_rotate);
else
- return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found);
+ return FSM_BRANCH (path_found);
}
else
{
/* Error, no new position, should we try again? */
if (--move_data.try_again_counter == 0)
{
- fsm_queue_post_event (FSM_EVENT (AI, move_failure));
- return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout);
+ fsm_queue_post_event (FSM_EVENT (move_failure));
+ return FSM_BRANCH (no_path_found_tryout);
}
else
- return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain);
+ return FSM_BRANCH (no_path_found_tryagain);
}
}
FSM_TRANS (MOVE_WAIT_FOR_CLEAR_PATH, move_stop, MOVE_IDLE)
{
- return FSM_NEXT (MOVE_WAIT_FOR_CLEAR_PATH, move_stop);
}
diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c
index 7896aa72..7cb39f11 100644
--- a/digital/io-hub/src/robospierre/pawn_sensor.c
+++ b/digital/io-hub/src/robospierre/pawn_sensor.c
@@ -181,7 +181,7 @@ pawn_sensor_bumper (uint8_t bumped, uint16_t dx, uint16_t dy)
}
/* OK, take it. */
pawn_sensor_global.last_bumped = bumped_pawn;
- fsm_queue_post_event (FSM_EVENT (AI, top_bumper));
+ fsm_queue_post_event (FSM_EVENT (top_bumper));
pawn_sensor_global.bump_wait = 3 * 250;
}
}
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 384eee48..6f37559c 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -104,25 +104,21 @@ FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1)
ctx.green_again = 3;
asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100),
PG_Y (PG_LENGTH - 200), 0);
- return FSM_NEXT (TOP_START, init_start_round);
}
FSM_TRANS (TOP_GOING_OUT1, robot_move_success, TOP_GOING_OUT2)
{
asserv_goto (PG_X (1500 - 2 * 350), PG_Y (PG_LENGTH - 350), 0);
- return FSM_NEXT (TOP_GOING_OUT1, robot_move_success);
}
FSM_TRANS (TOP_GOING_OUT1, robot_move_failure, TOP_GOING_OUT1_BLOCK_WAIT)
{
- return FSM_NEXT (TOP_GOING_OUT1, robot_move_failure);
}
FSM_TRANS_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT, 250, TOP_GOING_OUT1)
{
asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100),
PG_Y (PG_LENGTH - 200 - (++ctx.chaos % 4) * 10), 0);
- return FSM_NEXT_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT);
}
static uint8_t
@@ -232,17 +228,16 @@ FSM_TRANS (TOP_GOING_OUT2, robot_move_success,
element, TOP_GOING_TO_ELEMENT)
{
if (clamp_working ())
- return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working);
+ return FSM_BRANCH (clamp_working);
switch (top_decision ())
{
- default: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, drop);
- case 1: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element);
+ default: return FSM_BRANCH (drop);
+ case 1: return FSM_BRANCH (element);
}
}
FSM_TRANS (TOP_GOING_OUT2, robot_move_failure, TOP_GOING_OUT2_BLOCK_WAIT)
{
- return FSM_NEXT (TOP_GOING_OUT2, robot_move_failure);
}
FSM_TRANS_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, 250,
@@ -251,11 +246,11 @@ FSM_TRANS_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, 250,
element, TOP_GOING_TO_ELEMENT)
{
if (clamp_working ())
- return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, clamp_working);
+ return FSM_BRANCH (clamp_working);
switch (top_decision ())
{
- default: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, drop);
- case 1: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, element);
+ default: return FSM_BRANCH (drop);
+ case 1: return FSM_BRANCH (element);
}
}
@@ -266,14 +261,14 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success,
if (logistic_global.ready)
{
if (clamp_drop (logistic_global.collect_direction))
- return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready);
+ return FSM_BRANCH (ready);
else
- return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
+ return FSM_BRANCH (wait_clamp);
}
else
{
clamp_prepare (top_prepare_level ());
- return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp);
+ return FSM_BRANCH (wait_clamp);
}
}
@@ -285,18 +280,17 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_failure,
if (ctx.target_element_id != 0xff)
element_failure (ctx.target_element_id);
if (clamp_working ())
- return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, clamp_working);
+ return FSM_BRANCH (clamp_working);
switch (top_decision ())
{
- default: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, drop);
- case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, element);
+ default: return FSM_BRANCH (drop);
+ case 1: return FSM_BRANCH (element);
}
}
FSM_TRANS (TOP_GOING_TO_DROP, clamp_working, TOP_WAITING_CLAMP)
{
move_stop ();
- return FSM_NEXT (TOP_GOING_TO_DROP, clamp_working);
}
FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
@@ -307,7 +301,7 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
if (ctx.target_element_id != 0xff)
element_failure (ctx.target_element_id); /* Do not take this one again. */
if (clamp_working ())
- return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working);
+ return FSM_BRANCH (clamp_working);
if (ctx.green_again)
{
ctx.green_again--;
@@ -316,8 +310,8 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success,
}
switch (top_decision ())
{
- default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop);
- case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element);
+ default: return FSM_BRANCH (drop);
+ case 1: return FSM_BRANCH (element);
}
}
@@ -329,7 +323,7 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure,
if (ctx.target_element_id != 0xff)
element_failure (ctx.target_element_id);
if (clamp_working ())
- return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working);
+ return FSM_BRANCH (clamp_working);
if (ctx.green_again)
{
ctx.green_again--;
@@ -338,8 +332,8 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure,
}
switch (top_decision ())
{
- default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, drop);
- case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, element);
+ default: return FSM_BRANCH (drop);
+ case 1: return FSM_BRANCH (element);
}
}
@@ -352,7 +346,6 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP)
if (ctx.green_again == 0)
element_no_more_green ();
}
- return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working);
}
FSM_TRANS (TOP_GOING_TO_ELEMENT, top_bumper, TOP_GOING_TO_ELEMENT)
@@ -362,7 +355,6 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, top_bumper, TOP_GOING_TO_ELEMENT)
move_stop ();
ctx.target_element_id = 0xff;
top_go_this_element (pawn_sensor_get_last_bumped (), BOT_ELEMENT_RADIUS - 50);
- return FSM_NEXT (TOP_GOING_TO_ELEMENT, top_bumper);
}
FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
@@ -371,14 +363,13 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
{
switch (top_decision ())
{
- default: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, drop);
- case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element);
+ default: return FSM_BRANCH (drop);
+ case 1: return FSM_BRANCH (element);
}
}
FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT)
{
- return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked);
}
FSM_TRANS (TOP_WAITING_CLAMP, clamp_taken, TOP_WAITING_CLAMP)
@@ -388,7 +379,6 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_taken, TOP_WAITING_CLAMP)
if (robot_pos.v.x < 400 || robot_pos.v.x > PG_WIDTH - 400)
asserv_move_linearly (ctx.go_to_element_direction
== DIRECTION_FORWARD ? -50 : 50);
- return FSM_NEXT (TOP_WAITING_CLAMP, clamp_taken);
}
FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250,
@@ -400,26 +390,24 @@ FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250,
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);
+ return FSM_BRANCH (try_again);
}
else
{
ctx.broken = 1;
clamp_prepare (3);
- return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, tryout);
+ return FSM_BRANCH (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,
@@ -427,14 +415,13 @@ FSM_TRANS (TOP_WAITING_READY, clamp_done,
not_ready, TOP_WAITING_READY)
{
if (clamp_drop (logistic_global.collect_direction))
- return FSM_NEXT (TOP_WAITING_READY, clamp_done, drop);
+ return FSM_BRANCH (drop);
else
- return FSM_NEXT (TOP_WAITING_READY, clamp_done, not_ready);
+ return FSM_BRANCH (not_ready);
}
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)
@@ -447,7 +434,6 @@ FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING)
element_i_like_green ();
asserv_move_linearly (logistic_global.collect_direction
== DIRECTION_FORWARD ? 150 : -150);
- return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting);
}
FSM_TRANS (TOP_DROP_CLEARING, robot_move_success,
@@ -457,8 +443,8 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success,
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);
+ default: return FSM_BRANCH (drop);
+ case 1: return FSM_BRANCH (element);
}
}
@@ -469,8 +455,8 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure,
clamp_drop_clear ();
switch (top_decision ())
{
- default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop);
- case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, element);
+ default: return FSM_BRANCH (drop);
+ case 1: return FSM_BRANCH (element);
}
}