From a07b9473d9c95904a9f0ff9e6bb5f9431d8735d2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 22 Apr 2008 21:57:24 +0200 Subject: * digital/io/src: - made the arm control work. --- digital/io/src/asserv.c | 14 +++----------- digital/io/src/getsamples.h | 2 ++ digital/io/src/getsamples_cb.c | 31 +++++++++++++++++++++---------- digital/io/src/giboulee.h | 4 ++-- digital/io/src/main.c | 4 ---- 5 files changed, 28 insertions(+), 27 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c index edf08a5d..9addd320 100644 --- a/digital/io/src/asserv.c +++ b/digital/io/src/asserv.c @@ -424,15 +424,6 @@ asserv_move_arm (int16_t offset, uint8_t speed) asserv_move_arm_absolute (asserv_arm_current_position, speed); } -/* Move the arm to close the input hole. */ -void -asserv_close_input_hole (void) -{ - /* Move the arm */ - asserv_move_arm (asserv_arm_current_position % - BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); -} - /* Set current X position. */ void asserv_set_x_position (int32_t x) @@ -551,9 +542,10 @@ uint8_t asserv_arm_position_reached (void) { /* If the position has been reached */ - /* TODO: manage overflow! */ if (asserv_arm_notify_position && - (asserv_get_arm_position () >= asserv_arm_notify_position)) + ((int16_t) (asserv_get_arm_position () + - asserv_arm_notify_position) >= 0)) return 1; return 0; } + diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h index 70a76696..e927397e 100644 --- a/digital/io/src/getsamples.h +++ b/digital/io/src/getsamples.h @@ -45,6 +45,8 @@ struct getsamples_data_t * distributor. */ uint8_t sample_bitfield; + /** Position of the arm where we want to be awken. */ + uint16_t arm_noted_position; }; diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index 3ade76a7..57f088eb 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -70,6 +70,17 @@ getsamples_configure_classifier (void) } } +/* Count the number of samples to take. */ +uint8_t +getsamples_count_samples (void) +{ + uint8_t i, bit, count = 0; + for (i = 0, bit = 1; i < 8; i++, bit <<= 1) + if (getsamples_data_.sample_bitfield & bit) + count++; + return count; +} + /* * FACE_DISTRIBUTOR =bot_move_succeed=> * => OPEN_INPUT_HOLE @@ -120,19 +131,15 @@ getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void) fsm_branch_t getsamples__TAKE_SAMPLES__arm_pass_noted_position (void) { + /* Prepare classification */ + getsamples_configure_classifier (); /* More samples? */ if (getsamples_data_.sample_bitfield) { /* Compute notifier */ - uint16_t arm_current_position = asserv_get_arm_position (); - uint16_t arm_notify_position = - arm_current_position + BOT_ARM_NOTED_POSITION - - (arm_current_position % BOT_ARM_THIRD_ROUND); - asserv_arm_set_position_reached (arm_notify_position); - /* Prepare classification */ - getsamples_configure_classifier (); + getsamples_data_.arm_noted_position += BOT_ARM_THIRD_ROUND; + asserv_arm_set_position_reached (getsamples_data_.arm_noted_position); /* Continue to take sample */ - asserv_move_arm (BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, more); } else @@ -165,7 +172,7 @@ fsm_branch_t getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) { /* Move the arm to close the input hole */ - asserv_close_input_hole (); + asserv_move_arm (BOT_ARM_MIN_TO_OPEN, BOT_ARM_SPEED); return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_succeed); } @@ -178,6 +185,10 @@ fsm_branch_t getsamples__APPROACH_DISTRIBUTOR__bot_move_succeed (void) { /* start taking some samples */ - asserv_move_arm (BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); + getsamples_data_.arm_noted_position = asserv_get_arm_position () + + BOT_ARM_NOTED_POSITION; + asserv_arm_set_position_reached (getsamples_data_.arm_noted_position); + asserv_move_arm (getsamples_count_samples () * BOT_ARM_THIRD_ROUND, + BOT_ARM_SPEED); return getsamples_next (APPROACH_DISTRIBUTOR, bot_move_succeed); } diff --git a/digital/io/src/giboulee.h b/digital/io/src/giboulee.h index d8f93ef0..0dae6098 100644 --- a/digital/io/src/giboulee.h +++ b/digital/io/src/giboulee.h @@ -50,9 +50,9 @@ /** * The number of steps to do to open the input hole when the arm is closing * it. - * For the moment, 15% of a complete turn + * For the moment, 30 degree. */ -#define BOT_ARM_MIN_TO_OPEN (BOT_ARM_STEP_ROUND * 15L / 100) +#define BOT_ARM_MIN_TO_OPEN (BOT_ARM_STEP_ROUND * 30L / 360) /** * Steps number relative to the beginning of a third round for the 'noted' diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 73b41196..883bb25d 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -256,10 +256,6 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Go to the distributor */ asserv_go_to_distributor (); break; - case 'c': - /* Close the input hole with the arm */ - asserv_close_input_hole (); - break; } } break; -- cgit v1.2.3