From a87f059b7acdc9f00b2102249a647a51f8389de4 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Mon, 14 Apr 2008 12:00:59 +0200 Subject: * digital/io/src * asserv - improve the API of the arm functions to ease usage ; - add a new function to put the arm to the position to close the input hole. * get sample FSM - keep only the needed and real events ; - correctly names the states ; - improve the function to select where to put the samples ; * general - add headers for some standard configuration defines. * trap - add another entry to the enum to know its length. --- digital/io/src/asserv.c | 37 ++++++++++++- digital/io/src/asserv.h | 19 +++++-- digital/io/src/getsamples.c | 65 ++++++++++------------ digital/io/src/getsamples.fsm | 84 ++++++++++++++++++----------- digital/io/src/getsamples.h | 43 ++++++++++----- digital/io/src/getsamples_cb.c | 120 ++++++++++++++++++++++++----------------- digital/io/src/giboulee.h | 50 +++++++++++++++++ digital/io/src/playground.h | 96 +++++++++++++++++++++++++++++++++ digital/io/src/top_cb.c | 79 +++++++++++++++++---------- digital/io/src/trap.c | 2 + digital/io/src/trap.h | 2 + 11 files changed, 437 insertions(+), 160 deletions(-) create mode 100644 digital/io/src/giboulee.h create mode 100644 digital/io/src/playground.h (limited to 'digital/io/src') 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; /** -- cgit v1.2.3