summaryrefslogtreecommitdiff
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/elevator.c71
-rw-r--r--digital/io/src/elevator.h30
-rw-r--r--digital/io/src/elevator_cb.c17
-rw-r--r--digital/io/src/main.c16
-rw-r--r--digital/io/src/top_cb.c8
5 files changed, 120 insertions, 22 deletions
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);
}