From 79456a7d1b2116fc8f50b82bd73b877cf8ef1bdf Mon Sep 17 00:00:00 2001 From: Nicolas Haller Date: Fri, 8 May 2009 10:13:09 +0200 Subject: * digital/io/src: - removed old FSM (gutter and getsamples) --- digital/io/src/fsm.h | 2 - digital/io/src/getsamples.c | 52 ------ digital/io/src/getsamples.fsm | 104 ----------- digital/io/src/getsamples.h | 67 ------- digital/io/src/getsamples_cb.c | 391 ----------------------------------------- digital/io/src/gutter.c | 49 ------ digital/io/src/gutter.fsm | 53 ------ digital/io/src/gutter.h | 47 ----- digital/io/src/gutter_cb.c | 134 -------------- digital/io/src/main.c | 74 +------- digital/io/src/top_cb.c | 3 - 11 files changed, 4 insertions(+), 972 deletions(-) delete mode 100644 digital/io/src/getsamples.c delete mode 100644 digital/io/src/getsamples.fsm delete mode 100644 digital/io/src/getsamples.h delete mode 100644 digital/io/src/getsamples_cb.c delete mode 100644 digital/io/src/gutter.c delete mode 100644 digital/io/src/gutter.fsm delete mode 100644 digital/io/src/gutter.h delete mode 100644 digital/io/src/gutter_cb.c (limited to 'digital') diff --git a/digital/io/src/fsm.h b/digital/io/src/fsm.h index 83f51b4b..8700cadc 100644 --- a/digital/io/src/fsm.h +++ b/digital/io/src/fsm.h @@ -97,8 +97,6 @@ fsm_init (fsm_t *fsm); uint8_t fsm_handle_event (fsm_t *fsm, u8 event); -#include "getsamples_fsm.h" -#include "gutter_fsm.h" #include "move_fsm.h" #include "top_fsm.h" #include "filterbridge_fsm.h" diff --git a/digital/io/src/getsamples.c b/digital/io/src/getsamples.c deleted file mode 100644 index c1a7dc60..00000000 --- a/digital/io/src/getsamples.c +++ /dev/null @@ -1,52 +0,0 @@ -/* getsamples.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * 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. - * - * }}} */ -#include "getsamples.h" -#include "fsm.h" -#include "trap.h" /* trap_* */ - -#include "io.h" /* _BV */ - -/** - * 'Private' get samples data used internaly by the FSM. - */ -struct getsamples_data_t getsamples_data_; - -/* Start the get samples FSM. */ -void -getsamples_start (int16_t approach_angle, uint8_t sample_bitfield, uint8_t - direction) -{ - /* Set parameters */ - getsamples_data_.approach_angle = approach_angle; - getsamples_data_.sample_bitfield = sample_bitfield; - getsamples_data_.direction = direction; - - /* Remove unhandled traps */ - getsamples_data_.sample_bitfield &= (_BV (trap_count) - 1) & ~_BV (garbage); - - /* Start the get samples FSM */ - fsm_init (&getsamples_fsm); - fsm_handle_event (&getsamples_fsm, GETSAMPLES_EVENT_start); -} diff --git a/digital/io/src/getsamples.fsm b/digital/io/src/getsamples.fsm deleted file mode 100644 index 6b10eec1..00000000 --- a/digital/io/src/getsamples.fsm +++ /dev/null @@ -1,104 +0,0 @@ -# Get samples FSM -# A general remark on this FSM: it is not its responsibility to put the bot in -# front of the distributor, it the responsibility of the top FSM using the -# move FSM. -# For example, the top FSM will order the bot to move in front of the -# distributor using the move FSM. Then, when it is arrived, it will the get -# sample FSM that will ensure we well placed (in term of angle for example). -get_samples - Get samples from the distributor - -States: - IDLE - waiting for the beginning of the get samples FSM - FACE_DISTRIBUTOR - face 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 - 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 - MOVE_BACKWARD_FROM_DISTRIBUTOR - move a little bit backward from the distributor - WAIT_AND_TRY_AGAIN - wait and try again - -Events: - start - initialize the FSM -# For the moment, this event is currently not managed. -# TODO: manage it - 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 - wait_finished - we have wait the desired time - -IDLE: - start -> FACE_DISTRIBUTOR - do a goto angle to make the bot facing the distributor - -FACE_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 -> MOVE_BACKWARD_FROM_DISTRIBUTOR - move a little bit backward from the distributor - bot_move_failed -> . - try to move again - -MOVE_BACKWARD_FROM_DISTRIBUTOR: - bot_move_succeed -> TAKE_SAMPLES - start taking some samples - setup a timeout - -TAKE_SAMPLES: - arm_pass_noted_position: more -> . - prepare the classification of the taken sample - take a new one - reset the timeout - arm_pass_noted_position: no_more -> MOVE_AWAY_FROM_DISTRIBUTOR - store current position - go backward - wait_finished -> MOVE_AWAY_FROM_DISTRIBUTOR - store current position - timed out, give up - go backward - -MOVE_AWAY_FROM_DISTRIBUTOR: - bot_move_succeed -> CLOSE_INPUT_HOLE - close input hole - setup a close timeout - bot_move_failed -> WAIT_AND_TRY_AGAIN - ask to be wake up in a certain time - arm_pass_noted_position -> . - prepare the classification of the taken sample - -CLOSE_INPUT_HOLE: - arm_move_succeed -> IDLE - tell the top FSM we have finished - wait_finished -> IDLE - timed out, give up - tell the top FSM we have finished - arm_pass_noted_position -> . - prepare the classification of the taken sample - -WAIT_AND_TRY_AGAIN: - wait_finished -> MOVE_AWAY_FROM_DISTRIBUTOR - compute remaining distance (protection agains 0 & 300) - try to move away again diff --git a/digital/io/src/getsamples.h b/digital/io/src/getsamples.h deleted file mode 100644 index 2adf0cb4..00000000 --- a/digital/io/src/getsamples.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef getsamples_h -#define getsamples_h -/* getsamples.h */ -/* {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * 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. - * - * }}} */ - -#include "common.h" - -/** - * Get samples FSM associated data. - */ -struct getsamples_data_t -{ - /** - * The angle to approach to face the distributor. - */ - int16_t approach_angle; - /** - * Bit-field to indicate where to put the collected samples. - * For example, if the bit 0 is set to 1, the sample took will be put into - * the box id 0 (out_right_box). Otherwise, if this bit is set to 0, the - * out_right box will not be use to store the sample. This parameter is - * also used to know the number of samples to collect from the - * distributor. - */ - uint8_t sample_bitfield; - /** Position of the arm where we want to be awken. */ - uint16_t arm_noted_position; - uint8_t direction; -}; - - -/** - * Start the get samples FSM. - * @param approach_angle the angle of approach to face the distributor - * @param sample_bitfield a bit-field to indicate where to put the collected - * samples. For example, if the bit 0 is set to 1, the sample took will be put - * into the box id 0 (out_right_box). Otherwise, if this bit is set to 0, the - * out_right box will not be use to store the sample. This parameter is also - * used to know the number of samples to collect from the distributor. - */ -void -getsamples_start (int16_t approach_angle, uint8_t sample_bitfield, uint8_t - direction); - -#endif /* getsamples_h */ diff --git a/digital/io/src/getsamples_cb.c b/digital/io/src/getsamples_cb.c deleted file mode 100644 index 86792887..00000000 --- a/digital/io/src/getsamples_cb.c +++ /dev/null @@ -1,391 +0,0 @@ -/* getsamples_cb.c - getsamples FSM callbacks. */ -/* {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * 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. - * - * }}} */ -#include "common.h" -#include "fsm.h" -#include "getsamples_cb.h" -#include "getsamples.h" -#include "asserv.h" -#include "trap.h" -#include "move.h" - -#include "giboulee.h" /* BOT_ */ -#include "playground.h" /* PG_* */ -#include "main.h" /* main_post_event_for_top_fsm */ -#include "chrono.h" - -#include "modules/proto/proto.h" -#include "modules/utils/utils.h" - -#include "io.h" - - -uint16_t originate_position_; - -/** - * The distance to go backward from the distributor. - */ -#define GET_SAMPLES_MOVE_BACKWARD_DISTANCE 7 - -/** - * Arm time out. - */ -#define GET_SAMPLES_ARM_TIMEOUT (5 * 225) - -/** - * Move back timeout. - */ -#define GET_SAMPLES_MOVE_AWAY_TIMEOUT (225) - -/** - * 'Private' get samples data used internaly by the FSM. - */ -extern struct getsamples_data_t getsamples_data_; - -/** - * 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); - -/* Configure the classifier (using the trap and the internal bit field) for the first bit set to 1. */ -void -getsamples_configure_classifier (void) -{ - uint8_t trap_num; - /* Go through all the bits of the sample bit field */ - for (trap_num = out_left_box; trap_num < trap_count; trap_num++) - { - /* 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; - } - } -} - -/* Count the number of samples to take. */ -uint8_t -getsamples_count_samples (void) -{ - uint8_t i, bit, count = 0; - for (i = 0, bit = 1; i < 8; i++, bit <<= 1) - if (getsamples_data_.sample_bitfield & bit) - count++; - return count; -} - -/* - * MOVE_BACKWARD_FROM_DISTRIBUTOR =bot_move_succeed=> - * => TAKE_SAMPLES - * start taking some samples - * setup a timeout - */ -fsm_branch_t -getsamples__MOVE_BACKWARD_FROM_DISTRIBUTOR__bot_move_succeed (void) -{ - /* start taking some samples */ - getsamples_data_.arm_noted_position = asserv_get_arm_position () + - BOT_ARM_NOTED_POSITION; - asserv_arm_set_position_reached (getsamples_data_.arm_noted_position); - asserv_move_arm (getsamples_count_samples () * BOT_ARM_THIRD_ROUND, - BOT_ARM_SPEED); - /* Post an event for the top FSM to be waked up later */ - main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT; - proto_send1b ('T', 0); - return getsamples_next (MOVE_BACKWARD_FROM_DISTRIBUTOR, bot_move_succeed); -} - -/* - * FACE_DISTRIBUTOR =bot_move_succeed=> - * => OPEN_INPUT_HOLE - * move the arm to open the input hole - */ -fsm_branch_t -getsamples__FACE_DISTRIBUTOR__bot_move_succeed (void) -{ - /* Move the arm to open the input hole to be able to take some samples */ - asserv_move_arm (-BOT_ARM_MIN_TO_OPEN, BOT_ARM_SPEED); - proto_send0 ('O'); - return getsamples_next (FACE_DISTRIBUTOR, bot_move_succeed); -} - -/* - * OPEN_INPUT_HOLE =arm_move_succeed=> - * => APPROACH_DISTRIBUTOR - * start approaching the distributor now - */ -fsm_branch_t -getsamples__OPEN_INPUT_HOLE__arm_move_succeed (void) -{ - /* Approach the distributor */ - asserv_go_to_distributor (); - proto_send0 ('A'); - return getsamples_next (OPEN_INPUT_HOLE, arm_move_succeed); -} - -/* - * CLOSE_INPUT_HOLE =wait_finished=> - * => IDLE - * timed out, give up - * tell the top FSM we have finished - */ -fsm_branch_t -getsamples__CLOSE_INPUT_HOLE__wait_finished (void) -{ - /* Give up, tell the top FSM we have finished */ - main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished; - proto_send0 ('I'); - return getsamples_next (CLOSE_INPUT_HOLE, wait_finished); -} - -/* - * CLOSE_INPUT_HOLE =arm_move_succeed=> - * => IDLE - * tell the top FSM we have finished - */ -fsm_branch_t -getsamples__CLOSE_INPUT_HOLE__arm_move_succeed (void) -{ - /* Tell the top FSM we have finished */ - main_post_event_for_top_fsm = TOP_EVENT_get_samples_fsm_finished; - return getsamples_next (CLOSE_INPUT_HOLE, arm_move_succeed); -} - -/* - * CLOSE_INPUT_HOLE =arm_pass_noted_position=> - * => CLOSE_INPUT_HOLE - * prepare the classification of the taken sample - */ -fsm_branch_t -getsamples__CLOSE_INPUT_HOLE__arm_pass_noted_position (void) -{ - /* Prepare the classification of the taken sample */ - getsamples_configure_classifier (); - return getsamples_next (CLOSE_INPUT_HOLE, arm_pass_noted_position); -} - -/* - * WAIT_AND_TRY_AGAIN =wait_finished=> - * => MOVE_AWAY_FROM_DISTRIBUTOR - * compute remaining distance (protection agains 0 & 300) - * try to move away again - */ -fsm_branch_t -getsamples__WAIT_AND_TRY_AGAIN__wait_finished (void) -{ - /* Get position */ - asserv_position_t position; - asserv_get_position (&position); - - uint16_t remaining_distance; - - if (getsamples_data_.direction == 0) - { - /* Horizontal */ - remaining_distance = UTILS_ABS (position.x - originate_position_); - } - else - { - /* Vertical */ - remaining_distance = UTILS_ABS (position.y - originate_position_); - } - /* Compute real remaining */ - remaining_distance = PG_DISTANCE_DISTRIBUTOR - remaining_distance; - /* Bound */ - UTILS_BOUND (remaining_distance, 1, PG_DISTANCE_DISTRIBUTOR); - /* Move away */ - asserv_move_linearly (-remaining_distance); - return getsamples_next (WAIT_AND_TRY_AGAIN, wait_finished); -} - -/* - * TAKE_SAMPLES =wait_finished=> - * => MOVE_AWAY_FROM_DISTRIBUTOR - * store current position - * timed out, give up - * go backward - */ -fsm_branch_t -getsamples__TAKE_SAMPLES__wait_finished (void) -{ - /* Get position */ - asserv_position_t position; - asserv_get_position (&position); - if (getsamples_data_.direction == 0) - { - /* Horizontal */ - originate_position_ = position.x; - } - else - { - /* Vertical */ - originate_position_ = position.y; - } - /* Go backward */ - asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR); - proto_send1b ('M', 1); - return getsamples_next (TAKE_SAMPLES, wait_finished); -} - -/* - * TAKE_SAMPLES =arm_pass_noted_position=> - * no_more => MOVE_AWAY_FROM_DISTRIBUTOR - * store current position - * go backward - * more => TAKE_SAMPLES - * prepare the classification of the taken sample - * take a new one - * reset the timeout - */ -fsm_branch_t -getsamples__TAKE_SAMPLES__arm_pass_noted_position (void) -{ - /* Prepare classification */ - getsamples_configure_classifier (); - /* More samples? */ - if (getsamples_data_.sample_bitfield && !(chrono_remaining_time () > 83000)) - { - /* Compute notifier */ - getsamples_data_.arm_noted_position += BOT_ARM_THIRD_ROUND; - asserv_arm_set_position_reached (getsamples_data_.arm_noted_position); - /* Post an event for the top FSM to be waked up later */ - main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT; - /* Continue to take sample */ - proto_send1b ('T', 1); - return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, more); - } - else - { - /* Get position */ - asserv_position_t position; - asserv_get_position (&position); - if (getsamples_data_.direction == 0) - { - /* Horizontal */ - originate_position_ = position.x; - } - else - { - /* Vertical */ - originate_position_ = position.y; - } - /* Go backward */ - asserv_move_linearly (-PG_DISTANCE_DISTRIBUTOR); - proto_send1b ('M', 0); - return getsamples_next_branch (TAKE_SAMPLES, arm_pass_noted_position, no_more); - } -} - -/* - * IDLE =start=> - * => FACE_DISTRIBUTOR - * do a goto angle to make the bot facing the distributor - */ -fsm_branch_t -getsamples__IDLE__start (void) -{ - /* Face the distributor */ - asserv_goto_angle (getsamples_data_.approach_angle); - proto_send0 ('F'); - return getsamples_next (IDLE, start); -} - -/* - * MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_failed=> - * => WAIT_AND_TRY_AGAIN - * ask to be wake up in a certain time - */ -fsm_branch_t -getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_failed (void) -{ - /* Post an event for the top FSM to be waked up later */ - main_getsamples_wait_cycle = GET_SAMPLES_MOVE_AWAY_TIMEOUT; - return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_failed); -} - -/* - * MOVE_AWAY_FROM_DISTRIBUTOR =bot_move_succeed=> - * => CLOSE_INPUT_HOLE - * close input hole - * setup a close timeout - */ -fsm_branch_t -getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__bot_move_succeed (void) -{ - /* Move the arm to close the input hole */ - asserv_move_arm (BOT_ARM_MIN_TO_OPEN + BOT_ARM_THIRD_ROUND, BOT_ARM_SPEED); - /* Post an event for the top FSM to be waked up later */ - main_getsamples_wait_cycle = GET_SAMPLES_ARM_TIMEOUT; - proto_send0 ('C'); - return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, bot_move_succeed); -} - -/* - * MOVE_AWAY_FROM_DISTRIBUTOR =arm_pass_noted_position=> - * => MOVE_AWAY_FROM_DISTRIBUTOR - * prepare the classification of the taken sample - */ -fsm_branch_t -getsamples__MOVE_AWAY_FROM_DISTRIBUTOR__arm_pass_noted_position (void) -{ - /* Prepare the classification of the taken sample */ - getsamples_configure_classifier (); - return getsamples_next (MOVE_AWAY_FROM_DISTRIBUTOR, arm_pass_noted_position); -} - -/* - * APPROACH_DISTRIBUTOR =bot_move_failed=> - * => APPROACH_DISTRIBUTOR - * try to move again - */ -fsm_branch_t -getsamples__APPROACH_DISTRIBUTOR__bot_move_failed (void) -{ - /* Approach the distributor */ - asserv_go_to_distributor (); - return getsamples_next (APPROACH_DISTRIBUTOR, bot_move_failed); -} - -/* - * APPROACH_DISTRIBUTOR =bot_move_succeed=> - * => MOVE_BACKWARD_FROM_DISTRIBUTOR - * move a little bit backward from the distributor - */ -fsm_branch_t -getsamples__APPROACH_DISTRIBUTOR__bot_move_succeed (void) -{ - /* Move a little bit backward from the distributor */ - asserv_move_linearly (-GET_SAMPLES_MOVE_BACKWARD_DISTANCE); - proto_send0 ('m'); - return getsamples_next (APPROACH_DISTRIBUTOR, bot_move_succeed); -} diff --git a/digital/io/src/gutter.c b/digital/io/src/gutter.c deleted file mode 100644 index c93a872c..00000000 --- a/digital/io/src/gutter.c +++ /dev/null @@ -1,49 +0,0 @@ -/* gutter.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * 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. - * - * }}} */ -#include "common.h" -#include "gutter.h" -#include "fsm.h" - -/* Number of cycles to wait. */ -extern uint16_t gutter_wait_cycle_; - -/* Start the gutter FSM. */ -void -gutter_start (void) -{ - /* Start the FSM. */ - fsm_init (&gutter_fsm); - fsm_handle_event (&gutter_fsm, GUTTER_EVENT_start); -} - -/* Do we need to generate a wait_finished event? */ -uint8_t -gutter_generate_wait_finished_event (void) -{ - if (gutter_wait_cycle_) - return !(--gutter_wait_cycle_); - else - return 0; -} diff --git a/digital/io/src/gutter.fsm b/digital/io/src/gutter.fsm deleted file mode 100644 index 97163cc0..00000000 --- a/digital/io/src/gutter.fsm +++ /dev/null @@ -1,53 +0,0 @@ -# Gutter FSM -# Drop balls from the inside of the bot into the gutter to score points. -gutter - -States: - IDLE - waiting for the beginning of the gutter FSM - ROTATE_REAR_SIDE_TO_GUTTER - stand back to the gutter - GO_TO_THE_GUTTER_WALL - make the bot reversing against the gutter - DROP_BALLS - open the collector to let the balls fall into the gutter - WAIT_AND_TRY_AGAIN - ask the top FSM to wake us up in a few times - -Events: - start - initialize the FSM - bot_move_succeed - the bot has finished to moved successfully - bot_move_failed - the bot movement failed (blocked by something for example) - wait_finished - we have wait the desired time - -IDLE: - start -> ROTATE_REAR_SIDE_TO_GUTTER - put the bot back to the gutter - -ROTATE_REAR_SIDE_TO_GUTTER: - bot_move_succeed -> GO_TO_THE_GUTTER_WALL - make the bot reversing against the gutter - -GO_TO_THE_GUTTER_WALL: - bot_move_succeed -> DROP_BALLS - open the collector to drop the balls - wait for a while -# TODO: handle the case where a ball is between the bot and the border, -# preventing from fucking the wall -# The best way is probably to abort this FSM with an error, tell the upper one -# (top) we need to try at another place (the gutter is quite long). - bot_move_failed -> WAIT_AND_TRY_AGAIN - ask the top FSM to wake us up in a few times - -DROP_BALLS: - wait_finished -> IDLE - close the rear panel - tell the top FSM we have finished - -WAIT_AND_TRY_AGAIN: - wait_finished -> GO_TO_THE_GUTTER_WALL - try the fuck the wall again diff --git a/digital/io/src/gutter.h b/digital/io/src/gutter.h deleted file mode 100644 index c84a9e40..00000000 --- a/digital/io/src/gutter.h +++ /dev/null @@ -1,47 +0,0 @@ -#ifndef gutter_h -#define gutter_h -/* gutter.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * 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. - * - * }}} */ - -/** - * Start the gutter FSM. - */ -void -gutter_start (void); - -/** - * Do we need to generate a wait_finished event? - * You need to call this function in the main loop in order to ensure we - * generate a wait_finished event when the gutter FSM need one. - * The purpose is to let the gutter FSM wait a specific number of cycle it - * wants. - * @return - * - 0 if you do need to generate a wait_finished event - * - 1 if you need to generate a wait_finished event for the gutter FSM. - */ -uint8_t -gutter_generate_wait_finished_event (void); - -#endif /* gutter_h */ diff --git a/digital/io/src/gutter_cb.c b/digital/io/src/gutter_cb.c deleted file mode 100644 index 96f5c605..00000000 --- a/digital/io/src/gutter_cb.c +++ /dev/null @@ -1,134 +0,0 @@ -/* gutter_cb.c - gutter FSM callbacks. */ -/* {{{ - * - * Copyright (C) 2008 Nélio Laranjeiro - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT org - * - * 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. - * - * }}} */ -#include "common.h" -#include "fsm.h" -#include "gutter_cb.h" - -#include "asserv.h" /* asserv_go_to_the_wall */ -#include "trap.h" /* trap_* */ -#include "playground.h" /* PG_GUTTER_A */ -#include "main.h" /* main_post_event_for_top_fsm */ - -/** - * Gutter private data to wait a certain number of cycles. - */ -uint16_t gutter_wait_cycle_; - -/** - * Count of cycles to wait before we estimate all the balls have been dropped - * into the gutter. A cycle normally last 4.4ms. - */ -#define GUTTER_WAIT_FOR_BALLS_TO_DROP 300 - -/** - * Time to wait before trying the go to the wall again. - */ -#define GUTTER_WAIT_BEFORE_TRY_AGAIN (225) - -/* - * ROTATE_REAR_SIDE_TO_GUTTER =bot_move_succeed=> - * => GO_TO_THE_GUTTER_WALL - * make the bot reversing against the gutter - */ -fsm_branch_t -gutter__ROTATE_REAR_SIDE_TO_GUTTER__bot_move_succeed (void) -{ - /* Make the bot reversing against the gutter */ - asserv_go_to_the_wall (); - return gutter_next (ROTATE_REAR_SIDE_TO_GUTTER, bot_move_succeed); -} - -/* - * WAIT_AND_TRY_AGAIN =wait_finished=> - * => GO_TO_THE_GUTTER_WALL - * try the fuck the wall again - */ -fsm_branch_t -gutter__WAIT_AND_TRY_AGAIN__wait_finished (void) -{ - /* Make the bot reversing against the gutter */ - asserv_go_to_the_wall (); - return gutter_next (WAIT_AND_TRY_AGAIN, wait_finished); -} - -/* - * IDLE =start=> - * => ROTATE_REAR_SIDE_TO_GUTTER - * put the bot back to the gutter - */ -fsm_branch_t -gutter__IDLE__start (void) -{ - /* Initialize internal data */ - gutter_wait_cycle_ = 0; - /* Put the bot back to the gutter */ - asserv_goto_angle (PG_GUTTER_A); - return gutter_next (IDLE, start); -} - -/* - * GO_TO_THE_GUTTER_WALL =bot_move_failed=> - * => WAIT_AND_TRY_AGAIN - * ask the top FSM to wake us up in a few times - */ -fsm_branch_t -gutter__GO_TO_THE_GUTTER_WALL__bot_move_failed (void) -{ - main_getsamples_wait_cycle = GUTTER_WAIT_BEFORE_TRY_AGAIN; - return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_failed); -} - -/* - * GO_TO_THE_GUTTER_WALL =bot_move_succeed=> - * => DROP_BALLS - * open the collector to drop the balls - * wait for a while - */ -fsm_branch_t -gutter__GO_TO_THE_GUTTER_WALL__bot_move_succeed (void) -{ - /* Open the rear panel */ - trap_open_rear_panel (); - /* Wait for GUTTER_WAIT_FOR_BALLS_TO_DROP before being calling back by the - * main loop */ - gutter_wait_cycle_ = GUTTER_WAIT_FOR_BALLS_TO_DROP; - return gutter_next (GO_TO_THE_GUTTER_WALL, bot_move_succeed); -} - -/* - * DROP_BALLS =wait_finished=> - * => IDLE - * close the rear panel - * tell the top FSM we have finished - */ -fsm_branch_t -gutter__DROP_BALLS__wait_finished (void) -{ - /* Close the rear panel */ - trap_close_rear_panel (); - /* Tell the top FSM we have finished */ - main_post_event_for_top_fsm = TOP_EVENT_gutter_fsm_finished; - return gutter_next (DROP_BALLS, wait_finished); -} diff --git a/digital/io/src/main.c b/digital/io/src/main.c index c623eea5..a7c9030d 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -42,10 +42,8 @@ #include "trap.h" /* Trap module (trap_* functions) */ #include "fsm.h" /* fsm_* */ #include "giboulee.h" /* team_color */ -#include "getsamples.h" /* getsamples_start */ /* #include "top.h" */ /* top_* */ #include "chrono.h" /* chrono_end_match */ -#include "gutter.h" /* gutter_generate_wait_finished_event */ #include "sharp.h" /* sharp module */ #include "pwm.h" #include "playground.h" @@ -90,11 +88,6 @@ uint8_t main_always_stop_for_obstacle = 1; */ uint16_t main_move_wait_cycle; -/** - * Get samples timeout. - */ -uint16_t main_getsamples_wait_cycle; - /** * Sharps stats counters. */ @@ -224,9 +217,6 @@ main_loop (void) /* Update wait flag for move FSM */ if (main_move_wait_cycle) main_move_wait_cycle--; - /* Update wait flag for getsamples FSM */ - if (main_getsamples_wait_cycle) - main_getsamples_wait_cycle--; /* Update sharp module if required and only every * MAIN_SHARP_UPDATE_FREQ cycles */ if (++main_sharp_freq_counter_ == MAIN_SHARP_UPDATE_FREQ) @@ -238,11 +228,6 @@ main_loop (void) } /* Update main */ - uint8_t main_asserv_arm_position_reached = asserv_arm_position_reached (); - /* - uint8_t main_top_generate_settings_ack_event = top_generate_settings_ack_event (); - uint8_t main_gutter_generate_wait_finished_event = gutter_generate_wait_finished_event (); - */ asserv_status_e move_status = asserv_last_cmd_ack () ? asserv_move_cmd_status () : none; @@ -250,59 +235,19 @@ main_loop (void) if (move_status == success) { /* Pass it to all the FSM that need it */ - /* - FSM_HANDLE_EVENT (&getsamples_fsm, - GETSAMPLES_EVENT_bot_move_succeed); - FSM_HANDLE_EVENT (&gutter_fsm, - GUTTER_EVENT_bot_move_succeed); - */ FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_succeed); } else if (move_status == failure) { /* Move failed */ - /* - FSM_HANDLE_EVENT (&getsamples_fsm, - GETSAMPLES_EVENT_bot_move_failed); - FSM_HANDLE_EVENT (&getsamples_fsm, - GETSAMPLES_EVENT_bot_move_succeed); - FSM_HANDLE_EVENT (&gutter_fsm, - GUTTER_EVENT_bot_move_failed); - */ FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_bot_move_failed); } - asserv_status_e arm_status = asserv_last_cmd_ack () - ? asserv_arm_cmd_status () : none; - /* Check commands arm status */ - if (arm_status == success) - { - /* Pass it to all the FSM that need it */ - /* - FSM_HANDLE_EVENT (&getsamples_fsm, - GETSAMPLES_EVENT_arm_move_succeed); - */ - } - /* TODO: Check if the sensor placed at the noted position has seen - * an arm passed and forward this event to the getsamples FSM */ - if (main_asserv_arm_position_reached) - { - /* Reset the notifier */ - asserv_arm_set_position_reached (0); - /* - FSM_HANDLE_EVENT (&getsamples_fsm, - GETSAMPLES_EVENT_arm_pass_noted_position); - */ - } /* Jack */ FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? TOP_EVENT_jack_removed_from_bot : TOP_EVENT_jack_inserted_into_bot); - /*FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? - TOP_EVENT_jack_removed_from_bot : - TOP_EVENT_jack_inserted_into_bot); - */ /* Settings acknowledge */ /* if (main_top_generate_settings_ack_event) @@ -311,13 +256,6 @@ main_loop (void) } */ FSM_HANDLE_EVENT (&top_fsm, TOP_EVENT_settings_acknowledged); - /* Gutter wait_finished event */ - /* - if (main_gutter_generate_wait_finished_event) - { - FSM_HANDLE_EVENT (&gutter_fsm, GUTTER_EVENT_wait_finished); - } - */ /* Event generated at the end of the sub FSM to post to the top FSM */ if (main_post_event_for_top_fsm != 0xFF) { @@ -353,14 +291,6 @@ main_loop (void) { FSM_HANDLE_EVENT (&move_fsm, MOVE_EVENT_wait_finished); } - /* Wait flag for getsamples FSM */ - if (!main_getsamples_wait_cycle) - { - /* - FSM_HANDLE_EVENT (&getsamples_fsm, - GETSAMPLES_EVENT_wait_finished); - */ - } /* TODO: Check other sensors */ } @@ -443,6 +373,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: box identification */ switch (args[0]) + /*FSM_HANDLE_EVENT (&top_fsm, switch_get_jack () ? + TOP_EVENT_jack_removed_from_bot : + TOP_EVENT_jack_inserted_into_bot); + */ { case 'o': trap_open_rear_panel (); diff --git a/digital/io/src/top_cb.c b/digital/io/src/top_cb.c index 8532dc19..078cc9e9 100644 --- a/digital/io/src/top_cb.c +++ b/digital/io/src/top_cb.c @@ -47,7 +47,6 @@ top__MOVE_BACKWARD__move_fsm_finished (void) asserv_position_t position; asserv_get_position (&position); /* FIXME: linear move is better? */ - /* Move away from the gutter */ move_start (position.x, position.y - 1500, 0); return top_next (MOVE_BACKWARD, move_fsm_finished); } @@ -76,7 +75,6 @@ top__MOVE_FORWARD__move_fsm_finished (void) asserv_position_t position; asserv_get_position (&position); /* FIXME: linear move is better? */ - /* Move away from the gutter */ move_start (position.x, position.y + 1500, 0); return top_next (MOVE_FORWARD, move_fsm_finished); } @@ -104,7 +102,6 @@ top__CONFIGURE_ASSERV__settings_acknowledged (void) asserv_position_t position; asserv_get_position (&position); /* FIXME: linear move is better? */ - /* Move away from the gutter */ move_start (position.x, position.y - 1500, 0); return top_next (CONFIGURE_ASSERV, settings_acknowledged); } -- cgit v1.2.3