From 5c8d716b96d32609d18af5219c541201209e586d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 29 May 2011 19:19:28 +0200 Subject: digital/io-hub: handle element detection on table --- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 7 ++ digital/io-hub/src/robospierre/clamp.c | 12 +-- digital/io-hub/src/robospierre/pawn_sensor.c | 113 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 33 ++++++++ host/simu/robots/robospierre/model/clamp.py | 2 +- 6 files changed, 161 insertions(+), 8 deletions(-) create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.c create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.h 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 */ diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index a3476a46..0af4ccf3 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -220,7 +220,7 @@ class Clamp (Observable): m = self.__get_robot_matrix () # Look up elements. xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side] - xmargin = 20 + xmargin = 40 ymargin = 50 for o in self.table.obstacles: if o.level == 1 and o.pos is not None: -- cgit v1.2.3