summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/asserv.c2
-rw-r--r--digital/io/src/asserv.h4
-rw-r--r--digital/io/src/elevator.c6
-rw-r--r--digital/io/src/elevator.fsm46
-rw-r--r--digital/io/src/elevator.h24
-rw-r--r--digital/io/src/elevator_cb.c164
6 files changed, 196 insertions, 50 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index bc973ec4..2bb6b515 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -671,7 +671,7 @@ asserv_arm_zero_position (void)
void
asserv_elevator_zero_position (void)
{
- asserv_twi_buffer_param[0] = ASSERV_ELVT_SPEED_DEFAULT;
+ asserv_twi_buffer_param[0] = -0x10;
asserv_twi_send_command ('C', 1);
}
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index 17b396da..f2e29886 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -38,8 +38,8 @@
* ignored.
*/
-#define ASSERV_ARM_SPEED_DEFAULT 0x16
-#define ASSERV_ELVT_SPEED_DEFAULT 0x30
+#define ASSERV_ARM_SPEED_DEFAULT 0x18
+#define ASSERV_ELVT_SPEED_DEFAULT 0x46
/**
* Initialize the asserv control module.
diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c
index 3a4a81ee..c0eef588 100644
--- a/digital/io/src/elevator.c
+++ b/digital/io/src/elevator.c
@@ -29,6 +29,12 @@
* State of the elevator
*/
uint8_t elevator_is_ready;
+uint8_t nb_puck_in_elvt = 0;
+/**
+ * elevator orders
+ */
+uint8_t elvt_order;
+
/* +AutoDec */
/* -AutoDec */
diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm
index b35ce04e..c57bedb8 100644
--- a/digital/io/src/elevator.fsm
+++ b/digital/io/src/elevator.fsm
@@ -5,6 +5,10 @@ elevator
States:
IDLE
waiting for the beginning of the round
+ WAIT_JACK_IN
+ waiting for the jack
+ INIT
+ init the elevator
GO_TO_POS_X
go to a position to be ready to get a new puck
WAIT_A_PUCK
@@ -12,9 +16,13 @@ States:
WAIT_POS_ORDER
waiting for position order (depending where we want to drop our column)
GO_TO_POS_Y
- go to a position to be ready to release pucks
+ go to a position + 1cm to be ready to release pucks
WAIT_FOR_RELEASE_ORDER
ready to open doors at your command
+ LAND_ELEVATOR
+ land elevator to the ground
+ MINI_CLOSE
+ try to unblock pucks
OPEN_DOORS
releasing pucks
WAIT_FOR_CLOSE_ORDER
@@ -23,8 +31,12 @@ States:
closing the elevator's doors
Events:
- started
- round had begun!
+ start
+ io power on!
+ jack_inserted_into_bot
+ jack into the bot
+ init_done
+ init done
in_position
elevator is in position
new_puck
@@ -35,12 +47,20 @@ Events:
we received a new order
doors_opened
doors are opened
- doors_closed
- doors are closed
+ door_move_finished
+ doors move is done
state_timeout
IDLE:
- started -> GO_TO_POS_X
+ start -> WAIT_JACK_IN
+ waiting for jack
+
+WAIT_JACK_IN:
+ jack_inserted_into_bot -> INIT
+ make initializations
+
+INIT:
+ init_done -> GO_TO_POS_X
match begin, we're going to be ready to get a new puck
GO_TO_POS_X:
@@ -66,17 +86,27 @@ GO_TO_POS_Y:
ready to release pucks at altitude Y
WAIT_FOR_RELEASE_ORDER:
- order_received -> OPEN_DOORS
+ order_received -> LAND_ELEVATOR
+ make the elevator touch the target zone
+
+LAND_ELEVATOR:
+ in_position -> OPEN_DOORS
release pucks to the target position (I hope)
OPEN_DOORS:
doors_opened -> WAIT_FOR_CLOSE_ORDER
wait for close order
+ door_move_finished -> MINI_CLOSE
+ try to unblock doors
+
+MINI_CLOSE:
+ door_move_finished -> OPEN_DOORS
+ try to release pucks again
WAIT_FOR_CLOSE_ORDER:
order_received -> CLOSE_DOORS
closing doors
CLOSE_DOORS:
- doors_closed -> GO_TO_POS_X
+ door_move_finished -> GO_TO_POS_X
pucks are released and elevator is ready to make a new column
diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h
index 0684559c..9e31f488 100644
--- a/digital/io/src/elevator.h
+++ b/digital/io/src/elevator.h
@@ -30,4 +30,28 @@
*/
extern uint8_t elevator_is_ready;
+/**
+ * nb puck in the elevator
+ */
+extern uint8_t nb_puck_in_elvt;
+
+/**
+ * elevator orders
+ */
+extern uint8_t elvt_order;
+/**
+ * conversion stop/millimeter
+ */
+
+#define STEP_BY_MM 72.34
+
+/**
+ * pwm constant for elevator doors
+ */
+
+#define OPEN_DOOR_PWM 200
+#define CLOSE_DOOR_PWM -200
+#define TIME_DOORS_PWM 0xcb
+#define TIME_LIGHT_DOORS_PWM 0xaa
+
#endif // elevator_h
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);
}