From 1dd17a56e081e19dd1f8de9ea2035a6f4d83d1db Mon Sep 17 00:00:00 2001 From: NĂ©lio Laranjeiro Date: Fri, 11 Apr 2008 00:43:24 +0200 Subject: Update the fsms. --- digital/io/src/getsamples_cb.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) (limited to 'digital/io/src/getsamples_cb.c') diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c index fb7d2be7..68268795 100644 --- a/digital/io/src/getsamples_cb.c +++ b/digital/io/src/getsamples_cb.c @@ -27,6 +27,8 @@ #include "getsamples_cb.h" #include "getsamples.h" #include "asserv.h" +#include "trap.h" +#include "move.h" /* * PREPARE_ARM =arm_moved=> @@ -36,6 +38,9 @@ fsm_branch_t getsamples__PREPARE_ARM__arm_moved (void) { + //Prepare the classifier. + getsamples_configure_classifier (); + // final asserv_go_to_distributor (); return getsamples_next (PREPARE_ARM, arm_moved); @@ -62,15 +67,16 @@ getsamples__FORWARD_CONTROL__position_reached (void) fsm_branch_t getsamples__START__ok (void) { - asserv_set_x_position (getsamples_data.distributor_x - 100); - asserv_set_y_position (getsamples_data.distributor_y - 100); + // move to the distributor. + move_start (getsamples_data.distributor_x - 100, + getsamples_data.distributor_y - 100); return getsamples_next (START, ok); } /* * TAKE_SAMPLES =sample_took=> * no_more => BACKWARD - * If the quantity of samples are tooked, then go backeward and conitnue classifying the samples. + * If the quantity of samples are taken, then go backward and continue classifying the samples. * more => TAKE_SAMPLES * Continue to take samples and classify the next sample. */ @@ -80,15 +86,15 @@ getsamples__TAKE_SAMPLES__sample_took (void) // Decrement the samples counter. if (getsamples_data.samples) { + getsamples_configure_classifier (); asserv_move_arm (1666, 100); return getsamples_next_branch (TAKE_SAMPLES, sample_took, more); } else { // Try to end the position to the distributor. - asserv_set_x_position (getsamples_data.distributor_x - 20); - // Go to the color distributor. - asserv_set_y_position (getsamples_data.distributor_y - 20); + asserv_goto (getsamples_data.distributor_x - 200, + getsamples_data.distributor_y - 200); return getsamples_next_branch (TAKE_SAMPLES, sample_took, no_more); } -- cgit v1.2.3