From 47b8f6e963111adc81b2c41506acb231f35f416d Mon Sep 17 00:00:00 2001 From: Jerome Jutteau Date: Tue, 5 Mar 2013 19:49:09 +0100 Subject: digital/io-hub/src/robospierre: update FSM --- digital/io-hub/src/robospierre/Makefile | 16 +++- digital/io-hub/src/robospierre/clamp.c | 137 +++++++++------------------ digital/io-hub/src/robospierre/main.c | 2 +- digital/io-hub/src/robospierre/move.c | 75 +++++++-------- digital/io-hub/src/robospierre/pawn_sensor.c | 2 +- digital/io-hub/src/robospierre/top.c | 70 ++++++-------- 6 files changed, 124 insertions(+), 178 deletions(-) (limited to 'digital/io-hub/src') 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); } } -- cgit v1.2.3