From b76073669f43dca5e10cc1127d0818a05859058f Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Sat, 9 May 2009 00:10:04 +0200 Subject: * digital/io/src - modification on filterbridge FSM due to meca changes - implementing filterbridge FSM - fix init FSM to add the timeout feature --- digital/io/src/aquajim.h | 11 +++++ digital/io/src/elevator.c | 6 +++ digital/io/src/elevator.h | 5 +++ digital/io/src/filterbridge.fsm | 75 ++++++++++++++------------------- digital/io/src/filterbridge_cb.c | 89 ++++++++++++++++++++-------------------- digital/io/src/init.fsm | 1 + digital/io/src/main.c | 3 ++ 7 files changed, 101 insertions(+), 89 deletions(-) (limited to 'digital') diff --git a/digital/io/src/aquajim.h b/digital/io/src/aquajim.h index 6abc0f6b..53d06567 100644 --- a/digital/io/src/aquajim.h +++ b/digital/io/src/aquajim.h @@ -34,4 +34,15 @@ */ #define MATCH_DURATION_MS 90000 +/** + * filterbridge servo + */ + +#define SERVO_FINGER_ID 0 +#define SERVO_DOOR_ID 1 +#define SERVO_FINGER_IDLE 0 +#define SERVO_FINGER_PUSHING 1 +#define SERVO_DOOR_OPEN 0 +#define SERVO_DOOR_CLOSE 1 + #endif /* aquajim_h */ diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c index 8da3b6c7..3a4a81ee 100644 --- a/digital/io/src/elevator.c +++ b/digital/io/src/elevator.c @@ -22,8 +22,14 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "common.h" #include "elevator.h" +/** + * State of the elevator + */ +uint8_t elevator_is_ready; + /* +AutoDec */ /* -AutoDec */ diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h index 3e8c9065..0684559c 100644 --- a/digital/io/src/elevator.h +++ b/digital/io/src/elevator.h @@ -25,4 +25,9 @@ * * }}} */ +/** + * State of the elevator + */ +extern uint8_t elevator_is_ready; + #endif // elevator_h diff --git a/digital/io/src/filterbridge.fsm b/digital/io/src/filterbridge.fsm index 97bcfa54..2a87ab05 100644 --- a/digital/io/src/filterbridge.fsm +++ b/digital/io/src/filterbridge.fsm @@ -5,66 +5,51 @@ filterbridge States: IDLE waiting the lift is ready to accept new puck + WAIT_JACK_IN + waiting for the jack to be inserted into the bot + CLOSE_DOOR[timeout=225] + close access to the lift (and wait 1 sec) WAIT_A_PUCK waiting for a new puck - WAIT_RGB_PROBE - waiting for RGB probe the first puck - EJECT_PUCK - eject a bad puck - RETURN_NORMAL_POS - move the bridge to his normal position - OPEN_DOOR + OPEN_DOOR[timeout=225] release puck to the lift and block a new puck on pos1 - CLOSE_DOOR - release pos1 puck and close access to the lift + PUSH_PUCK + push the puck to the lift Events: - lift_ready - lift ready to accept new puck + start + robot start + jack_inserted_into_bot + jack into the bot puck_on_pos2 a puck has been detected on bridge's position 2 no_puck_on_pos2 there is no puck on position 2 - bridge_in_position - bridge is in his normal position - door_opened - puck 1 blocked and puck 2 release to the lift - ejection_done - the ejection procedure is completed - color_probed - the result of the investigation will be revealed (suspens!!) - door_closed - ready to test a new puck state_timeout + timeout used to wait the end of servo move IDLE: - lift_ready -> WAIT_A_PUCK - the lift is ready to get pucks, we can begin testing procedure + start -> WAIT_JACK_IN + we wait for the jack -WAIT_A_PUCK: - puck_on_pos2: too_much_puck -> EJECT_PUCK - we have too much puck, eject the puck quickly and whistle - puck_on_pos2: nb_puck_ok -> WAIT_RGB_PROBE - probe color of the new puck - -WAIT_RGB_PROBE: - color_probed: bad_color -> EJECT_PUCK - eject bad puck - color_probed: good_color -> OPEN_DOOR - put puck to the lift +WAIT_JACK_IN: + jack_inserted_into_bot -> CLOSE_DOOR + we init the bridge -EJECT_PUCK: - ejection_done -> RETURN_NORMAL_POS - put bridge on normal position after puck ejection +CLOSE_DOOR: + state_timeout -> WAIT_A_PUCK + we are ready, we wait a puck -RETURN_NORMAL_POS: - bridge_in_position -> CLOSE_DOOR - make bridge ready to test a new puck +WAIT_A_PUCK: + puck_on_pos2: lift_not_ready -> . + Lift not ready, we stand by + puck_on_pos2: lift_ready -> OPEN_DOOR + Lift ok, we deliver OPEN_DOOR: - no_puck_on_pos2 -> CLOSE_DOOR - release puck to the lift + state_timeout -> PUSH_PUCK + we push the puck to the lift -CLOSE_DOOR: - door_closed -> IDLE - filter bridge ready +PUSH_PUCK: + no_puck_on_pos2 -> CLOSE_DOOR + the puck disappears, we close doors diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c index 7949f176..12210416 100644 --- a/digital/io/src/filterbridge_cb.c +++ b/digital/io/src/filterbridge_cb.c @@ -25,86 +25,87 @@ #include "common.h" #include "fsm.h" #include "filterbridge_cb.h" +#include "elevator.h" +#include "servo_pos.h" +#include "aquajim.h" /* - * WAIT_A_PUCK =puck_on_pos2=> - * nb_puck_ok => WAIT_RGB_PROBE - * probe color of the new puck - * too_much_puck => EJECT_PUCK - * we have too much puck, eject the puck quickly and whistle + * IDLE =start=> + * => WAIT_JACK_IN + * we wait for the jack */ fsm_branch_t -filterbridge__WAIT_A_PUCK__puck_on_pos2 (void) +filterbridge__IDLE__start (void) { - return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, nb_puck_ok); - return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, too_much_puck); + return filterbridge_next (IDLE, start); } /* - * WAIT_RGB_PROBE =color_probed=> - * bad_color => EJECT_PUCK - * eject bad puck - * good_color => OPEN_DOOR - * put puck to the lift + * WAIT_JACK_IN =jack_inserted_into_bot=> + * => CLOSE_DOOR + * we init the bridge */ fsm_branch_t -filterbridge__WAIT_RGB_PROBE__color_probed (void) +filterbridge__WAIT_JACK_IN__jack_inserted_into_bot (void) { - return filterbridge_next_branch (WAIT_RGB_PROBE, color_probed, bad_color); - return filterbridge_next_branch (WAIT_RGB_PROBE, color_probed, good_color); + servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE); + servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE); + return filterbridge_next (WAIT_JACK_IN, jack_inserted_into_bot); } /* - * IDLE =lift_ready=> + * CLOSE_DOOR =state_timeout=> * => WAIT_A_PUCK - * the lift is ready to get pucks, we can begin testing procedure + * we are ready, we wait a puck */ fsm_branch_t -filterbridge__IDLE__lift_ready (void) +filterbridge__CLOSE_DOOR__state_timeout (void) { - return filterbridge_next (IDLE, lift_ready); + return filterbridge_next (CLOSE_DOOR, state_timeout); } /* - * CLOSE_DOOR =door_closed=> - * => IDLE - * filter bridge ready + * WAIT_A_PUCK =puck_on_pos2=> + * lift_not_ready => WAIT_A_PUCK + * Lift not ready, we stand by + * lift_ready => OPEN_DOOR + * Lift ok, we deliver */ fsm_branch_t -filterbridge__CLOSE_DOOR__door_closed (void) +filterbridge__WAIT_A_PUCK__puck_on_pos2 (void) { - return filterbridge_next (CLOSE_DOOR, door_closed); + if(elevator_is_ready) + { + servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_OPEN); + return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, lift_ready); + } + else + return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, lift_not_ready); } /* - * RETURN_NORMAL_POS =bridge_in_position=> - * => CLOSE_DOOR - * make bridge ready to test a new puck + * OPEN_DOOR =state_timeout=> + * => PUSH_PUCK + * we push the puck to the lift */ fsm_branch_t -filterbridge__RETURN_NORMAL_POS__bridge_in_position (void) +filterbridge__OPEN_DOOR__state_timeout (void) { - return filterbridge_next (RETURN_NORMAL_POS, bridge_in_position); + servo_pos_move_to(SERVO_FINGER_ID,SERVO_FINGER_PUSHING); + return filterbridge_next (OPEN_DOOR, state_timeout); } /* - * OPEN_DOOR =no_puck_on_pos2=> + * PUSH_PUCK =no_puck_on_pos2=> * => CLOSE_DOOR - * release puck to the lift + * the puck disappears, we close doors */ fsm_branch_t -filterbridge__OPEN_DOOR__no_puck_on_pos2 (void) +filterbridge__PUSH_PUCK__no_puck_on_pos2 (void) { - return filterbridge_next (OPEN_DOOR, no_puck_on_pos2); + servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE); + servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE); + return filterbridge_next (PUSH_PUCK, no_puck_on_pos2); } -/* - * EJECT_PUCK =ejection_done=> - * => RETURN_NORMAL_POS - * put bridge on normal position after puck ejection - */ -fsm_branch_t -filterbridge__EJECT_PUCK__ejection_done (void) -{ - return filterbridge_next (EJECT_PUCK, ejection_done); -} + diff --git a/digital/io/src/init.fsm b/digital/io/src/init.fsm index db69e15c..fda5247a 100644 --- a/digital/io/src/init.fsm +++ b/digital/io/src/init.fsm @@ -35,6 +35,7 @@ Events: the tempo is finished move_done the move is finished + state_timeout IDLE: start -> WAIT_JACK_IN diff --git a/digital/io/src/main.c b/digital/io/src/main.c index c3f85770..4eeddc87 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -151,6 +151,7 @@ main_init (void) /* Start all FSM (except move FSM) */ fsm_handle_event (&top_fsm, TOP_EVENT_start); fsm_handle_event (&init_fsm, INIT_EVENT_start); + fsm_handle_event (&filterbridge_fsm, FILTERBRIDGE_EVENT_start); /* fsm_handle_event (&top_fsm, TOP_EVENT_start); fsm_handle_event (&top_fsm, TOP_EVENT_start); fsm_handle_event (&top_fsm, TOP_EVENT_start); */ @@ -338,6 +339,8 @@ main_loop (void) /* TODO: Check other sensors */ } + /* TODO: implement filterbridge events */ + /* Send Sharps raw stats. */ if (main_stats_sharps && !--main_stats_sharps_cpt) { -- cgit v1.2.3