summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.c60
-rw-r--r--digital/io-hub/src/guybrush/bottom_clamp.h35
2 files changed, 79 insertions, 16 deletions
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 */