summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Haller2009-05-21 06:20:34 +0200
committerNicolas Haller2009-05-21 06:20:34 +0200
commit1f89ecb260b2915db08412c315d697c1eb7dd6d6 (patch)
tree836f1abf36aad70c53779b6f35e72b0e3f6835bb
parent7715de301fe6b1095bbd061893239c77ba089593 (diff)
* digital/io/src:
- add dropping pucks command before we have 4 pucks in elevator - fix top_puck_inside_bot assignation on elevator fsm
-rw-r--r--digital/io/src/elevator.fsm14
-rw-r--r--digital/io/src/elevator_cb.c33
-rw-r--r--digital/io/src/filterbridge.c1
-rw-r--r--digital/io/src/filterbridge.h3
-rw-r--r--digital/io/src/filterbridge_cb.c3
-rw-r--r--digital/io/src/main.c4
6 files changed, 44 insertions, 14 deletions
diff --git a/digital/io/src/elevator.fsm b/digital/io/src/elevator.fsm
index d9388690..8befad77 100644
--- a/digital/io/src/elevator.fsm
+++ b/digital/io/src/elevator.fsm
@@ -13,6 +13,8 @@ States:
go to a position to be ready to get a new puck
WAIT_A_PUCK
waiting for a new puck (hurry up filter bridge!)
+ WAIT_FB_IDLE
+ waiting for the fb is idle
WAIT_POS_ORDER
waiting for position order (depending where we want to drop our column)
GO_TO_POS_Y
@@ -40,8 +42,8 @@ Events:
elevator is in position
new_puck
we received a new puck from FB (filter bridge, not facebook)
- time_up
- complex event (!time_ok . fb_empty)
+ fb_idle
+ filterbridge idle
order_received
we received a new order
doors_opened
@@ -74,8 +76,12 @@ WAIT_A_PUCK:
new_puck: not_ok_for_other_pucks -> WAIT_POS_ORDER
incrementing nb_puck var
no more time to wait a new puck
- time_up -> WAIT_POS_ORDER
- no more time to wait a new puck
+ order_received -> WAIT_FB_IDLE
+ elevator filling has been shut, get ready to drop pucks
+
+WAIT_FB_IDLE:
+ fb_idle -> GO_TO_POS_Y
+ execute order
WAIT_POS_ORDER:
order_received -> GO_TO_POS_Y
diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c
index c37ef69c..caef50c2 100644
--- a/digital/io/src/elevator_cb.c
+++ b/digital/io/src/elevator_cb.c
@@ -144,16 +144,29 @@ elevator__WAIT_A_PUCK__new_puck (void)
}
/*
- * TODO time_up
- * WAIT_A_PUCK =time_up=>
- * => WAIT_POS_ORDER
- * no more time to wait a new puck
+ * WAIT_A_PUCK =order_received=>
+ * => WAIT_FB_IDLE
+ * elevator filling has been shut, get ready to drop pucks
*/
fsm_branch_t
-elevator__WAIT_A_PUCK__time_up (void)
+elevator__WAIT_A_PUCK__order_received (void)
{
elvt_is_ready = 0;
- return elevator_next (WAIT_A_PUCK, time_up);
+ return elevator_next (WAIT_A_PUCK, order_received);
+}
+
+/*
+ * WAIT_FB_IDLE =fb_idle=>
+ * => GO_TO_POS_Y
+ * execute order
+ */
+fsm_branch_t
+elevator__WAIT_FB_IDLE__fb_idle (void)
+{
+ elvt_new_puck = 0;
+ asserv_move_elevator_absolute(posy[elvt_order - 1] - MAJ_POSY,
+ ASSERV_ELVT_SPEED_DEFAULT);
+ return elevator_next (WAIT_FB_IDLE, fb_idle);
}
/*
@@ -166,7 +179,6 @@ elevator__WAIT_POS_ORDER__order_received (void)
{
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);
}
@@ -178,6 +190,7 @@ elevator__WAIT_POS_ORDER__order_received (void)
fsm_branch_t
elevator__GO_TO_POS_Y__in_position (void)
{
+ elvt_order = 0;
return elevator_next (GO_TO_POS_Y, in_position);
}
@@ -191,7 +204,6 @@ elevator__WAIT_FOR_RELEASE_ORDER__order_received (void)
{
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);
}
@@ -203,6 +215,7 @@ elevator__WAIT_FOR_RELEASE_ORDER__order_received (void)
fsm_branch_t
elevator__LAND_ELEVATOR__in_position (void)
{
+ elvt_order = 0;
pwm_set(OPEN_DOOR_PWM, 0);
return elevator_next (LAND_ELEVATOR, in_position);
}
@@ -227,8 +240,8 @@ elevator__MINI_CLOSE__state_timeout (void)
fsm_branch_t
elevator__OPEN_DOORS__doors_opened (void)
{
+ top_puck_inside_bot -= elvt_nb_puck;
elvt_nb_puck = 0;
- top_puck_inside_bot = 0;
pwm_set(0,0);
return elevator_next (OPEN_DOORS, doors_opened);
}
@@ -254,7 +267,6 @@ fsm_branch_t
elevator__WAIT_FOR_CLOSE_ORDER__order_received (void)
{
pwm_set(CLOSE_DOOR_PWM, 0);
- elvt_order = 0;
return elevator_next (WAIT_FOR_CLOSE_ORDER, order_received);
}
@@ -266,6 +278,7 @@ elevator__WAIT_FOR_CLOSE_ORDER__order_received (void)
fsm_branch_t
elevator__CLOSE_DOORS__state_timeout (void)
{
+ elvt_order = 0;
pwm_set(0,0);
asserv_move_elevator_absolute(posx[elvt_nb_puck],
ASSERV_ELVT_SPEED_DEFAULT);
diff --git a/digital/io/src/filterbridge.c b/digital/io/src/filterbridge.c
index 64fcbbfc..f61cd27d 100644
--- a/digital/io/src/filterbridge.c
+++ b/digital/io/src/filterbridge.c
@@ -25,6 +25,7 @@
#include "filterbridge.h"
uint8_t fb_nb_puck = 0;
+uint8_t fb_work_in_progress = 0;
/* +AutoDec */
/* -AutoDec */
diff --git a/digital/io/src/filterbridge.h b/digital/io/src/filterbridge.h
index 235d29ee..49f3819c 100644
--- a/digital/io/src/filterbridge.h
+++ b/digital/io/src/filterbridge.h
@@ -29,4 +29,7 @@
extern uint8_t fb_nb_puck;
+/* Is servo moving ? */
+extern uint8_t fb_work_in_progress;
+
#endif // filterbridge_h
diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c
index 9393a702..f8db2e2c 100644
--- a/digital/io/src/filterbridge_cb.c
+++ b/digital/io/src/filterbridge_cb.c
@@ -49,6 +49,7 @@ filterbridge__IDLE__start (void)
fsm_branch_t
filterbridge__WAIT_JACK_IN__jack_inserted_into_bot (void)
{
+ fb_work_in_progress = 1;
servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE);
servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE);
return filterbridge_next (WAIT_JACK_IN, jack_inserted_into_bot);
@@ -62,6 +63,7 @@ filterbridge__WAIT_JACK_IN__jack_inserted_into_bot (void)
fsm_branch_t
filterbridge__CLOSE_DOOR__state_timeout (void)
{
+ fb_work_in_progress = 0;
return filterbridge_next (CLOSE_DOOR, state_timeout);
}
@@ -91,6 +93,7 @@ filterbridge__WAIT_A_PUCK__puck_on_pos2 (void)
fsm_branch_t
filterbridge__MARCEL_WAIT__state_timeout (void)
{
+ fb_work_in_progress = 1;
servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_OPEN);
return filterbridge_next (MARCEL_WAIT, state_timeout);
}
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 2c11ba89..39df3357 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -240,6 +240,10 @@ main_event_to_fsm (void)
if(fb_nb_puck < 2)
FSM_HANDLE_EVENT (&cylinder_fsm,
CYLINDER_EVENT_bridge_ready);
+ if(!fb_work_in_progress)
+ FSM_HANDLE_EVENT (&elevator_fsm,
+ ELEVATOR_EVENT_fb_idle);
+
/* bot empty */
if(top_puck_inside_bot < 4)
FSM_HANDLE_EVENT (&cylinder_fsm,