summaryrefslogtreecommitdiffhomepage
path: root/digital/io-hub
diff options
context:
space:
mode:
authorNicolas Schodet2011-05-29 19:19:28 +0200
committerNicolas Schodet2011-05-30 01:12:06 +0200
commit5c8d716b96d32609d18af5219c541201209e586d (patch)
tree8dd54149d201b451d7926c293b1087a6489c8e98 /digital/io-hub
parent2700442e8ecb331844687e349f5b20162a008b27 (diff)
digital/io-hub: handle element detection on table
Diffstat (limited to 'digital/io-hub')
-rw-r--r--digital/io-hub/src/robospierre/Makefile2
-rw-r--r--digital/io-hub/src/robospierre/bot.h7
-rw-r--r--digital/io-hub/src/robospierre/clamp.c12
-rw-r--r--digital/io-hub/src/robospierre/pawn_sensor.c113
-rw-r--r--digital/io-hub/src/robospierre/pawn_sensor.h33
5 files changed, 160 insertions, 7 deletions
diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile
index e4b87124..a9bb2283 100644
--- a/digital/io-hub/src/robospierre/Makefile
+++ b/digital/io-hub/src/robospierre/Makefile
@@ -5,7 +5,7 @@ PROGS = io_hub
HOST_PROGS = test_element
# Sources to compile.
io_hub_SOURCES = main.c \
- clamp.c logistic.c element.c \
+ clamp.c logistic.c element.c pawn_sensor.c \
radar_defs.c radar.c path.c move.c \
init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \
pwm.avr.c pwm.host.c \
diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h
index 890780b3..7e773032 100644
--- a/digital/io-hub/src/robospierre/bot.h
+++ b/digital/io-hub/src/robospierre/bot.h
@@ -46,6 +46,13 @@
/** Angle error at the front contact point. */
#define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0
+/** Distance from robot center to front pawn detection threshold. */
+#define BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM 190
+/** Distance from robot center to back pawn detection threshold. */
+#define BOT_PAWN_BACK_DETECTION_THRESHOLD_MM -190
+/** Distance from robot center to an element near enough to be taken. */
+#define BOT_PAWN_TAKING_DISTANCE_MM 150
+
/** Speed used for initialisation. */
#define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10
/** Normal cruise speed. */
diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c
index 4c0fc9a7..bae3591d 100644
--- a/digital/io-hub/src/robospierre/clamp.c
+++ b/digital/io-hub/src/robospierre/clamp.c
@@ -38,6 +38,7 @@
#include "modules/proto/proto.h"
#include "logistic.h"
+#include "pawn_sensor.h"
/*
* There is two FSM in this file.
@@ -275,16 +276,15 @@ clamp_handle_event (void)
{
if (FSM_CAN_HANDLE (AI, clamp_new_element))
{
- /* XXX: temporary hack. */
- uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING;
- if (!IO_GET (CONTACT_FRONT_BOTTOM)
- && !logistic_global.slots[CLAMP_SLOT_FRONT_BOTTOM])
+ uint8_t element_type;
+ element_type = pawn_sensor_get (DIRECTION_FORWARD);
+ if (element_type)
{
clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element_type);
return 1;
}
- if (!IO_GET (CONTACT_BACK_BOTTOM)
- && !logistic_global.slots[CLAMP_SLOT_BACK_BOTTOM])
+ element_type = pawn_sensor_get (DIRECTION_BACKWARD);
+ if (element_type)
{
clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element_type);
return 1;
diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c
new file mode 100644
index 00000000..65f0afe4
--- /dev/null
+++ b/digital/io-hub/src/robospierre/pawn_sensor.c
@@ -0,0 +1,113 @@
+/* pawn_sensor.c */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2011 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.
+ *
+ * }}} */
+#include "common.h"
+#include "pawn_sensor.h"
+
+#include "asserv.h"
+#include "contact.h"
+#include "logistic.h"
+#include "element.h"
+#include "clamp.h"
+#include "bot.h"
+
+#include "modules/math/geometry/distance.h"
+
+/* Handle pawn sensors. When a pawn is detected, it can not be taken
+ * directly, but only once it is inside the robot. */
+
+/** Pawn sensor context. */
+struct pawn_sensor_t
+{
+ /** Is there something in front of the sensor? */
+ uint8_t active;
+ /** If active, supposed position of element. */
+ vect_t active_position;
+};
+
+/** Global contexts. */
+struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back;
+
+static uint8_t
+pawn_sensor_get_type (uint8_t direction)
+{
+ uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING;
+ return element_type;
+}
+
+uint8_t
+pawn_sensor_get (uint8_t direction)
+{
+ struct pawn_sensor_t *ctx;
+ uint8_t contact_value, slot;
+ int16_t dist;
+ /* Check direction. */
+ if (direction == DIRECTION_FORWARD)
+ {
+ ctx = &pawn_sensor_front;
+ contact_value = !IO_GET (CONTACT_FRONT_BOTTOM);
+ slot = CLAMP_SLOT_FRONT_BOTTOM;
+ dist = BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM;
+ }
+ else
+ {
+ ctx = &pawn_sensor_back;
+ contact_value = !IO_GET (CONTACT_BACK_BOTTOM);
+ slot = CLAMP_SLOT_BACK_BOTTOM;
+ dist = BOT_PAWN_BACK_DETECTION_THRESHOLD_MM;
+ }
+ /* Handle contact. */
+ if (contact_value)
+ {
+ if (!logistic_global.slots[slot]
+ && logistic_global.moving_to != slot)
+ {
+ position_t robot_position;
+ asserv_get_position (&robot_position);
+ if (ctx->active)
+ {
+ int32_t d = distance_point_point (&ctx->active_position,
+ &robot_position.v);
+ if (d < BOT_PAWN_TAKING_DISTANCE_MM)
+ {
+ ctx->active = 0;
+ return pawn_sensor_get_type (direction);
+ }
+ }
+ else
+ {
+ ctx->active = 1;
+ vect_from_polar_uf016 (&ctx->active_position, dist,
+ robot_position.a);
+ vect_translate (&ctx->active_position, &robot_position.v);
+ }
+ }
+ }
+ else
+ {
+ ctx->active = 0;
+ }
+ return 0;
+}
+
diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h
new file mode 100644
index 00000000..14ce2d62
--- /dev/null
+++ b/digital/io-hub/src/robospierre/pawn_sensor.h
@@ -0,0 +1,33 @@
+#ifndef pawn_sensor_h
+#define pawn_sensor_h
+/* pawn_sensor.h */
+/* robospierre - Eurobot 2011 AI. {{{
+ *
+ * Copyright (C) 2011 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 sensor state and return the element type if an element is ready to
+ * be taken. */
+uint8_t
+pawn_sensor_get (uint8_t direction);
+
+#endif /* pawn_sensor_h */