summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre/top.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-06-03 08:24:48 +0200
committerNicolas Schodet2011-06-03 08:24:48 +0200
commitd5ca3805ed0eb4829b491352a31165b9fd58b15e (patch)
treee781708f0223b881fb57bf3c7a91f45b2d65894b /digital/io-hub/src/robospierre/top.c
parent002c8f8819a34cc8d589115cca433de716036d8c (diff)
digital/io-hub: handle bumpers
Diffstat (limited to 'digital/io-hub/src/robospierre/top.c')
-rw-r--r--digital/io-hub/src/robospierre/top.c28
1 files changed, 24 insertions, 4 deletions
diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c
index 9cdefc3e..52bb05ae 100644
--- a/digital/io-hub/src/robospierre/top.c
+++ b/digital/io-hub/src/robospierre/top.c
@@ -33,6 +33,7 @@
#include "logistic.h"
#include "move.h"
#include "chrono.h"
+#include "pawn_sensor.h"
/*
* Here is the top FSM. This FSM is suppose to give life to the robot with an
@@ -70,6 +71,10 @@ FSM_STATES (
/* Dropping, clearing so that doors can be closed. */
TOP_DROP_CLEARING)
+FSM_EVENTS (
+ /* Bumpers have seen something. */
+ top_bumper)
+
FSM_START_WITH (TOP_START)
/** Top context. */
@@ -127,6 +132,15 @@ top_prepare_level (void)
return 1;
}
+static void
+top_go_this_element (vect_t pos, int16_t shorten)
+{
+ ctx.go_to_element_direction = logistic_global.collect_direction;
+ uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
+ ? 0 : ASSERV_BACKWARD;
+ move_start_noangle (pos, backward, shorten);
+}
+
static uint8_t
top_go_element (void)
{
@@ -142,10 +156,7 @@ top_go_element (void)
logistic_global.prepare = top_prepare_level ();
}
vect_t element_pos = element_get_pos (ctx.target_element_id);
- ctx.go_to_element_direction = logistic_global.collect_direction;
- uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD
- ? 0 : ASSERV_BACKWARD;
- move_start_noangle (element_pos, backward, 0);
+ top_go_this_element (element_pos, 0);
return 1;
}
@@ -312,6 +323,15 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP)
return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working);
}
+FSM_TRANS (TOP_GOING_TO_ELEMENT, top_bumper, TOP_GOING_TO_ELEMENT)
+{
+ if (!ctx.broken)
+ logistic_global.prepare = top_prepare_level ();
+ move_stop ();
+ top_go_this_element (pawn_sensor_get_last_bumped (), BOT_ELEMENT_RADIUS - 50);
+ return FSM_NEXT (TOP_GOING_TO_ELEMENT, top_bumper);
+}
+
FSM_TRANS (TOP_WAITING_CLAMP, clamp_done,
drop, TOP_GOING_TO_DROP,
element, TOP_GOING_TO_ELEMENT)