summaryrefslogtreecommitdiff
path: root/digital/io/src/elevator_cb.c
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io/src/elevator_cb.c')
-rw-r--r--digital/io/src/elevator_cb.c164
1 files changed, 125 insertions, 39 deletions
diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c
index d6cf1005..025f947c 100644
--- a/digital/io/src/elevator_cb.c
+++ b/digital/io/src/elevator_cb.c
@@ -25,38 +25,65 @@
#include "common.h"
#include "fsm.h"
#include "elevator_cb.h"
+#include "elevator.h"
+#include "asserv.h"
+#include "pwm.h"
+
+/* Positions when waiting a puck*/
+uint16_t posx[4] = {0,0,0,0};
+/* Positions when we go to a target zone */
+uint16_t posy[3] = {0,0,0};
+
+/* increase/decrease of pos y */
+#define MAJ_POSY 100
+#define MIN_POSY 50
/*
- * OPEN_DOORS =doors_opened=>
- * => WAIT_FOR_CLOSE_ORDER
- * wait for close order
+ * IDLE =start=>
+ * => WAIT_JACK_IN
+ * waiting for jack
*/
fsm_branch_t
-elevator__OPEN_DOORS__doors_opened (void)
+elevator__IDLE__start (void)
{
- return elevator_next (OPEN_DOORS, doors_opened);
+ return elevator_next (IDLE, start);
}
/*
- * WAIT_FOR_CLOSE_ORDER =order_received=>
- * => CLOSE_DOORS
- * closing doors
+ * WAIT_JACK_IN =jack_inserted_into_bot=>
+ * => INIT
+ * make initializations
*/
fsm_branch_t
-elevator__WAIT_FOR_CLOSE_ORDER__order_received (void)
+elevator__WAIT_JACK_IN__jack_inserted_into_bot (void)
{
- return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received);
+ return elevator_next (WAIT_JACK_IN, jack_inserted_into_bot);
}
/*
- * WAIT_A_PUCK =time_up=>
- * => WAIT_POS_ORDER
- * no more time to wait a new puck
+ * INIT =init_done=>
+ * => GO_TO_POS_X
+ * match begin, we're going to be ready to get a new puck
*/
fsm_branch_t
-elevator__WAIT_A_PUCK__time_up (void)
+elevator__INIT__init_done (void)
{
- return elevator_next (WAIT_A_PUCK, time_up);
+ asserv_move_elevator_absolute(posx[nb_puck_in_elvt],
+ ASSERV_ELVT_SPEED_DEFAULT);
+ elevator_is_ready = 0;
+ return elevator_next (INIT, init_done);
+}
+
+/*
+ * GO_TO_POS_X =in_position=>
+ * => WAIT_A_PUCK
+ * in position and ready to get a new puck
+ */
+fsm_branch_t
+elevator__GO_TO_POS_X__in_position (void)
+{
+ elevator_is_ready = 1;
+ return elevator_next (GO_TO_POS_X, in_position);
}
/*
@@ -71,8 +98,36 @@ elevator__WAIT_A_PUCK__time_up (void)
fsm_branch_t
elevator__WAIT_A_PUCK__new_puck (void)
{
- return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks);
- return elevator_next_branch (WAIT_A_PUCK, new_puck, not_ok_for_other_pucks);
+ // TODO time_ok
+ //if(nb_puck_in_elvt < 4 && (time_ok || !fb-empty))
+ return elevator_next_branch (WAIT_A_PUCK, new_puck, ok_for_other_pucks);
+ //else
+ return elevator_next_branch (WAIT_A_PUCK, new_puck, not_ok_for_other_pucks);
+}
+
+/*
+ * WAIT_A_PUCK =time_up=>
+ * => WAIT_POS_ORDER
+ * no more time to wait a new puck
+ */
+fsm_branch_t
+elevator__WAIT_A_PUCK__time_up (void)
+{
+ return elevator_next (WAIT_A_PUCK, time_up);
+}
+
+/*
+ * WAIT_POS_ORDER =order_received=>
+ * => GO_TO_POS_Y
+ * go to position Y
+ */
+fsm_branch_t
+elevator__WAIT_POS_ORDER__order_received (void)
+{
+ asserv_move_elevator_absolute(posy[elvt_order] + MAJ_POSY,
+ ASSERV_ELVT_SPEED_DEFAULT);
+ elvt_order = 0;
+ return elevator_next (WAIT_POS_ORDER, order_received);
}
/*
@@ -87,58 +142,89 @@ elevator__GO_TO_POS_Y__in_position (void)
}
/*
- * GO_TO_POS_X =in_position=>
- * => WAIT_A_PUCK
- * in position and ready to get a new puck
+ * WAIT_FOR_RELEASE_ORDER =order_received=>
+ * => LAND_ELEVATOR
+ * make the elevator touch the target zone
*/
fsm_branch_t
-elevator__GO_TO_POS_X__in_position (void)
+elevator__WAIT_FOR_RELEASE_ORDER__order_received (void)
{
- return elevator_next (GO_TO_POS_X, in_position);
+ asserv_move_elevator_absolute(posy[elvt_order] - MIN_POSY,
+ ASSERV_ELVT_SPEED_DEFAULT);
+ return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received);
}
/*
- * IDLE =started=>
- * => GO_TO_POS_X
- * match begin, we're going to be ready to get a new puck
+ * LAND_ELEVATOR =in_position=>
+ * => OPEN_DOORS
+ * release pucks to the target position (I hope)
*/
fsm_branch_t
-elevator__IDLE__started (void)
+elevator__LAND_ELEVATOR__in_position (void)
{
- return elevator_next (IDLE, started);
+ pwm_set(OPEN_DOOR_PWM, TIME_DOORS_PWM);
+ return elevator_next (LAND_ELEVATOR, in_position);
}
/*
- * WAIT_FOR_RELEASE_ORDER =order_received=>
+ * MINI_CLOSE =door_move_finished=>
* => OPEN_DOORS
- * release pucks to the target position (I hope)
+ * try to release pucks again
*/
fsm_branch_t
-elevator__WAIT_FOR_RELEASE_ORDER__order_received (void)
+elevator__MINI_CLOSE__door_move_finished (void)
{
- return elevator_next (WAIT_FOR_RELEASE_ORDER, order_received);
+ pwm_set(OPEN_DOOR_PWM, TIME_DOORS_PWM);
+ return elevator_next (MINI_CLOSE, door_move_finished);
}
/*
- * WAIT_POS_ORDER =order_received=>
- * => GO_TO_POS_Y
- * go to position Y
+ * OPEN_DOORS =doors_opened=>
+ * => WAIT_FOR_CLOSE_ORDER
+ * wait for close order
*/
fsm_branch_t
-elevator__WAIT_POS_ORDER__order_received (void)
+elevator__OPEN_DOORS__doors_opened (void)
{
- return elevator_next (WAIT_POS_ORDER, order_received);
+ nb_puck_in_elvt = 0;
+ return elevator_next (OPEN_DOORS, doors_opened);
+}
+
+/*
+ * OPEN_DOORS =door_move_finished=>
+ * => MINI_CLOSE
+ * try to unblock doors
+ */
+fsm_branch_t
+elevator__OPEN_DOORS__door_move_finished (void)
+{
+ pwm_set(CLOSE_DOOR_PWM, TIME_LIGHT_DOORS_PWM);
+ return elevator_next (OPEN_DOORS, door_move_finished);
+}
+
+/*
+ * WAIT_FOR_CLOSE_ORDER =order_received=>
+ * => CLOSE_DOORS
+ * closing doors
+ */
+fsm_branch_t
+elevator__WAIT_FOR_CLOSE_ORDER__order_received (void)
+{
+ pwm_set(CLOSE_DOOR_PWM, TIME_DOORS_PWM);
+ return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received);
}
/*
- * CLOSE_DOORS =doors_closed=>
+ * CLOSE_DOORS =door_move_finished=>
* => GO_TO_POS_X
* pucks are released and elevator is ready to make a new column
*/
fsm_branch_t
-elevator__CLOSE_DOORS__doors_closed (void)
+elevator__CLOSE_DOORS__door_move_finished (void)
{
- return elevator_next (CLOSE_DOORS, doors_closed);
+ asserv_move_elevator_absolute(posx[nb_puck_in_elvt],
+ ASSERV_ELVT_SPEED_DEFAULT);
+ return elevator_next (CLOSE_DOORS, door_move_finished);
}