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.c | 33 +++++++++++++++++++++++++++++++++ digital/io/src/getsamples.h | 6 ++++++ digital/io/src/getsamples_cb.c | 18 ++++++++++++------ 3 files changed, 51 insertions(+), 6 deletions(-) (limited to 'digital') diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c index 73859fa3..b80881eb 100644 --- a/digital/io/src/getsamples.c +++ b/digital/io/src/getsamples.c @@ -25,6 +25,7 @@ #include "common.h" #include "getsamples.h" #include "fsm.h" +#include "trap.h" struct getsamples_data_t getsamples_data; @@ -42,3 +43,35 @@ getsamples_start (uint32_t distributor_x, uint32_t distributor_y, fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_ok); } +/** Configure the classifier using the bit fields in the getsamples_data + * structure. + */ +void +getsamples_configure_classifier (void) +{ + switch (getsamples_data.samples) + { + case 0x15: + trap_setup_path_to_box (out_left_box); + getsamples_data.samples &= 0xF; + break; + case 0xA: + trap_setup_path_to_box (middle_left_box); + getsamples_data.samples &= 0x7; + break; + case 0x5: + trap_setup_path_to_box (middle_box); + getsamples_data.samples &= 0x3; + break; + case 0x2: + trap_setup_path_to_box (middle_right_box); + getsamples_data.samples &= 0x1; + break; + case 0x1: + trap_setup_path_to_box (out_right_box); + getsamples_data.samples = 0x0; + break; + default: + trap_setup_path_to_box (garbage); + } +} diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h index ac2b68f3..883441d5 100644 --- a/digital/io/src/getsamples.h +++ b/digital/io/src/getsamples.h @@ -44,4 +44,10 @@ void getsamples_start (uint32_t distributor_x, uint32_t distributor_y, uint8_t samples); +/** Configure the classifier using the bit fields in the getsamples_data + * structure. + */ +void +getsamples_configure_classifier (void); + #endif /* getsamples_h */ 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