From c2281f29a3f526612953cfa57aafc98d0cd874a8 Mon Sep 17 00:00:00 2001 From: Julien Balleyguier Date: Sun, 13 May 2012 13:04:41 +0200 Subject: digital/io-hub/src/guybrush/bottom_clamp.c adding the find_0 function --- digital/io-hub/src/guybrush/bottom_clamp.c | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 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 1af0a8ad..af477cf5 100644 --- a/digital/io-hub/src/guybrush/bottom_clamp.c +++ b/digital/io-hub/src/guybrush/bottom_clamp.c @@ -122,6 +122,8 @@ FSM_STATES ( ) FSM_EVENTS ( + /* O detected*/ + 0_found, /* coin detected in the clamp */ coin_detected, /*signal sent to the top fsm when taking a coin*/ @@ -252,19 +254,21 @@ FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPEN) } -FSM_TRANS_TIMEOUT (CLAMP_INIT_OPEN, TIMEOUT_OPEN_CLAMPS, CLAMP_INIT_FIND_0) +FSM_TRANS_TIMEOUT (CLAMP_INIT_OPEN, 5*TIMEOUT_OPEN_CLAMPS, CLAMP_INIT_FIND_0) { /*Findig the 0 position. */ - mimot_motor0_find_zero (0x05, 1, 0); - ctx.pos_current = 0; + move_needed(8000,SLOW_ROTATION); return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPEN); } -FSM_TRANS (CLAMP_INIT_FIND_0, lower_clamp_rotation_success, CLAMP_INIT_HIDE_CLAMP) +FSM_TRANS (CLAMP_INIT_FIND_0, 0_found, CLAMP_INIT_HIDE_CLAMP) { + /*init of the position*/ + ctx.pos_current = mimot_get_motor0_position(); + /* Hidding the clamp inside the robot. */ - move_needed(HIDE_POS * 250,SPEED_ROTATION); - return FSM_NEXT (CLAMP_INIT_FIND_0, lower_clamp_rotation_success); + move_needed(INIT_BOTTOM_POS * 250,FAST_ROTATION); + return FSM_NEXT (CLAMP_INIT_FIND_0, 0_found); } FSM_TRANS (CLAMP_INIT_HIDE_CLAMP, lower_clamp_rotation_success, -- cgit v1.2.3