From 4ae03c6e14b75e62b9a8b446e6d7ae64c7d0b969 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Fri, 22 May 2009 13:33:28 +0200 Subject: * digital/io/src: - split elevator order into 3 functions --- digital/io/src/elevator.c | 71 +++++++++++++++++++++++++++++++++++++++++++- digital/io/src/elevator.h | 30 ++++++++++++++++++- digital/io/src/elevator_cb.c | 17 ++++++----- digital/io/src/main.c | 16 +++++----- digital/io/src/top_cb.c | 8 ++--- 5 files changed, 120 insertions(+), 22 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c index 6319501f..1843fb42 100644 --- a/digital/io/src/elevator.c +++ b/digital/io/src/elevator.c @@ -43,7 +43,76 @@ uint8_t elvt_new_puck = 0; /** * elevator orders */ -uint8_t elvt_order = 0; +elvt_order_e elvt_order_position = CLOSE; +uint8_t elvt_order_in_progress = 0; +uint8_t elvt_degraded_mode = 0; +uint8_t elvt_position_required = 0; + +/** + * We prepare the elevator +*/ +void +elvt_prepare(uint8_t pos) +{ + if(elvt_order_position == CLOSE) + { + elvt_order_in_progress = 1; + elvt_order_position = PREPARE; + elvt_position_required = pos; + } + else /* if we are already prepared, just say it's done */ + elvt_order_in_progress = 0; +} + +/** + * We open the elevator +*/ +void +elvt_open(uint8_t pos) +{ + if(elvt_order_position == PREPARE) + { + elvt_order_in_progress = 1; + elvt_order_position = OPEN; + elvt_position_required = pos; + elvt_degraded_mode = 0; + } + else /* We are already open, just say it's done and whistle */ + elvt_order_in_progress = 0; +} + +/** + * We open the elevator in degradad mode +*/ +void +elvt_open_degraded(uint8_t pos) +{ + if(elvt_order_position == PREPARE) + { + elvt_order_in_progress = 1; + elvt_order_position = OPEN; + elvt_position_required = pos; + elvt_degraded_mode = 1; + } + else /* We are already open, just say it's done and whistle */ + elvt_order_in_progress = 0; +} + + +/** + * We close the elevator and go away +*/ +void +elvt_close(void) +{ + if(elvt_order_position == OPEN) + { + elvt_order_in_progress = 1; + elvt_order_position = CLOSE; + } + else /* We are already close, just say it's done and play maracas */ + elvt_order_in_progress = 0; +} /* +AutoDec */ /* -AutoDec */ diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h index 6d131861..c8d3a780 100644 --- a/digital/io/src/elevator.h +++ b/digital/io/src/elevator.h @@ -43,7 +43,35 @@ extern uint8_t elvt_new_puck; /** * elevator orders */ -extern uint8_t elvt_order; +typedef enum elvt_order_e {CLOSE, PREPARE, OPEN} elvt_order_e; +extern elvt_order_e elvt_order_position; +extern uint8_t elvt_order_in_progress; +extern uint8_t elvt_degraded_mode; +extern uint8_t elvt_position_required; + +/** + * We prepare the elevator +*/ +void +elvt_prepare(uint8_t pos); + +/** + * We open the elevator +*/ +void +elvt_open(uint8_t pos); + +/** + * We open the elevator in degradad mode +*/ +void +elvt_open_degraded(uint8_t pos); + +/** + * We close the elevator and go away +*/ +void +elvt_close(void); /** * conversion stop/millimeter diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c index 32412d25..6d6c79c6 100644 --- a/digital/io/src/elevator_cb.c +++ b/digital/io/src/elevator_cb.c @@ -166,7 +166,7 @@ fsm_branch_t elevator__WAIT_BRIDGE_EMPTY__bridge_empty (void) { elvt_is_ready = 0; - asserv_move_elevator_absolute(posy[elvt_order - 1] - MAJ_POSY, + asserv_move_elevator_absolute(posy[elvt_position_required - 1] - MAJ_POSY, ASSERV_ELVT_SPEED_DEFAULT); return elevator_next (WAIT_BRIDGE_EMPTY, bridge_empty); } @@ -182,7 +182,7 @@ elevator__WAIT_BRIDGE_EMPTY__state_timeout (void) elvt_is_ready = 0; /* XXX bridge is empty but he don't know it yet */ fb_nb_puck = 0; - asserv_move_elevator_absolute(posy[elvt_order - 1] - MAJ_POSY, + asserv_move_elevator_absolute(posy[elvt_position_required - 1] - MAJ_POSY, ASSERV_ELVT_SPEED_DEFAULT); TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); @@ -208,7 +208,7 @@ elevator__WAIT_BRIDGE_EMPTY__new_puck (void) fsm_branch_t elevator__WAIT_POS_ORDER__order_received (void) { - asserv_move_elevator_absolute(posy[elvt_order - 1] - MAJ_POSY, + asserv_move_elevator_absolute(posy[elvt_position_required - 1] - MAJ_POSY, ASSERV_ELVT_SPEED_DEFAULT); return elevator_next (WAIT_POS_ORDER, order_received); } @@ -221,7 +221,7 @@ elevator__WAIT_POS_ORDER__order_received (void) fsm_branch_t elevator__GO_TO_POS_Y__in_position (void) { - elvt_order = 0; + elvt_order_in_progress = 0; return elevator_next (GO_TO_POS_Y, in_position); } @@ -233,8 +233,9 @@ elevator__GO_TO_POS_Y__in_position (void) fsm_branch_t elevator__WAIT_FOR_RELEASE_ORDER__order_received (void) { - asserv_move_elevator_absolute(posy[elvt_order - 1] + MIN_POSY, - ASSERV_ELVT_SPEED_DEFAULT); + uint16_t true_pos = elvt_degraded_mode?posy[elvt_position_required - 1]: + posy[elvt_position_required - 1] + MIN_POSY; + asserv_move_elevator_absolute(true_pos, ASSERV_ELVT_SPEED_DEFAULT); return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received); } @@ -273,7 +274,7 @@ elevator__OPEN_DOORS__doors_opened (void) top_puck_inside_bot -= elvt_nb_puck; elvt_nb_puck = 0; pwm_set(0,0); - elvt_order = 0; + elvt_order_in_progress = 0; TRACE (TRACE_FSM__NBPUCKS, top_total_puck_taken, top_puck_inside_bot, cylinder_nb_puck, fb_nb_puck, elvt_nb_puck); return elevator_next (OPEN_DOORS, doors_opened); @@ -311,7 +312,7 @@ elevator__WAIT_FOR_CLOSE_ORDER__order_received (void) fsm_branch_t elevator__CLOSE_DOORS__state_timeout (void) { - elvt_order = 0; + elvt_order_in_progress = 0; pwm_set(0,0); asserv_move_elevator_absolute(posx[elvt_nb_puck], ASSERV_ELVT_SPEED_DEFAULT); diff --git a/digital/io/src/main.c b/digital/io/src/main.c index dbe5eddd..ba47aac5 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -226,17 +226,20 @@ main_event_to_fsm (void) CYLINDER_EVENT_of_no_puck); - /* FIXME: use general setting ack. */ /* send event if elevator received an order */ - if (elvt_order) + if (elvt_order_in_progress) FSM_HANDLE_EVENT (&elevator_fsm, ELEVATOR_EVENT_order_received); else FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_elevator_order_done); + /* relou case to avoid a loop */ + if(elvt_order_in_progress && !elvt_new_puck) + FSM_HANDLE_EVENT (&elevator_fsm, + ELEVATOR_EVENT_order_bypass); + - /* FIXME: use general setting ack or not? */ /* elevator new puck (set by filterbridge) */ if(elvt_new_puck) FSM_HANDLE_EVENT (&elevator_fsm, @@ -249,10 +252,6 @@ main_event_to_fsm (void) if(fb_nb_puck < 2) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_bridge_ready); - if(elvt_order) - FSM_HANDLE_EVENT (&elevator_fsm, - ELEVATOR_EVENT_order_bypass); - if(cylinder_distributor_fucked) FSM_HANDLE_EVENT (&cylinder_fsm, CYLINDER_EVENT_distrib_fucked); @@ -566,7 +565,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) switch(args[0]) { case 'o': - elvt_order = args[1]; + elvt_order_in_progress = 1; + elvt_position_required = args[1]; break; } } diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 0530ae97..099ed3ae 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -331,7 +331,7 @@ fsm_branch_t top__GO_TO_UNLOAD_AREA__move_fsm_succeed (void) { /* Prepare elevator. */ - elvt_order = 3; + elvt_prepare(3); return top_next (GO_TO_UNLOAD_AREA, move_fsm_succeed); } @@ -365,7 +365,7 @@ fsm_branch_t top__FUCK_UNLOAD_AREA__bot_move_succeed (void) { /* Unload elevator. */ - elvt_order = 3; + elvt_open(3); return top_next (FUCK_UNLOAD_AREA, bot_move_succeed); } @@ -393,7 +393,7 @@ fsm_branch_t top__UNLOAD_PUCKS__elevator_order_done (void) { /* Close elevator. */ - elvt_order = 3; + elvt_close(); /* Move forward. */ asserv_move_linearly (PG_BORDER_DISTANCE); return top_next (UNLOAD_PUCKS, elevator_order_done); @@ -409,7 +409,7 @@ fsm_branch_t top__UNLOAD_PUCKS__state_timeout (void) { /* Elevator close. */ - elvt_order = 3; + elvt_close(); return top_next (UNLOAD_PUCKS, state_timeout); } -- cgit v1.2.3