From f5456f4aa1d4ec38fee4190cc8067b5e7de1a459 Mon Sep 17 00:00:00 2001 From: Julien Balleyguier Date: Sat, 12 May 2012 12:37:23 +0200 Subject: digital/io-hub/src/guybrush/ : Adding a position_to_drop_cd function This has been done to have a proper signal to tell the bottom clamp it has to open --- digital/io-hub/src/guybrush/bottom_clamp.c | 16 ++++++++++++---- digital/io-hub/src/guybrush/main.c | 10 ++++++++++ digital/io-hub/src/guybrush/main.h | 1 + 3 files changed, 23 insertions(+), 4 deletions(-) (limited to 'digital') diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c index 0e198804..fd160f17 100644 --- a/digital/io-hub/src/guybrush/bottom_clamp.c +++ b/digital/io-hub/src/guybrush/bottom_clamp.c @@ -109,6 +109,8 @@ FSM_EVENTS ( coin_detected, /*signal sent to the top fsm when taking a coin*/ taking_coin, + /*time to drop the cd. Sent by main*/ + time_to_drop_coin, /* Lower clamp rotation motor success. */ lower_clamp_rotation_success, /* Lower clamp rotation motor failure. */ @@ -143,6 +145,7 @@ FSM_START_WITH (CLAMP_START) /*------------------------------------- ROTATION DEFINITION ---------------------------------------*/ +#define POS_DELAY 1250 #define HIDE_POS 3 #define BACK_TO_READY 16-HIDE_POS #define HALF_TURN 8 @@ -258,11 +261,12 @@ FSM_TRANS (CLAMP_IDLE, coin_detected, CLAMP_TAKE_COIN) FSM_TRANS_TIMEOUT (CLAMP_TAKE_COIN, TIMEOUT_CLOSE_CLAMPS, CLAMP_TURN_HALF_WAY) { + main_set_drop_coin_pos(ctx.pos_current + (HALF_TURN * 250) - POS_DELAY); move_needed(HALF_TURN * 250); return FSM_NEXT_TIMEOUT (CLAMP_TAKE_COIN); } -FSM_TRANS_TIMEOUT (CLAMP_TURN_HALF_WAY, TIMEOUT_DROP_CD, CLAMP_DROP_CD) +FSM_TRANS (CLAMP_TURN_HALF_WAY, time_to_drop_coin, CLAMP_DROP_CD) { /*If the clamp 1 has the CD.*/ if (ctx.clamp_1_down) @@ -279,7 +283,8 @@ FSM_TRANS_TIMEOUT (CLAMP_TURN_HALF_WAY, TIMEOUT_DROP_CD, CLAMP_DROP_CD) /*Clamp 1 is now down (clamp 2 is up). */ ctx.clamp_1_down = 1; } - return FSM_NEXT_TIMEOUT (CLAMP_TURN_HALF_WAY); + return FSM_NEXT (CLAMP_TURN_HALF_WAY,time_to_drop_coin); +} } FSM_TRANS (CLAMP_DROP_CD,lower_clamp_rotation_success,CLAMP_IDLE) @@ -427,13 +432,16 @@ FSM_TRANS_TIMEOUT (CLAMP_OPEN_UPPER_CLAMPS, TIMEOUT_OPEN_CLAMPS, CLAMP_TURN_HALF IO_SET (OUTPUT_UPPER_CLAMP_OPEN); /*We reopen clamp 2.*/ IO_CLR (OUTPUT_LOWER_CLAMP_2_CLOSE); + if (ctx.stop_tree_approach) if (ctx.stop_tree_approach) { - move_needed(BACK_TO_READY_TREE * 250); + main_set_drop_coin_pos(ctx.pos_current + ((HALF_TURN - HIDE_POS_TREE) * 250) - POS_DELAY); + move_needed((HALF_TURN - HIDE_POS_TREE) * 250,SPEED_ROTATION); } else { - move_needed((BACK_TO_READY_TREE_2) * 250); + main_set_drop_coin_pos(ctx.pos_current + (BACK_TO_READY_TREE_2 * 250) - POS_DELAY); + move_needed((BACK_TO_READY_TREE_2) * 250,SPEED_ROTATION); } ctx.stop_tree_approach = 0; fsm_queue_post_event (FSM_EVENT (AI, clamps_ready)); diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c index 326cb423..8239b263 100644 --- a/digital/io-hub/src/guybrush/main.c +++ b/digital/io-hub/src/guybrush/main.c @@ -106,6 +106,9 @@ static uint8_t main_stats_clamp_zero_last_io_; /** Clamp zero stats: last position. */ static uint16_t main_stats_clamp_zero_last_position_; +/** Position to drop CD */ +static int position_to_drop; + /** Main initialisation. */ static void main_init (void) @@ -176,6 +179,8 @@ main_event_to_fsm (void) || !IO_GET (CONTACT_LOWER_CLAMP_SENSOR_3) || !IO_GET (CONTACT_LOWER_CLAMP_SENSOR_4)) FSM_HANDLE_E (AI, coin_detected); + if ((int16_t) (mimot_get_motor0_position() - position_to_drop) > 0) + FSM_HANDLE_E (AI, time_to_drop_coin); /* Jack. */ if (!contact_get_jack ()) FSM_HANDLE_E (AI, jack_inserted); @@ -395,6 +400,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) #undef c } +void main_set_drop_coin_pos(int pos_to_drop) +{ + position_to_drop = pos_to_drop; +} + int main (int argc, char **argv) { diff --git a/digital/io-hub/src/guybrush/main.h b/digital/io-hub/src/guybrush/main.h index 617096ff..1be4d3c7 100644 --- a/digital/io-hub/src/guybrush/main.h +++ b/digital/io-hub/src/guybrush/main.h @@ -27,5 +27,6 @@ extern vect_t main_obstacles_pos[2]; extern uint8_t main_obstacles_nb; +void main_set_drop_coin_pos(int); #endif /* main_h */ -- cgit v1.2.3