summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/robospierre
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
parent002c8f8819a34cc8d589115cca433de716036d8c (diff)
digital/io-hub: handle bumpers
Diffstat (limited to 'digital/io-hub/src/robospierre')
-rw-r--r--digital/io-hub/src/robospierre/main.c4
-rw-r--r--digital/io-hub/src/robospierre/pawn_sensor.c77
-rw-r--r--digital/io-hub/src/robospierre/pawn_sensor.h8
-rw-r--r--digital/io-hub/src/robospierre/top.c28
4 files changed, 112 insertions, 5 deletions
diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c
index 1e12c427..9ae644e6 100644
--- a/digital/io-hub/src/robospierre/main.c
+++ b/digital/io-hub/src/robospierre/main.c
@@ -41,6 +41,7 @@
#include "pwm.h"
#include "contact.h"
#include "codebar.h"
+#include "pawn_sensor.h"
#include "radar.h"
#define FSM_NAME AI
@@ -207,6 +208,7 @@ main_loop (void)
/* Update IO modules. */
pwm_update ();
contact_update ();
+ pawn_sensor_update ();
if (usdist_update ())
{
position_t robot_pos;
@@ -234,7 +236,7 @@ main_loop (void)
}
if (main_stats_contact_ && !--main_stats_contact_cpt_)
{
- proto_send1d ('P', contact_all ());
+ proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24);
main_stats_contact_cpt_ = main_stats_contact_;
}
if (main_stats_codebar_ && !--main_stats_codebar_cpt_)
diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c
index ccb5eb5b..5ca18759 100644
--- a/digital/io-hub/src/robospierre/pawn_sensor.c
+++ b/digital/io-hub/src/robospierre/pawn_sensor.c
@@ -23,6 +23,7 @@
*
* }}} */
#include "common.h"
+#include "defs.h"
#include "pawn_sensor.h"
#include "asserv.h"
@@ -33,6 +34,12 @@
#include "bot.h"
#include "playground.h"
#include "codebar.h"
+#include "mimot.h"
+#include "main.h"
+
+#define FSM_NAME AI
+#include "fsm.h"
+#include "fsm_queue.h"
#include "modules/utils/utils.h"
#include "modules/math/geometry/distance.h"
@@ -49,8 +56,18 @@ struct pawn_sensor_t
vect_t active_position;
};
+/** Pawn sensor general context. */
+struct pawn_sensor_general_t
+{
+ /** Last bumped element position. */
+ vect_t last_bumped;
+ /** Bumper triggered, wait until the next one. */
+ uint16_t bump_wait;
+};
+
/** Global contexts. */
struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back;
+struct pawn_sensor_general_t pawn_sensor_global;
static uint8_t
pawn_sensor_get_type (uint8_t direction)
@@ -132,3 +149,63 @@ pawn_sensor_get (uint8_t direction)
return 0;
}
+void
+pawn_sensor_bumper (uint8_t bumped, uint16_t dx, uint16_t dy)
+{
+ uint8_t i;
+ if (bumped)
+ {
+ /* Compute pawn position. */
+ position_t robot_pos;
+ asserv_get_position (&robot_pos);
+ vect_t bumped_pawn;
+ bumped_pawn.x = dx;
+ bumped_pawn.y = dy;
+ vect_rotate_uf016 (&bumped_pawn, robot_pos.a);
+ vect_translate (&bumped_pawn, &robot_pos.v);
+ /* Do not count if out of the table. */
+ if (bumped_pawn.x < 400 + BOT_ELEMENT_RADIUS
+ && bumped_pawn.x >= PG_WIDTH - 400 - BOT_ELEMENT_RADIUS
+ && bumped_pawn.y < BOT_ELEMENT_RADIUS
+ && bumped_pawn.y >= PG_WIDTH - BOT_ELEMENT_RADIUS)
+ return;
+ /* Do not count the opponent as a pawn. */
+ for (i = 0; i < main_obstacles_nb; i++)
+ {
+ uint16_t dist = distance_point_point (&bumped_pawn,
+ &main_obstacles_pos[i]);
+ if (dist < 300 + BOT_ELEMENT_RADIUS)
+ return;
+ }
+ /* OK, take it. */
+ pawn_sensor_global.last_bumped = bumped_pawn;
+ fsm_queue_post_event (FSM_EVENT (AI, top_bumper));
+ pawn_sensor_global.bump_wait = 3 * 250;
+ }
+}
+
+void
+pawn_sensor_update (void)
+{
+#define BUMPER_FRONT_LEFT _BV (6)
+#define BUMPER_FRONT_RIGHT _BV (7)
+#define BUMPER_BACK_RIGHT _BV (5)
+#define BUMPER_BACK_LEFT _BV (4)
+ if (pawn_sensor_global.bump_wait)
+ pawn_sensor_global.bump_wait--;
+ else
+ {
+ uint8_t bumpers = mimot_get_input ();
+ pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265);
+ pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265);
+ pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265);
+ pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265);
+ }
+}
+
+vect_t
+pawn_sensor_get_last_bumped (void)
+{
+ return pawn_sensor_global.last_bumped;
+}
+
diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h
index 14ce2d62..04ea1a2f 100644
--- a/digital/io-hub/src/robospierre/pawn_sensor.h
+++ b/digital/io-hub/src/robospierre/pawn_sensor.h
@@ -30,4 +30,12 @@
uint8_t
pawn_sensor_get (uint8_t direction);
+/** Update pawn sensors. */
+void
+pawn_sensor_update (void);
+
+/** Return last bumped pawn. */
+vect_t
+pawn_sensor_get_last_bumped (void);
+
#endif /* pawn_sensor_h */
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)