summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
authorNicolas Haller2009-05-15 00:36:58 +0200
committerNicolas Haller2009-05-15 00:36:58 +0200
commit78204f6bcba74ab5c5505ac06c3384e193177718 (patch)
tree04e23aa4753cc68be6c11762408f89cb2d409c67 /digital
parent473d501d1ead422ecc9680cb20e46a04a6356ed8 (diff)
* digital/io/src:
- work on elevator fsm
Diffstat (limited to 'digital')
-rw-r--r--digital/io/src/elevator.c6
-rw-r--r--digital/io/src/elevator.h6
-rw-r--r--digital/io/src/elevator_cb.c9
-rw-r--r--digital/io/src/filterbridge_cb.c1
-rw-r--r--digital/io/src/main.c34
5 files changed, 50 insertions, 6 deletions
diff --git a/digital/io/src/elevator.c b/digital/io/src/elevator.c
index c0eef588..d89ff7a1 100644
--- a/digital/io/src/elevator.c
+++ b/digital/io/src/elevator.c
@@ -28,12 +28,16 @@
/**
* State of the elevator
*/
-uint8_t elevator_is_ready;
+uint8_t elevator_is_ready = 0;
uint8_t nb_puck_in_elvt = 0;
/**
* elevator orders
*/
uint8_t elvt_order;
+/**
+ * new_puck information (from filterbridge)
+ */
+uint8_t elvt_new_puck = 0;
/* +AutoDec */
diff --git a/digital/io/src/elevator.h b/digital/io/src/elevator.h
index 9e31f488..69970e4c 100644
--- a/digital/io/src/elevator.h
+++ b/digital/io/src/elevator.h
@@ -36,9 +36,15 @@ extern uint8_t elevator_is_ready;
extern uint8_t nb_puck_in_elvt;
/**
+ * new_puck information (from filterbridge)
+ */
+extern uint8_t elvt_new_puck;
+
+/**
* elevator orders
*/
extern uint8_t elvt_order;
+
/**
* conversion stop/millimeter
*/
diff --git a/digital/io/src/elevator_cb.c b/digital/io/src/elevator_cb.c
index f1184cc8..d20496dc 100644
--- a/digital/io/src/elevator_cb.c
+++ b/digital/io/src/elevator_cb.c
@@ -32,16 +32,13 @@
#include "filterbridge.h"
/* Positions when waiting a puck*/
-uint16_t posx[4] = {0,0,0,0};
+uint16_t posx[4] = {0,25,55,85};
/* Positions when we go to a target zone */
-uint16_t posy[3] = {0,0,0};
+uint16_t posy[3] = {125,95,65};
/* nb puck on the elevator */
uint8_t nb_puck_elvt = 0;
-/* ready flag */
-uint8_t elevator_is_ready = 0;
-
/* increase/decrease of pos y */
#define MAJ_POSY 100
#define MIN_POSY 50
@@ -113,6 +110,7 @@ elevator__WAIT_A_PUCK__new_puck (void)
{
elevator_is_ready = 0;
++nb_puck_in_elvt;
+ elvt_new_puck = 0;
// TODO time_ok
if(nb_puck_elvt < 4 &&
((chrono_remaining_time() - OK_TIME_LIMIT > 0)
@@ -123,6 +121,7 @@ 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
diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c
index 00848d57..dd210e91 100644
--- a/digital/io/src/filterbridge_cb.c
+++ b/digital/io/src/filterbridge_cb.c
@@ -114,6 +114,7 @@ filterbridge__OPEN_DOOR__state_timeout (void)
fsm_branch_t
filterbridge__PUSH_PUCK__no_puck_on_pos2 (void)
{
+ elvt_new_puck = 1;
servo_pos_move_to(SERVO_FINGER_ID, SERVO_FINGER_IDLE);
servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_CLOSE);
return filterbridge_next (PUSH_PUCK, no_puck_on_pos2);
diff --git a/digital/io/src/main.c b/digital/io/src/main.c
index 32be6ac0..df111afc 100644
--- a/digital/io/src/main.c
+++ b/digital/io/src/main.c
@@ -48,6 +48,7 @@
#include "pwm.h"
#include "playground.h"
#include "contact.h"
+#include "elevator.h"
#include "io.h"
@@ -155,6 +156,7 @@ main_init (void)
//fsm_handle_event (&top_fsm, TOP_EVENT_start);
//fsm_handle_event (&init_fsm, INIT_EVENT_start);
fsm_handle_event (&filterbridge_fsm, FILTERBRIDGE_EVENT_start);
+ fsm_handle_event (&elevator_fsm, ELEVATOR_EVENT_start);
/* fsm_handle_event (&top_fsm, TOP_EVENT_start);
fsm_handle_event (&top_fsm, TOP_EVENT_start);
fsm_handle_event (&top_fsm, TOP_EVENT_start); */
@@ -277,6 +279,36 @@ main_loop (void)
FSM_HANDLE_EVENT (&move_fsm,
MOVE_EVENT_bot_move_failed);
}
+ /* elevator asserv status */
+ asserv_status_e elevator_status = asserv_last_cmd_ack()
+ ? asserv_elevator_cmd_status() : none;
+ if (elevator_status == success)
+ {
+ /* TODO le init init juste la position 0 du robal */
+ FSM_HANDLE_EVENT (&elevator_fsm,
+ ELEVATOR_EVENT_init_done);
+ FSM_HANDLE_EVENT (&elevator_fsm,
+ ELEVATOR_EVENT_in_position);
+ }
+ /*else if (elevator_status == failed)
+ {
+ TODO when asserv elevator can failed
+ }
+ */
+ /* send event if elevator received an order */
+ if(elvt_order)
+ FSM_HANDLE_EVENT (&elevator_fsm,
+ ELEVATOR_EVENT_order_received);
+
+
+ /* elevator new puck (set by filterbridge) */
+ if(elvt_new_puck)
+ FSM_HANDLE_EVENT (&elevator_fsm,
+ ELEVATOR_EVENT_new_puck);
+ /* elvt door switch */
+ if(!IO_GET (CONTACT_ELEVATOR_DOOR))
+ FSM_HANDLE_EVENT (&elevator_fsm,
+ ELEVATOR_EVENT_doors_opened);
/* Jack */
if(switch_get_jack())
{
@@ -289,6 +321,8 @@ main_loop (void)
TOP_EVENT_jack_inserted_into_bot);
FSM_HANDLE_EVENT (&init_fsm,
INIT_EVENT_jack_inserted_into_bot);
+ FSM_HANDLE_EVENT (&elevator_fsm,
+ ELEVATOR_EVENT_jack_inserted_into_bot);
}
/*
FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ?