summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/cylinder_cb.c1
-rw-r--r--digital/io/src/elevator.fsm14
-rw-r--r--digital/io/src/elevator_cb.c47
-rw-r--r--digital/io/src/main.c8
4 files changed, 36 insertions, 34 deletions
diff --git a/digital/io/src/cylinder_cb.c b/digital/io/src/cylinder_cb.c
index f820f633..5694d95d 100644
--- a/digital/io/src/cylinder_cb.c
+++ b/digital/io/src/cylinder_cb.c
@@ -155,6 +155,7 @@ cylinder__TURN_PLUS_1_AND_OFO__move_done (void)
{
++top_total_puck_taken;
++top_puck_inside_bot;
+ ++cylinder_nb_puck;
}
if(top_puck_inside_bot < 4)
{
diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm
index 3bb6ed37..d9388690 100644
--- a/digital/io/src/elevator.fsm
+++ b/digital/io/src/elevator.fsm
@@ -21,13 +21,13 @@ States:
ready to open doors at your command
LAND_ELEVATOR
land elevator to the ground
- MINI_CLOSE
+ MINI_CLOSE[timeout=45]
try to unblock pucks
- OPEN_DOORS
+ OPEN_DOORS[timeout=675]
releasing pucks
WAIT_FOR_CLOSE_ORDER
ready to close doors at your command
- CLOSE_DOORS
+ CLOSE_DOORS[timeout=675]
closing the elevator's doors
@@ -46,8 +46,6 @@ Events:
we received a new order
doors_opened
doors are opened
- door_move_finished
- doors move is done
state_timeout
IDLE:
@@ -98,11 +96,11 @@ LAND_ELEVATOR:
OPEN_DOORS:
doors_opened -> WAIT_FOR_CLOSE_ORDER
wait for close order
- door_move_finished -> MINI_CLOSE
+ state_timeout -> MINI_CLOSE
try to unblock doors
MINI_CLOSE:
- door_move_finished -> OPEN_DOORS
+ state_timeout -> OPEN_DOORS
try to release pucks again
WAIT_FOR_CLOSE_ORDER:
@@ -110,5 +108,5 @@ WAIT_FOR_CLOSE_ORDER:
closing doors
CLOSE_DOORS:
- door_move_finished -> GO_TO_POS_X
+ state_timeout -> GO_TO_POS_X
pucks are released and elevator is ready to make a new column
diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c
index 471c46b9..c37ef69c 100644
--- a/digital/io/src/elevator_cb.c
+++ b/digital/io/src/elevator_cb.c
@@ -48,12 +48,9 @@ uint16_t posy[3] =
65 * ELEVATOR_MM_TO_STEP
};
-/* nb puck on the elevator */
-uint8_t nb_puck_elvt = 0;
-
/* increase/decrease of pos y */
-#define MAJ_POSY 100
-#define MIN_POSY 50
+#define MAJ_POSY (10 * ELEVATOR_MM_TO_STEP)
+#define MIN_POSY (5 * ELEVATOR_MM_TO_STEP)
/* time limit */
/* TODO to be define */
@@ -112,7 +109,7 @@ elevator__GO_TO_POS_X__in_position (void)
/* FIXME: this sucks, look at elevator.fsm for a real fix. */
elvt_is_ready = 1;
/* move to first position. */
- asserv_move_elevator_absolute(posx[nb_puck_elvt],
+ asserv_move_elevator_absolute(posx[elvt_nb_puck],
ASSERV_ELVT_SPEED_DEFAULT);
return elevator_next (GO_TO_POS_X, in_position);
}
@@ -132,13 +129,13 @@ elevator__WAIT_A_PUCK__new_puck (void)
elvt_is_ready = 0;
elvt_new_puck = 0;
// TODO time_ok
- if(++nb_puck_elvt < 4)
+ if(elvt_nb_puck < 4)
{
/* &&
((chrono_remaining_time() - OK_TIME_LIMIT > 0)
|| nb_puck_fb != 0))
*/
- asserv_move_elevator_absolute(posx[nb_puck_elvt],
+ asserv_move_elevator_absolute(posx[elvt_nb_puck],
ASSERV_ELVT_SPEED_DEFAULT);
return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks);
}
@@ -167,7 +164,7 @@ elevator__WAIT_A_PUCK__time_up (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_order - 1] - MAJ_POSY,
ASSERV_ELVT_SPEED_DEFAULT);
elvt_order = 0;
return elevator_next (WAIT_POS_ORDER, order_received);
@@ -192,7 +189,7 @@ 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_move_elevator_absolute(posy[elvt_order - 1] + MIN_POSY,
ASSERV_ELVT_SPEED_DEFAULT);
elvt_order = 0;
return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received);
@@ -211,15 +208,15 @@ elevator__LAND_ELEVATOR__in_position (void)
}
/*
- * MINI_CLOSE =door_move_finished=>
+ * MINI_CLOSE =state_timeout=>
* => OPEN_DOORS
* try to release pucks again
*/
fsm_branch_t
-elevator__MINI_CLOSE__door_move_finished (void)
+elevator__MINI_CLOSE__state_timeout (void)
{
pwm_set(OPEN_DOOR_PWM, 0);
- return elevator_next (MINI_CLOSE, door_move_finished);
+ return elevator_next (MINI_CLOSE, state_timeout);
}
/*
@@ -230,21 +227,22 @@ elevator__MINI_CLOSE__door_move_finished (void)
fsm_branch_t
elevator__OPEN_DOORS__doors_opened (void)
{
- nb_puck_elvt = 0;
+ elvt_nb_puck = 0;
top_puck_inside_bot = 0;
+ pwm_set(0,0);
return elevator_next (OPEN_DOORS, doors_opened);
}
/*
- * OPEN_DOORS =door_move_finished=>
+ * OPEN_DOORS =state_timeout=>
* => MINI_CLOSE
* try to unblock doors
*/
fsm_branch_t
-elevator__OPEN_DOORS__door_move_finished (void)
+elevator__OPEN_DOORS__state_timeout (void)
{
- pwm_set(CLOSE_DOOR_PWM, TIME_LIGHT_DOORS_PWM);
- return elevator_next (OPEN_DOORS, door_move_finished);
+ pwm_set(CLOSE_DOOR_PWM,0);
+ return elevator_next (OPEN_DOORS, state_timeout);
}
/*
@@ -255,22 +253,21 @@ elevator__OPEN_DOORS__door_move_finished (void)
fsm_branch_t
elevator__WAIT_FOR_CLOSE_ORDER__order_received (void)
{
- pwm_set(CLOSE_DOOR_PWM, TIME_DOORS_PWM);
+ pwm_set(CLOSE_DOOR_PWM, 0);
elvt_order = 0;
return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received);
}
/*
- * CLOSE_DOORS =door_move_finished=>
+ * CLOSE_DOORS =state_timeout=>
* => GO_TO_POS_X
* pucks are released and elevator is ready to make a new column
*/
fsm_branch_t
-elevator__CLOSE_DOORS__door_move_finished (void)
+elevator__CLOSE_DOORS__state_timeout (void)
{
- asserv_move_elevator_absolute(posx[nb_puck_elvt],
+ pwm_set(0,0);
+ asserv_move_elevator_absolute(posx[elvt_nb_puck],
ASSERV_ELVT_SPEED_DEFAULT);
- return elevator_next (CLOSE_DOORS, door_move_finished);
+ return elevator_next (CLOSE_DOORS, state_timeout);
}
-
-
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 0f31221a..20717222 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -55,6 +55,7 @@
#include "filterbridge.h"
#include "cylinder.h"
#include "init.h"
+#include "top.h"
#include "io.h"
@@ -244,14 +245,19 @@ main_event_to_fsm (void)
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_bridge_ready);
/* bot empty */
- if(fb_nb_puck + elvt_nb_puck + cylinder_nb_puck < 4)
+ if(top_puck_inside_bot < 4)
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_bot_not_full);
/* cylinder_close_order */
if(cylinder_close_order)
+ {
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_close_order);
+ }
+ else
+ FSM_HANDLE_EVENT (&cylinder_fsm,
+ CYLINDER_EVENT_no_close_order);
/* cylinder_flush_order */
if(cylinder_flush_order)