summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io/src/asserv.c37
-rw-r--r--digital/io/src/asserv.h19
-rw-r--r--digital/io/src/getsamples.c65
-rw-r--r--digital/io/src/getsamples.fsm84
-rw-r--r--digital/io/src/getsamples.h43
-rw-r--r--digital/io/src/getsamples_cb.c120
-rw-r--r--digital/io/src/giboulee.h50
-rw-r--r--digital/io/src/playground.h96
-rw-r--r--digital/io/src/top_cb.c79
-rw-r--r--digital/io/src/trap.c2
-rw-r--r--digital/io/src/trap.h2
11 files changed, 437 insertions, 160 deletions
diff --git a/digital/io/src/asserv.c b/digital/io/src/asserv.c
index 66f407a8..4302ce10 100644
--- a/digital/io/src/asserv.c
+++ b/digital/io/src/asserv.c
@@ -28,6 +28,7 @@
#include "modules/twi/twi.h" /* twi_* */
#include "modules/utils/byte.h" /* v*_to_v* */
+#include "giboulee.h" /* BOT_* */
#include "io.h"
/**
@@ -121,6 +122,21 @@ asserv_twi_send_command (uint8_t command, uint8_t length);
*/
static inline uint8_t
asserv_twi_send (uint8_t length);
+/**
+ * Move the arm.
+ * A complete rotation correspond to 5000 steps.
+ * @param position desired goal position (in step).
+ * @param speed speed of the movement.
+ */
+void
+asserv_move_arm_absolute (uint16_t position, uint8_t speed);
+
+/**
+ * Current position of the arm.
+ * We need to maintain it by ourself as it is more accurate than the one sent
+ * by the asserv board.
+ */
+static uint16_t asserv_arm_current_position;
/* Update TWI module until request (send or receive) is finished. */
static inline void
@@ -338,7 +354,7 @@ asserv_go_to_distributor (void)
/* Move the arm. */
void
-asserv_move_arm (uint16_t position, uint8_t speed)
+asserv_move_arm_absolute (uint16_t position, uint8_t speed)
{
/* Put position and speed as parameters */
asserv_twi_buffer_param[0] = v16_to_v8 (position, 1);
@@ -348,6 +364,25 @@ asserv_move_arm (uint16_t position, uint8_t speed)
asserv_twi_send_command ('b', 3);
}
+/* Move the arm to a certain number of steps. */
+void
+asserv_move_arm (int16_t offset, uint8_t speed)
+{
+ /* Compute the new desired position with the desired offset */
+ asserv_arm_current_position += offset;
+ /* Move the arm to the desired position */
+ 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)
diff --git a/digital/io/src/asserv.h b/digital/io/src/asserv.h
index ab81118a..e6bc06c4 100644
--- a/digital/io/src/asserv.h
+++ b/digital/io/src/asserv.h
@@ -191,14 +191,25 @@ void
asserv_go_to_distributor (void);
/**
- * Move the arm.
- * A complete rotation correspond to 5000 steps.
+ * Move the arm to a certain number of steps.
* Arm class command.
- * @param position desired goal position (in step).
+ * This function take the number of steps you want to move to. This is not an
+ * absolute position, this is only the number of steps you want to add to the
+ * current position.
+ * @param offset number of steps to add to the current position.
* @param speed speed of the movement.
*/
void
-asserv_move_arm (uint16_t position, uint8_t speed);
+asserv_move_arm (int16_t offset, uint8_t speed);
+
+/**
+ * Move the arm to close the input hole.
+ * It will compute the forward offset to close the input hole by moving the
+ * arm in front of it.
+ * @todo backward/forward selection support.
+ */
+void
+asserv_close_input_hole (void);
/**
* Set current X position.
diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c
index 96644a59..9feed1eb 100644
--- a/digital/io/src/getsamples.c
+++ b/digital/io/src/getsamples.c
@@ -27,52 +27,45 @@
#include "fsm.h"
#include "trap.h"
+#include "io.h"
+
+/**
+ * Get samples shared data.
+ */
struct getsamples_data_t getsamples_data;
-/** Start a getsamples FSM. */
+/* Start the get samples FSM. */
void
-getsamples_start (uint32_t distributor_x, uint32_t distributor_y,
- uint8_t samples, uint8_t event_to_post)
+getsamples_start (struct getsamples_data_t data)
{
- /* Set parameters. */
- getsamples_data.distributor_x = distributor_x;
- getsamples_data.distributor_y = distributor_y;
- getsamples_data.samples = samples;
- getsamples_data.event = event_to_post;
- /* Start FSM. */
+ /* Set parameters */
+ getsamples_data.distributor_x = data.distributor_x;
+ getsamples_data.distributor_y = data.distributor_y;
+ getsamples_data.sample_bitfield = data.sample_bitfield;
+ getsamples_data.event = data.event;
+
+ /* Start the get samples FSM */
fsm_init (&getsamples_fsm);
- fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_ok);
+ fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_start);
}
-/** Configure the classifier using the bit fields in the getsamples_data
- * structure.
- */
+/* Configure the classifier (using the trap and the internal bit field) for the first bit set to 1. */
void
getsamples_configure_classifier (void)
{
- switch (getsamples_data.samples)
+ uint8_t trap_num;
+ /* Go through all the bits of the sample bit field */
+ for (trap_num = 0; trap_num < trap_count; trap_num++)
{
- 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);
+ /* Is the bit set? */
+ if (bit_is_set (getsamples_data.sample_bitfield, trap_num))
+ {
+ /* Configure the classifier */
+ trap_setup_path_to_box (trap_num);
+ /* Reset this bit */
+ getsamples_data.sample_bitfield &= ~ _BV (trap_num);
+ /* Stop here */
+ return;
+ }
}
}
diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm
index daf1b738..4c833bc0 100644
--- a/digital/io/src/getsamples.fsm
+++ b/digital/io/src/getsamples.fsm
@@ -1,41 +1,63 @@
-#FSM get samples
+# Get samples FSM
get_samples
+ Get samples from the distributor
States:
- START
- GO_TO_POSITION
- PREPARE_ARM
- FORWARD_CONTROL
+ IDLE
+ waiting for the beginning of the get samples FSM
+ GO_IN_FRONT_OF_DISTRIBUTOR
+ move the bot in front of a distributor (not in contact with the distributor)
+ OPEN_INPUT_HOLE
+ move the arm to let the samples enter into the bot by the input hole
+ APPROACH_DISTRIBUTOR
+ make the bot move forward in order to be in contact with the distributor
TAKE_SAMPLES
- END
+ take one or more samples
+ MOVE_AWAY_FROM_DISTRIBUTOR
+ move the bot away from the distributor by going backward
+ CLOSE_INPUT_HOLE
+ put the arm back in front of the input hole to prevent sample to enter
Events:
- ok
- position_failed
- arm_moved
- sample_took
- classifier_ready
- position_reached
+ start
+ initialize the FSM
+ bot_move_failed
+ the bot movement failed (blocked by something for example)
+ bot_move_succeed
+ the bot has finished to moved and arrived at the desired position
+ arm_move_succeed
+ the arm has reached the desired position
+ arm_pass_noted_position
+ the arm has just passed the 'noted' position
+
+IDLE:
+ start -> GO_IN_FRONT_OF_DISTRIBUTOR
+ start going in front of the desired distributor
+
+GO_IN_FRONT_OF_DISTRIBUTOR:
+ bot_move_succeed -> OPEN_INPUT_HOLE
+ move the arm to open the input hole
+
+OPEN_INPUT_HOLE:
+ arm_move_succeed -> APPROACH_DISTRIBUTOR
+ start approaching the distributor now
+
+APPROACH_DISTRIBUTOR:
+ bot_move_succeed -> TAKE_SAMPLES
+ start taking some samples
-START:
- ok -> GO_TO_POSITION
- Go to the distributor. The distributor to reach shall be setted in the fsm structure.
-
-GO_TO_POSITION:
- position_reached -> PREPARE_ARM
- Go to the position desired, it is very near the position of the distributor in case it is a ice distributor or sample distributor.
-
-PREPARE_ARM:
- arm_moved -> FORWARD_CONTROL
- Prepare the arm to the correct position.
+TAKE_SAMPLES:
+ arm_pass_noted_position: more -> .
+ prepare the classification of the taken sample
+ take a new one
+ arm_pass_noted_position: no_more -> MOVE_AWAY_FROM_DISTRIBUTOR
+ go backward
-FORWARD_CONTROL:
- position_reached -> TAKE_SAMPLES
- End the position to the distributor.
+MOVE_AWAY_FROM_DISTRIBUTOR:
+ bot_move_succeed -> CLOSE_INPUT_HOLE
+ close input hole
-TAKE_SAMPLES:
- sample_took: more -> .
- Continue to take samples and classify the next sample.
- sample_took: no_more -> END
- If the quantity of samples are taken, then go backward and continue classifying the samples.
+CLOSE_INPUT_HOLE:
+ arm_move_succeed -> IDLE
+ tell the top FSM we have finished
diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h
index 6bfb3d63..c033665d 100644
--- a/digital/io/src/getsamples.h
+++ b/digital/io/src/getsamples.h
@@ -25,29 +25,48 @@
*
* }}} */
-/** getsamples FSM associated data. */
+/**
+ * Get samples FSM associated data.
+ */
struct getsamples_data_t
{
- /* Distributor x position to get samples. */
+ /**
+ * X position of the distributor where to get samples from.
+ */
uint32_t distributor_x;
- /* Distributor y position to get samples. */
+ /**
+ * Y position of the distributor where to get samples from.
+ */
uint32_t distributor_y;
- /* Samples to take. */
- uint8_t samples;
- /* event of the main fsm to post. */
+ /**
+ * Bit field to indicate where to put the sample.
+ * If bit 0 is set to 1, a sample will be put into the out_right_box. If
+ * set to 0, the out_right_box will not be used to store a sample.
+ */
+ uint8_t sample_bitfield;
+ /**
+ * Event to post to the top FSM when this one is finished.
+ */
uint8_t event;
};
-/** getsamples global. */
+/**
+ * Get samples shared data.
+ */
extern struct getsamples_data_t getsamples_data;
-/** Start a getsamples FSM. */
+/**
+ * Start the get samples FSM.
+ * @param data get sample data initial configuration.
+ */
void
-getsamples_start (uint32_t distributor_x, uint32_t distributor_y,
- uint8_t samples, uint8_t event_to_post);
+getsamples_start (struct getsamples_data_t data);
-/** Configure the classifier using the bit fields in the getsamples_data
- * structure.
+/**
+ * Configure the classifier (using the trap and the internal bit field) for
+ * the first bit set to 1.
+ * After the configuring the classifier, the bit will be reset to 0 to use the
+ * next one when calling this function again.
*/
void
getsamples_configure_classifier (void);
diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c
index 3c15fc42..0de105e4 100644
--- a/digital/io/src/getsamples_cb.c
+++ b/digital/io/src/getsamples_cb.c
@@ -30,90 +30,112 @@
#include "trap.h"
#include "move.h"
+#include "giboulee.h" /* BOT_ */
+#include "playground.h" /* PG_* */
+
/*
- * PREPARE_ARM =arm_moved=>
- * => FORWARD_CONTROL
- * Prepare the arm to the correct position.
+ * OPEN_INPUT_HOLE =arm_move_succeed=>
+ * => APPROACH_DISTRIBUTOR
+ * start approaching the distributor now
*/
fsm_branch_t
-getsamples__PREPARE_ARM__arm_moved (void)
+getsamples__OPEN_INPUT_HOLE__arm_move_succeed (void)
{
- //Prepare the classifier.
- getsamples_configure_classifier ();
-
- // final
- asserv_go_to_distributor ();
- return getsamples_next (PREPARE_ARM, arm_moved);
+ /* Approach the distributor */
+ asserv_go_to_distributor ();
+ return getsamples_next (OPEN_INPUT_HOLE, arm_move_succeed);
}
/*
- * FORWARD_CONTROL =position_reached=>
- * => TAKE_SAMPLES
- * End the position to the distributor.
+ * CLOSE_INPUT_HOLE =arm_move_succeed=>
+ * => IDLE
+ * tell the top FSM we have finished
*/
fsm_branch_t
-getsamples__FORWARD_CONTROL__position_reached (void)
+getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void)
{
- // Take as many samples as necessary. This shall be updated to verify the
- asserv_move_arm (1666, 100);
- return getsamples_next (FORWARD_CONTROL, position_reached);
+ /* Tell the top FSM we have finished */
+ fsm_handle_event (&top_fsm, getsamples_data.event);
+ return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed);
}
/*
- * START =ok=>
- * => GO_TO_POSITION
- * Go to the distributor. The distributor to reach shall be setted in the fsm structure.
+ * GO_IN_FRONT_OF_DISTRIBUTOR =bot_move_succeed=>
+ * => OPEN_INPUT_HOLE
+ * move the arm to open the input hole
*/
fsm_branch_t
-getsamples__START__ok (void)
+getsamples__GO_IN_FRONT_OF_DISTRIBUTOR__bot_move_succeed (void)
{
- // move to the distributor.
- move_start (getsamples_data.distributor_x - 100,
- getsamples_data.distributor_y - 100);
- return getsamples_next (START, ok);
+ /* Move the arm to open the input hole to be able to take some samples */
+ asserv_close_input_hole ();
+ return getsamples_next (GO_IN_FRONT_OF_DISTRIBUTOR, bot_move_succeed);
}
/*
- * TAKE_SAMPLES =sample_took=>
- * no_more => END
- * If the quantity of samples are taken, then go backward and continue classifying the samples.
+ * TAKE_SAMPLES =arm_pass_noted_position=>
+ * no_more => MOVE_AWAY_FROM_DISTRIBUTOR
+ * go backward
* more => TAKE_SAMPLES
- * Continue to take samples and classify the next sample.
+ * prepare the classification of the taken sample
+ * take a new one
*/
fsm_branch_t
-getsamples__TAKE_SAMPLES__sample_took (void)
+getsamples__TAKE_SAMPLES__arm_pass_noted_position (void)
{
- // Decrement the samples counter.
- if (getsamples_data.samples)
+ /* More samples? */
+ if (getsamples_data.sample_bitfield)
{
+ /* Prepare classification */
getsamples_configure_classifier ();
- asserv_move_arm (1666, 100);
- return getsamples_next_branch (TAKE_SAMPLES, sample_took, more);
+ /* 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
{
- // Try to end the position to the distributor.
- asserv_goto (getsamples_data.distributor_x - 200,
- getsamples_data.distributor_y - 200);
-
- fsm_handle_event (&top_fsm, getsamples_data.event);
- return getsamples_next_branch (TAKE_SAMPLES, sample_took, no_more);
+ /* Go backward */
+ asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR);
+ return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, no_more);
}
}
-
/*
- * GO_TO_POSITION =position_reached=>
- * => PREPARE_ARM
- * Go to the position desired, it is very near the position of the distributor in case it is a ice distributor or sample distributor.
+ * IDLE =start=>
+ * => GO_IN_FRONT_OF_DISTRIBUTOR
+ * start going in front of the desired distributor
*/
fsm_branch_t
-getsamples__GO_TO_POSITION__position_reached (void)
+getsamples__IDLE__start (void)
{
- // Put the ARM in the position to allow the robot to take samples from the
- // distributor.
- asserv_move_arm (625, 100);
- return getsamples_next (GO_TO_POSITION, position_reached);
+ /* Move to the desired distributor */
+ move_start (getsamples_data.distributor_x,
+ getsamples_data.distributor_y);
+ return getsamples_next (IDLE, start);
}
+/*
+ * MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=>
+ * => CLOSE_INPUT_HOLE
+ * close input hole
+ */
+fsm_branch_t
+getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void)
+{
+ /* Move the arm to close the input hole */
+ asserv_close_input_hole ();
+ return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_succeed);
+}
+/*
+ * APPROACH_DISTRIBUTOR =bot_move_succeed=>
+ * => TAKE_SAMPLES
+ * start taking some samples
+ */
+fsm_branch_t
+getsamples__APPROACH_DISTRIBUTOR__bot_move_succeed (void)
+{
+ /* start taking some samples */
+ asserv_move_arm (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
new file mode 100644
index 00000000..167addaa
--- /dev/null
+++ b/digital/io/src/giboulee.h
@@ -0,0 +1,50 @@
+#ifndef giboulee_h
+#define giboulee_h
+// giboulee.h
+// io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+//
+// Copyright (C) 2008 Dufour Jérémy
+//
+// Robot APB Team/Efrei 2004.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+
+/**
+ * @file Some defines for the bot itself.
+ * In this header file, you can find some configuration value for the length
+ * of a step when the bot is moving for example.
+ */
+
+/**
+ * The speed of the arm.
+ * It is expressed in number of steps by asserv board cycle (4.44ms).
+ */
+#define BOT_ARM_SPEED 3
+
+/**
+ * The number of step of a whole rotation.
+ */
+#define BOT_ARM_STEP_ROUND 5000
+
+/**
+ * Number of step to make a third of a round.
+ */
+#define BOT_ARM_THIRD_ROUND BOT_ARM_STEP_ROUND / 3
+
+#endif // giboulee_h
diff --git a/digital/io/src/playground.h b/digital/io/src/playground.h
new file mode 100644
index 00000000..c1b0ea78
--- /dev/null
+++ b/digital/io/src/playground.h
@@ -0,0 +1,96 @@
+#ifndef playground_h
+#define playground_h
+// playground.h
+// io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{
+//
+// Copyright (C) 2008 Dufour Jérémy
+//
+// Robot APB Team/Efrei 2004.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+
+/**
+ * @file Some defines for the playground.
+ * For example, you can find the size of the table, the positions of the
+ * distributors, ...
+ * A few important remarks:
+ * - the (0,0) position is the bottom left position on the table (when you
+ * see it with the two start zone at the top of the scheme).
+ */
+
+/**
+ * The width of the table, in millimeters.
+ */
+#define PG_WIDTH 3000
+
+/**
+ * The height of the table, in millimeters.
+ */
+#define PG_HEIGHT 2100
+
+/**
+ * Our color.
+ * XXX our_color = 0 = bleu, 1 = rouge
+ */
+extern uint8_t our_color;
+
+/**
+ * Considering there is a symmetry axis on X, this macro will compute the
+ * value for on the X axis depending on the color.
+ */
+#define PG_X_VALUE_COMPUTING(x) \
+ (our_color ? x : PG_WIDTH - x)
+
+/**
+ * The position where to reset the bot when it starts, depending on the color.
+ */
+#define PG_X_START (PG_X_VALUE_COMPUTING (200))
+#define PG_Y_START (PG_HEIGHT - 70)
+
+/**
+ * The distance to remove from the real position of the distributor before
+ * doing the "go to the distributor".
+ */
+#define PG_DISTANCE_DISTRIBUTOR 100
+
+/**
+ * The position of our ice distributor minus the distance to remove (only for the
+ * X axis).
+ */
+#define PG_DISTRIBUTOR_ICE_OUR_X \
+ (PG_X_VALUE_COMPUTING (0 + PG_DISTANCE_DISTRIBUTOR))
+#define PG_DISTRIBUTOR_ICE_OUR_Y 1350
+
+/**
+ * The position of adverse ice distributor minus the distance to remove (only
+ * for the X axis).
+ */
+#define PG_DISTRIBUTOR_ICE_ADVERSE_X \
+ (PG_X_VALUE_COMPUTING (PG_WIDTH - PG_DISTANCE_DISTRIBUTOR))
+#define PG_DISTRIBUTOR_ICE_ADVERSE_Y 1350
+
+/**
+ * The position of our distributor minus the distance to remove (only for the
+ * Y axis).
+ */
+#define PG_DISTRIBUTOR_SAMPLE_OUR_X \
+ (PG_X_VALUE_COMPUTING (700))
+#define PG_DISTRIBUTOR_SAMPLE_OUR_Y (PG_HEIGHT - PG_DISTANCE_DISTRIBUTOR)
+
+#endif // playground_h
diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c
index bb2f6c1d..ff45c290 100644
--- a/digital/io/src/top_cb.c
+++ b/digital/io/src/top_cb.c
@@ -34,15 +34,20 @@ top__BACKWARD__collector_full (void)
fsm_branch_t
top__BACKWARD__ended (void)
{
+ /* Generic get sample data */
+ struct getsamples_data_t get_sample_data;
+ get_sample_data.event = TOP_EVENT_ice_took;
+ get_sample_data.sample_bitfield = ~top_data.boxes_used; // XXX
+ get_sample_data.distributor_y = ICE_DISTRIBUTOR_Y;
if (top_data.team_color)
{
- getsamples_start ( ICE_DISTRIBUTOR_LEFT, ICE_DISTRIBUTOR_Y,
- ~top_data.boxes_used, TOP_EVENT_ice_took);
+ get_sample_data.distributor_x = ICE_DISTRIBUTOR_LEFT;
+ getsamples_start (get_sample_data);
}
else
{
- getsamples_start ( ICE_DISTRIBUTOR_RIGHT, ICE_DISTRIBUTOR_Y,
- ~top_data.boxes_used, TOP_EVENT_ice_took);
+ get_sample_data.distributor_x = ICE_DISTRIBUTOR_RIGHT;
+ getsamples_start (get_sample_data);
}
return top_next (BACKWARD, ended);
}
@@ -55,17 +60,21 @@ top__BACKWARD__ended (void)
fsm_branch_t
top__BACKWARD__ice_dist_empty (void)
{
+ /* Generic get sample data */
+ struct getsamples_data_t get_sample_data;
+ get_sample_data.event = TOP_EVENT_ice_took;
+ get_sample_data.sample_bitfield = ~top_data.boxes_used; // XXX
+ get_sample_data.distributor_y = ICE_DISTRIBUTOR_Y;
if (top_data.team_color)
{
- getsamples_start ( ICE_DISTRIBUTOR_RIGHT, ICE_DISTRIBUTOR_Y,
- ~top_data.boxes_used, TOP_EVENT_ice_took);
+ get_sample_data.distributor_x = ICE_DISTRIBUTOR_LEFT;
+ getsamples_start (get_sample_data);
}
else
{
- getsamples_start ( ICE_DISTRIBUTOR_LEFT, ICE_DISTRIBUTOR_Y,
- ~top_data.boxes_used, TOP_EVENT_ice_took);
+ get_sample_data.distributor_x = ICE_DISTRIBUTOR_RIGHT;
+ getsamples_start (get_sample_data);
}
-
return top_next (BACKWARD, ice_dist_empty);
}
@@ -93,17 +102,22 @@ top__GET_ICE__ice_took (void)
fsm_branch_t
top__START__ok (void)
{
+ /* Generic get sample data */
+ struct getsamples_data_t get_sample_data;
+ get_sample_data.event = TOP_EVENT_samples_took;
+ get_sample_data.sample_bitfield = ~top_data.boxes_used; // XXX
if (top_data.team_color == BLUE_TEAM)
{
- getsamples_start ( BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y ,
- ~top_data.boxes_used, TOP_EVENT_samples_took);
+ get_sample_data.distributor_x = BLUE_DISTRIBUTOR_X;
+ get_sample_data.distributor_y = BLUE_DISTRIBUTOR_Y;
+ getsamples_start (get_sample_data);
}
else
{
- getsamples_start ( RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y,
- ~top_data.boxes_used, TOP_EVENT_samples_took);
+ get_sample_data.distributor_x = RED_DISTRIBUTOR_X;
+ get_sample_data.distributor_y = RED_DISTRIBUTOR_Y;
+ getsamples_start (get_sample_data);
}
-
return top_next (START, ok);
}
@@ -115,15 +129,21 @@ top__START__ok (void)
fsm_branch_t
top__GO_TO_GOAL__samples_deposed (void)
{
+ /* Generic get sample data */
+ struct getsamples_data_t get_sample_data;
+ get_sample_data.event = TOP_EVENT_samples_took;
+ get_sample_data.sample_bitfield = ~top_data.boxes_used; // XXX
if (top_data.team_color == BLUE_TEAM)
{
- getsamples_start ( BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y ,
- ~top_data.boxes_used, TOP_EVENT_samples_took);
+ get_sample_data.distributor_x = BLUE_DISTRIBUTOR_X;
+ get_sample_data.distributor_y = BLUE_DISTRIBUTOR_Y;
+ getsamples_start (get_sample_data);
}
else
{
- getsamples_start ( RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y,
- ~top_data.boxes_used, TOP_EVENT_samples_took);
+ get_sample_data.distributor_x = RED_DISTRIBUTOR_X;
+ get_sample_data.distributor_y = RED_DISTRIBUTOR_Y;
+ getsamples_start (get_sample_data);
}
return top_next (GO_TO_GOAL, samples_deposed);
}
@@ -155,17 +175,22 @@ top__GET_SAMPLES__samples_took (void)
// Call the get samples state machine.
// Blue color.
+ /* Generic get sample data */
+ struct getsamples_data_t get_sample_data;
+ get_sample_data.event = TOP_EVENT_samples_took;
+ get_sample_data.sample_bitfield = top_data.sequence; // XXX
if (top_data.team_color == BLUE_TEAM)
- {
- getsamples_start (BLUE_DISTRIBUTOR_X, BLUE_DISTRIBUTOR_Y,
- top_data.sequence, TOP_EVENT_samples_took);
- }
+ {
+ get_sample_data.distributor_x = BLUE_DISTRIBUTOR_X;
+ get_sample_data.distributor_y = BLUE_DISTRIBUTOR_Y;
+ getsamples_start (get_sample_data);
+ }
else
- {
- getsamples_start (RED_DISTRIBUTOR_X, RED_DISTRIBUTOR_Y,
- top_data.sequence, TOP_EVENT_samples_took);
- }
-
+ {
+ get_sample_data.distributor_x = RED_DISTRIBUTOR_X;
+ get_sample_data.distributor_y = RED_DISTRIBUTOR_Y;
+ getsamples_start (get_sample_data);
+ }
return top_next (START, ok);
}
diff --git a/digital/io/src/trap.c b/digital/io/src/trap.c
index 61a667fc..b359b8af 100644
--- a/digital/io/src/trap.c
+++ b/digital/io/src/trap.c
@@ -113,6 +113,8 @@ trap_setup_path_to_box (trap_box_id_e box)
servo_set_high_time (3, trap_high_time_pos[horizontal][3]);
servo_set_high_time (4, trap_high_time_pos[horizontal][4]);
break;
+ default:
+ return;
}
}
diff --git a/digital/io/src/trap.h b/digital/io/src/trap.h
index bba780d8..d1bdd558 100644
--- a/digital/io/src/trap.h
+++ b/digital/io/src/trap.h
@@ -87,6 +87,8 @@ typedef enum trap_box_id_e
middle_right_box,
/** Most right box. */
out_right_box,
+ /** Count of trap boxes. */
+ trap_count,
} trap_box_id_e;
/**