From 44c97e60087faedaa41e406921171d6b5f6822c1 Mon Sep 17 00:00:00 2001 From: Julien Balleyguier Date: Sun, 13 May 2012 12:37:52 +0200 Subject: digital/io-hub/src/guybrush/bottom_clamp.c adding the calm_clamp_mode --- digital/io-hub/src/guybrush/bottom_clamp.c | 60 ++++++++++++++++++++++-------- digital/io-hub/src/guybrush/bottom_clamp.h | 35 +++++++++++++++++ 2 files changed, 79 insertions(+), 16 deletions(-) create mode 100644 digital/io-hub/src/guybrush/bottom_clamp.h (limited to 'digital') diff --git a/digital/io-hub/src/guybrush/bottom_clamp.c b/digital/io-hub/src/guybrush/bottom_clamp.c index 177f4436..1d260b3c 100644 --- a/digital/io-hub/src/guybrush/bottom_clamp.c +++ b/digital/io-hub/src/guybrush/bottom_clamp.c @@ -29,6 +29,7 @@ #include "contact.h" #include "bot.h" #include "main.h" +#include "bottom_clamp.h" #define FSM_NAME AI @@ -175,6 +176,8 @@ struct clamp_t uint8_t clamp_1_down; /** True we are stopping the tree approach. */ uint8_t stop_tree_approach; + /** True if we are in "Calm mod" (clamps can't move)*/ + uint8_t calm_mode; }; /*Global context. */ @@ -193,6 +196,11 @@ static void move_needed(int move,int speed) mimot_move_motor0_absolute (ctx.pos_current, speed); } +void clamp_calm_mode(int mode) +{ + ctx.calm_mode=mode; +} + /*---------------------------------------------------------*/ /* INIT part of the FSM */ @@ -255,17 +263,29 @@ FSM_TRANS (CLAMP_GOING_IDLE, lower_clamp_rotation_success, CLAMP_IDLE) FSM_TRANS (CLAMP_IDLE, coin_detected, CLAMP_TAKE_COIN) { if (ctx.clamp_1_down) +FSM_TRANS (CLAMP_IDLE, coin_detected, + normal_clamp,CLAMP_TAKE_COIN, + calm_clamp,CLAMP_IDLE) +{ + if (!ctx.calm_mode) { - /*Close it.*/ - IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE); + if (ctx.clamp_1_down) + { + /*Close it.*/ + IO_SET (OUTPUT_LOWER_CLAMP_1_CLOSE); + } + /*If the clamp 2 is ready*/ + else + { + IO_SET(OUTPUT_LOWER_CLAMP_2_CLOSE); + } + fsm_queue_post_event (FSM_EVENT (AI, taking_coin)); + return FSM_NEXT (CLAMP_IDLE, coin_detected,normal_clamp); } - /*If the clamp 2 is ready*/ - else + else { - IO_SET(OUTPUT_LOWER_CLAMP_2_CLOSE); + return FSM_NEXT (CLAMP_IDLE, coin_detected,calm_clamp); } - fsm_queue_post_event (FSM_EVENT (AI, taking_coin)); - return FSM_NEXT (CLAMP_IDLE, coin_detected); } @@ -317,21 +337,29 @@ FSM_TRANS (CLAMP_DROP_CD, lower_clamp_rotation_failure, CLAMP_BLOCKED) /* Parts of the FSM that is Approaching the tree (totem) */ /*---------------------------------------------------------*/ -FSM_TRANS (CLAMP_IDLE, tree_detected, CLAMP_BOTTOM_CLAMP_HIDE_POS) +FSM_TRANS (CLAMP_IDLE, tree_detected, + normal_clamp,CLAMP_BOTTOM_CLAMP_HIDE_POS, + calm_clamp,CLAMP_IDLE) { - /*Contrepression*/ - IO_SET (OUTPUT_UPPER_CLAMP_UP); - /*Hidding the clamp inside the robot*/ - if (ctx.clamp_1_down) + ctx.calm_mode=0; + if (!ctx.calm_mode) { - move_needed(HIDE_POS_TREE * 250,SPEED_ROTATION); + /*Hidding the clamp inside the robot*/ + if (ctx.clamp_1_down) + { + move_needed(HIDE_POS_TREE * 250,FAST_ROTATION); + } + else + { + move_needed((HALF_TURN + HIDE_POS_TREE) * 250,FAST_ROTATION); + ctx.clamp_1_down = 1; + } + return FSM_NEXT (CLAMP_IDLE, tree_detected,normal_clamp); } else { - move_needed((HALF_TURN + HIDE_POS_TREE) * 250,SPEED_ROTATION); - ctx.clamp_1_down = 1; + return FSM_NEXT (CLAMP_IDLE, tree_detected,calm_clamp); } - return FSM_NEXT (CLAMP_IDLE, tree_detected); } diff --git a/digital/io-hub/src/guybrush/bottom_clamp.h b/digital/io-hub/src/guybrush/bottom_clamp.h new file mode 100644 index 00000000..c3cd773a --- /dev/null +++ b/digital/io-hub/src/guybrush/bottom_clamp.h @@ -0,0 +1,35 @@ +#ifndef bottom_clamp_h +#define bottom_clam_h +/* bottom_clam.h */ +/* guybrush - Eurobot 2012 AI. {{{ + * + * Copyright (C) 2012 Julien Balleyguier + * + * 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. + * + * }}} */ + +/*function to put the clamp in the "clam_mode" (i.e putting the global var ctx.calm_mode at 1)*/ +void clamp_calm_mode(int mode); + +/*function to find out how many times the clamp has been blocked*/ +uint8_t clamp_read_blocked_cpt(void); + + +#endif /* bottom_clamp_h */ -- cgit v1.2.3