From a367d0c7148a6de0c16aa8f9dabd597be01d3584 Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Sat, 9 May 2009 05:05:39 +0200 Subject: * digital/io/src: - fix init FSM (fix angle values) - implement filterbridge FSM --- digital/io/src/aquajim.h | 6 ++++++ digital/io/src/contact.c | 3 +-- digital/io/src/filterbridge.fsm | 10 ++++++++-- digital/io/src/filterbridge_cb.c | 17 ++++++++++++++--- digital/io/src/init_cb.c | 5 +++-- digital/io/src/main.c | 27 +++++++++++++++++++++++++-- digital/io/src/simu.host.h | 2 ++ 7 files changed, 59 insertions(+), 11 deletions(-) diff --git a/digital/io/src/aquajim.h b/digital/io/src/aquajim.h index 53d06567..a64fc4cd 100644 --- a/digital/io/src/aquajim.h +++ b/digital/io/src/aquajim.h @@ -45,4 +45,10 @@ #define SERVO_DOOR_OPEN 0 #define SERVO_DOOR_CLOSE 1 +/** + * How to compute a angle for giboulee? + * One degree is 65536 / 360 + */ +#define BOT_ANGLE_DEGREE (65536 / 360) + #endif /* aquajim_h */ diff --git a/digital/io/src/contact.c b/digital/io/src/contact.c index a9b408a3..3a26e962 100644 --- a/digital/io/src/contact.c +++ b/digital/io/src/contact.c @@ -22,14 +22,13 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ -#include "common.h" + #include "contact.h" #include "io.h" void contact_init (void) { - /* Pull-up. */ IO_SET (CONTACT_FILTER_BRIDGE_PUCK); IO_SET (CONTACT_ELEVATOR_DOOR); } diff --git a/digital/io/src/filterbridge.fsm b/digital/io/src/filterbridge.fsm index 2a87ab05..5722f5e2 100644 --- a/digital/io/src/filterbridge.fsm +++ b/digital/io/src/filterbridge.fsm @@ -11,6 +11,8 @@ States: close access to the lift (and wait 1 sec) WAIT_A_PUCK waiting for a new puck + MARCEL_WAIT[timeout=45] + waiting the puck is entirely in pos2 OPEN_DOOR[timeout=225] release puck to the lift and block a new puck on pos1 PUSH_PUCK @@ -43,8 +45,12 @@ CLOSE_DOOR: WAIT_A_PUCK: puck_on_pos2: lift_not_ready -> . Lift not ready, we stand by - puck_on_pos2: lift_ready -> OPEN_DOOR - Lift ok, we deliver + puck_on_pos2: lift_ready -> MARCEL_WAIT + wait the puck is entirely on pos2 + +MARCEL_WAIT: + state_timeout -> OPEN_DOOR + lift ready, we deliver OPEN_DOOR: state_timeout -> PUSH_PUCK diff --git a/digital/io/src/filterbridge_cb.c b/digital/io/src/filterbridge_cb.c index 12210416..00848d57 100644 --- a/digital/io/src/filterbridge_cb.c +++ b/digital/io/src/filterbridge_cb.c @@ -68,21 +68,32 @@ filterbridge__CLOSE_DOOR__state_timeout (void) * WAIT_A_PUCK =puck_on_pos2=> * lift_not_ready => WAIT_A_PUCK * Lift not ready, we stand by - * lift_ready => OPEN_DOOR - * Lift ok, we deliver + * lift_ready => MARCEL_WAIT + * wait the puck is entirely on pos2 */ fsm_branch_t filterbridge__WAIT_A_PUCK__puck_on_pos2 (void) { if(elevator_is_ready) { - servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_OPEN); return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, lift_ready); } else return filterbridge_next_branch (WAIT_A_PUCK, puck_on_pos2, lift_not_ready); } +/* + * MARCEL_WAIT =state_timeout=> + * => OPEN_DOOR + * lift ready, we deliver + */ +fsm_branch_t +filterbridge__MARCEL_WAIT__state_timeout (void) +{ + servo_pos_move_to(SERVO_DOOR_ID, SERVO_DOOR_OPEN); + return filterbridge_next (MARCEL_WAIT, state_timeout); +} + /* * OPEN_DOOR =state_timeout=> * => PUSH_PUCK diff --git a/digital/io/src/init_cb.c b/digital/io/src/init_cb.c index f05a5fef..70a964c1 100644 --- a/digital/io/src/init_cb.c +++ b/digital/io/src/init_cb.c @@ -29,6 +29,7 @@ #include "init.h" #include "playground.h" #include "main.h" +#include "aquajim.h" /* * GOTO_THE_WALL_AGAIN =move_done=> @@ -86,7 +87,7 @@ init__TURN_90_DEGREES_CCW__move_done (void) fsm_branch_t init__GO_BACKWARD__move_done (void) { - asserv_move_angularly(90); + asserv_move_angularly(90*BOT_ANGLE_DEGREE); return init_next (GO_BACKWARD, move_done); } @@ -136,7 +137,7 @@ init__WAIT_2_SEC__init_tempo_ended (void) fsm_branch_t init__GO_BACKWARD_AGAIN__move_done (void) { - asserv_move_angularly(180); + asserv_move_angularly(180*BOT_ANGLE_DEGREE); return init_next (GO_BACKWARD_AGAIN, move_done); } diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 33f6c838..32be6ac0 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -89,6 +89,8 @@ uint8_t main_always_stop_for_obstacle = 1; */ uint16_t main_move_wait_cycle; +/* FIXME to be delete */ +uint8_t jack_emulation = 0; /** * The same for init FSM */ @@ -150,8 +152,8 @@ main_init (void) fsm_init(&elevator_fsm); fsm_init(&filterbridge_fsm); /* Start all FSM (except move FSM) */ - fsm_handle_event (&top_fsm, TOP_EVENT_start); - fsm_handle_event (&init_fsm, INIT_EVENT_start); + //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 (&top_fsm, TOP_EVENT_start); fsm_handle_event (&top_fsm, TOP_EVENT_start); @@ -391,6 +393,24 @@ main_loop (void) --main_init_wait_cycle; if(!main_init_wait_cycle) FSM_HANDLE_EVENT (&init_fsm, INIT_EVENT_init_tempo_ended); + /* test filterbridge sensor */ + if(!IO_GET (CONTACT_FILTER_BRIDGE_PUCK)) + { + FSM_HANDLE_EVENT (&filterbridge_fsm, + FILTERBRIDGE_EVENT_puck_on_pos2); + } + else + { + FSM_HANDLE_EVENT (&filterbridge_fsm, + FILTERBRIDGE_EVENT_no_puck_on_pos2); + } + /* FIXME to be delete */ + if(jack_emulation) + { + FSM_HANDLE_EVENT (&filterbridge_fsm, + FILTERBRIDGE_EVENT_jack_inserted_into_bot); + } + } } @@ -406,6 +426,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) #define c(cmd, size) (cmd << 8 | size) switch (c (cmd, size)) { + case c ('j', 0): + jack_emulation = 1; + break; case c ('z', 0): /* Reset */ utils_reset (); diff --git a/digital/io/src/simu.host.h b/digital/io/src/simu.host.h index 214fce3d..f314db87 100644 --- a/digital/io/src/simu.host.h +++ b/digital/io/src/simu.host.h @@ -25,6 +25,8 @@ * * }}} */ +#include "common.h" + #ifdef HOST extern uint8_t PORTC, PINC; -- cgit v1.2.3