From 8522ab22baa8f0a93914398d97800c62d557cd20 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Sat, 16 May 2009 20:00:40 +0200 Subject: * digital/io/src: - fixes to elevator and filterbridge FSM --- digital/io/src/contact.h | 1 + digital/io/src/elevator_cb.c | 2 +- digital/io/src/filterbridge_cb.c | 1 + digital/io/src/main.c | 12 ++++++++++-- 4 files changed, 13 insertions(+), 3 deletions(-) (limited to 'digital/io/src') diff --git a/digital/io/src/contact.h b/digital/io/src/contact.h index 1e986f09..c752fc56 100644 --- a/digital/io/src/contact.h +++ b/digital/io/src/contact.h @@ -28,6 +28,7 @@ #define CONTACT_FILTER_BRIDGE_PUCK C, 0 #define CONTACT_ELEVATOR_DOOR C, 1 +#define CONTACT_PUCK_CYLINDER C, 2 /** * Initialize the contact module. diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index 2fc029d7..c1d7b926 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -109,7 +109,6 @@ fsm_branch_t elevator__WAIT_A_PUCK__new_puck (void) { elevator_is_ready = 0; - ++nb_puck_in_elvt; elvt_new_puck = 0; // TODO time_ok if(nb_puck_elvt < 4 && @@ -168,6 +167,7 @@ elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) { asserv_move_elevator_absolute(posy[elvt_order] - MIN_POSY, ASSERV_ELVT_SPEED_DEFAULT); + elvt_order = 0; return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received); } diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c index dd210e91..1692345d 100644 --- a/digital/io/src/filterbridge_cb.c +++ b/digital/io/src/filterbridge_cb.c @@ -114,6 +114,7 @@ filterbridge__OPEN_DOOR__state_timeout (void) fsm_branch_t filterbridge__PUSH_PUCK__no_puck_on_pos2 (void) { + ++nb_puck_in_elvt; elvt_new_puck = 1; servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE); servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE); diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 76bc66b9..feab6629 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -303,7 +303,7 @@ main_loop (void) FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_doors_opened); /* bridge ready */ - if(nb_puck_fb) + if(nb_puck_fb < 2) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bridge_ready); /* bot empty */ @@ -328,6 +328,8 @@ main_loop (void) ELEVATOR_EVENT_jack_inserted_into_bot); FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_jack_inserted_into_bot); + FSM_HANDLE_EVENT (&filterbridge_fsm, + FILTERBRIDGE_EVENT_jack_inserted_into_bot); } /* Settings acknowledge */ /* @@ -436,7 +438,13 @@ main_loop (void) FILTERBRIDGE_EVENT_no_puck_on_pos2); } /* test cylinder sensor */ - /* if(!IO_GET (CON */ + /* TODO check if we need !IO_GET or IO_GET */ + if(!IO_GET(CONTACT_PUCK_CYLINDER)) + { + FSM_HANDLE_EVENT (&cylinder_fsm, + CYLINDER_EVENT_new_puck); + } + /* FIXME to be delete */ if(cylinder_puck1_emulation) { FSM_HANDLE_EVENT (&cylinder_fsm, -- cgit v1.2.3