summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/getsamples.c33
-rw-r--r--digital/io/src/getsamples.h6
-rw-r--r--digital/io/src/getsamples_cb.c18
3 files changed, 51 insertions, 6 deletions
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);
}