From 70020d019a3057a58251b713622a681f91a0cf5a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 14 May 2012 15:56:22 +0200 Subject: digital/io-hub/src/guybrush: close door when the hold has been left --- digital/io-hub/src/guybrush/main.c | 2 ++ digital/io-hub/src/guybrush/top.c | 42 +++++++++++++++++++++++--------------- digital/io-hub/src/guybrush/top.h | 32 +++++++++++++++++++++++++++++ 3 files changed, 60 insertions(+), 16 deletions(-) create mode 100644 digital/io-hub/src/guybrush/top.h (limited to 'digital/io-hub') diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c index 046398dc..b59594ea 100644 --- a/digital/io-hub/src/guybrush/main.c +++ b/digital/io-hub/src/guybrush/main.c @@ -53,6 +53,7 @@ #include "path.h" #include "move.h" +#include "top.h" #include "bot.h" @@ -239,6 +240,7 @@ main_loop (void) } pressure_update (); /* Update AI modules. */ + top_update (); path_decay (); /* Only manage events if slaves are synchronised. */ if (twi_master_sync () && main_fsm_debug_mode != MAIN_FSM_DEBUG_STOP) diff --git a/digital/io-hub/src/guybrush/top.c b/digital/io-hub/src/guybrush/top.c index 2197b0f3..ffd96767 100644 --- a/digital/io-hub/src/guybrush/top.c +++ b/digital/io-hub/src/guybrush/top.c @@ -96,9 +96,7 @@ FSM_STATES ( /* Going to an unload position. */ TOP_UNLOAD_GOING, /* Unloading, waiting for elements to fall. */ - TOP_UNLOADING, - /* Unloading, going back to playground. */ - TOP_UNLOADING_GOING_BACK) + TOP_UNLOADING) FSM_START_WITH (TOP_START) @@ -109,11 +107,30 @@ struct top_t vect_t decision_pos; /** Current distance to totem. */ int16_t totem_distance; + /** Close door when out of unloading zone. */ + uint8_t close_door; }; /** Global context. */ struct top_t top; +void +top_update (void) +{ + if (top.close_door) + { + position_t robot_pos; + asserv_get_position (&robot_pos); + if (robot_pos.v.x > PG_HOLD_NORTH_X + BOT_SIZE_BACK + && robot_pos.v.x < PG_MIRROR_X (PG_HOLD_NORTH_X + BOT_SIZE_BACK)) + { + IO_CLR (OUTPUT_DOOR_OPEN); + IO_SET (OUTPUT_DOOR_CLOSE); + top.close_door = 0; + } + } +} + /** Go collect a totem. */ static void top_go_totem (void) @@ -432,21 +449,14 @@ FSM_TRANS (TOP_UNLOAD_GOING, move_success, TOP_UNLOADING) return FSM_NEXT (TOP_UNLOAD_GOING, move_success); } -FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, TOP_UNLOADING_GOING_BACK) +FSM_TRANS_TIMEOUT (TOP_UNLOADING, 250, + totem, TOP_TOTEM_GOING, + bottle, TOP_BOTTLE_GOING, + unload, TOP_UNLOAD_GOING) { strat_success (); - asserv_move_linearly (100); - return FSM_NEXT_TIMEOUT (TOP_UNLOADING); -} - -FSM_TRANS (TOP_UNLOADING_GOING_BACK, robot_move_success, - totem, TOP_TOTEM_GOING, - bottle, TOP_BOTTLE_GOING, - unload, TOP_UNLOAD_GOING) -{ - IO_CLR (OUTPUT_DOOR_OPEN); - IO_SET (OUTPUT_DOOR_CLOSE); - RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING_GOING_BACK, robot_move_success); + top.close_door = 1; + RETURN_TOP_DECISION_SWITCH (TOP_UNLOADING, TOP_UNLOADING_TIMEOUT); } /** UNLOAD failures. */ diff --git a/digital/io-hub/src/guybrush/top.h b/digital/io-hub/src/guybrush/top.h new file mode 100644 index 00000000..28c35da5 --- /dev/null +++ b/digital/io-hub/src/guybrush/top.h @@ -0,0 +1,32 @@ +#ifndef top_h +#define top_h +/* top.h */ +/* guybrush - Eurobot 2012 AI. {{{ + * + * Copyright (C) 2012 Nicolas Schodet + * + * 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. + * + * }}} */ + +/** Update top own things. */ +void +top_update (void); + +#endif /* top_h */ -- cgit v1.2.3