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/pawn_sensor.c | 113 +++++++++++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.c (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') 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; +} + -- cgit v1.2.3 From 5e4f6921f117d650025fee172af7bf4e68b69129 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 11:45:09 +0200 Subject: digital/io-hub, host/simu: add Strat contact --- digital/io-hub/src/robospierre/contact_defs.h | 8 +++++--- digital/io-hub/src/robospierre/pawn_sensor.c | 2 +- digital/io-hub/tools/io_hub/mex.py | 2 +- host/simu/robots/robospierre/model/bag.py | 3 ++- host/simu/robots/robospierre/view/bag.py | 2 ++ 5 files changed, 11 insertions(+), 6 deletions(-) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h index 1e04f6bf..8c60342a 100644 --- a/digital/io-hub/src/robospierre/contact_defs.h +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -25,8 +25,9 @@ * * }}} */ -#define CONTACT_COLOR A, 7 -#define CONTACT_JACK F, 7 +#define CONTACT_COLOR E, 5 +#define CONTACT_JACK E, 6 +#define CONTACT_STRAT E, 5 #define CONTACT_FRONT_BOTTOM A, 4 #define CONTACT_FRONT_MIDDLE F, 4 #define CONTACT_BACK_BOTTOM A, 5 @@ -42,6 +43,7 @@ CONTACT (CONTACT_BACK_BOTTOM) \ CONTACT (CONTACT_BACK_MIDDLE) \ CONTACT (CONTACT_BACK_TOP) \ - CONTACT (CONTACT_SIDE) + CONTACT (CONTACT_SIDE) \ + CONTACT (CONTACT_STRAT) #endif /* contact_defs_h */ diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index 65f0afe4..3a59e908 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -52,7 +52,7 @@ 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; + uint8_t element_type = IO_GET (CONTACT_STRAT) ? ELEMENT_PAWN : ELEMENT_KING; return element_type; } diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 44c41010..66060ea1 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -31,7 +31,7 @@ ADC_NB = 8 PWM_NB = 6 PWM_VALUE_MAX = 1024 -CONTACT_NB = 9 +CONTACT_NB = 10 CONTACT_INIT = 0xffffffff class Mex: diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 260960f8..b98dafd9 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -34,8 +34,9 @@ class Bag: def __init__ (self, scheduler, table, link_bag): self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True) self.jack = Switch (link_bag.io_hub.contact[1], invert = True) + self.strat_switch = Switch (link_bag.io_hub.contact[9], invert = True) self.contact = [ Switch (contact) - for contact in link_bag.io_hub.contact[2:] ] + for contact in link_bag.io_hub.contact[2:9] ] self.position = Position (link_bag.asserv.position) self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler, 8 * pi, 0, pi) diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py index 73aa9fe3..b718f6fe 100644 --- a/host/simu/robots/robospierre/view/bag.py +++ b/host/simu/robots/robospierre/view/bag.py @@ -35,6 +35,8 @@ class Bag: self.jack = Switch (sensor_frame, model_bag.jack, 'Jack') self.color_switch = Switch (sensor_frame, model_bag.color_switch, 'Color') + self.strat_switch = Switch (sensor_frame, model_bag.strat_switch, + 'Strat') self.robot = Robot (table, model_bag.position, model_bag.clamp) self.clamp = ( ClampTop (actuator_view.add_view (ClampTop.width, -- cgit v1.2.3 From 079e9cd0832d9854b172d120c822b578a2788e9a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 15:45:24 +0200 Subject: digital/io-hub: fix pawn sensor --- digital/io-hub/src/robospierre/pawn_sensor.c | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index 3a59e908..be6f3c87 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -32,6 +32,7 @@ #include "clamp.h" #include "bot.h" +#include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" /* Handle pawn sensors. When a pawn is detected, it can not be taken @@ -94,6 +95,12 @@ pawn_sensor_get (uint8_t direction) ctx->active = 0; return pawn_sensor_get_type (direction); } + else if (d > UTILS_ABS (dist)) + { + vect_from_polar_uf016 (&ctx->active_position, dist, + robot_position.a); + vect_translate (&ctx->active_position, &robot_position.v); + } } else { -- cgit v1.2.3 From 51b917f7994d6d53fde11263ba4da3470fd2a6c4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 15:26:22 +0200 Subject: digital/io-hub, host/simu: add codebar --- digital/io-hub/src/robospierre/Makefile | 1 + digital/io-hub/src/robospierre/codebar.avr.c | 64 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/codebar.h | 36 +++++++++++++++ digital/io-hub/src/robospierre/codebar.host.c | 56 +++++++++++++++++++++++ digital/io-hub/src/robospierre/main.c | 15 +++++++ digital/io-hub/src/robospierre/pawn_sensor.c | 5 ++- digital/io-hub/tools/io_hub/mex.py | 43 ++++++++++++++++++ host/simu/robots/robospierre/model/bag.py | 2 +- host/simu/robots/robospierre/model/clamp.py | 15 +++++-- 9 files changed, 231 insertions(+), 6 deletions(-) create mode 100644 digital/io-hub/src/robospierre/codebar.avr.c create mode 100644 digital/io-hub/src/robospierre/codebar.h create mode 100644 digital/io-hub/src/robospierre/codebar.host.c (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 714541dc..88e6c79e 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -6,6 +6,7 @@ HOST_PROGS = test_element # Sources to compile. io_hub_SOURCES = main.c top.c \ clamp.c logistic.c element.c pawn_sensor.c \ + codebar.avr.c codebar.host.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/codebar.avr.c b/digital/io-hub/src/robospierre/codebar.avr.c new file mode 100644 index 00000000..d2609012 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.avr.c @@ -0,0 +1,64 @@ +/* codebar.avr.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 "codebar.h" + +#include "defs.h" + +#include "modules/twi/twi.h" +#include "modules/utils/utils.h" +#include "modules/utils/crc.h" +#include "modules/utils/byte.h" + +#define CODEBAR_ADDRESS 0x20 +#define CODEBAR_STATUS_LENGTH 7 + +void +codebar_init (void) +{ +} + +uint8_t +codebar_get (uint8_t direction) +{ + uint8_t buffer[CODEBAR_STATUS_LENGTH]; + /* Read status. */ + twi_master_recv (CODEBAR_ADDRESS, buffer, sizeof (buffer)); + uint8_t ret = twi_master_wait (); + if (ret != CODEBAR_STATUS_LENGTH) + return 0; + uint8_t crc = crc_compute (buffer + 1, CODEBAR_STATUS_LENGTH - 1); + if (crc != buffer[0]) + return 0; + /* Get data. */ + uint8_t offset = direction == DIRECTION_FORWARD ? 1 : 4; + uint16_t age = v8_to_v16 (buffer[offset], buffer[offset + 1]); + uint16_t type = buffer[offset + 2]; + if (age > 225) + return 0; + else + return type; +} + diff --git a/digital/io-hub/src/robospierre/codebar.h b/digital/io-hub/src/robospierre/codebar.h new file mode 100644 index 00000000..d334f131 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.h @@ -0,0 +1,36 @@ +#ifndef codebar_h +#define codebar_h +/* codebar.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. + * + * }}} */ + +/** Initialise module. */ +void +codebar_init (void); + +/** Get element type on the specified direction. */ +uint8_t +codebar_get (uint8_t direction); + +#endif /* codebar_h */ diff --git a/digital/io-hub/src/robospierre/codebar.host.c b/digital/io-hub/src/robospierre/codebar.host.c new file mode 100644 index 00000000..68ced300 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.host.c @@ -0,0 +1,56 @@ +/* codebar.host.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 "codebar.h" +#include "defs.h" + +#include "modules/host/host.h" +#include "modules/host/mex.h" + +uint8_t codebar_front, codebar_back; + +static void +codebar_handle (void *user, mex_msg_t *msg) +{ + mex_msg_pop (msg, "BB", &codebar_front, &codebar_back); +} + +void +codebar_init (void) +{ + const char *mex_instance = host_get_instance ("io-hub0", 0); + uint8_t mtype = mex_node_reservef ("%s:codebar", mex_instance); + mex_node_register (mtype, codebar_handle, 0); +} + +uint8_t +codebar_get (uint8_t direction) +{ + if (direction == DIRECTION_FORWARD) + return codebar_front; + else + return codebar_back; +} + diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 79c0fe2a..42a2809a 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -40,6 +40,7 @@ #include "pwm.h" #include "contact.h" +#include "codebar.h" #include "radar.h" #define FSM_NAME AI @@ -77,6 +78,9 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; /** Contact stats counters. */ static uint8_t main_stats_contact_, main_stats_contact_cpt_; +/** Codebar stats counters. */ +static uint8_t main_stats_codebar_, main_stats_codebar_cpt_; + /** US sensors stats counters. */ static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; @@ -103,6 +107,7 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + codebar_init (); usdist_init (); /* AI modules. */ clamp_init (); @@ -223,6 +228,12 @@ main_loop (void) proto_send1d ('P', contact_all ()); main_stats_contact_cpt_ = main_stats_contact_; } + if (main_stats_codebar_ && !--main_stats_codebar_cpt_) + { + proto_send2b ('B', codebar_get (DIRECTION_FORWARD), + codebar_get (DIRECTION_BACKWARD)); + main_stats_codebar_cpt_ = main_stats_codebar_; + } if (main_stats_usdist_ && !--main_stats_usdist_cpt_) { proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2], @@ -331,6 +342,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Contact stats. */ main_stats_contact_ = main_stats_contact_cpt_ = args[0]; break; + case c ('B', 1): + /* Codebar stats. */ + main_stats_codebar_ = main_stats_codebar_cpt_ = args[0]; + break; case c ('U', 1): /* US sensors stats. */ main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index be6f3c87..cb4cfe4a 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -31,6 +31,7 @@ #include "element.h" #include "clamp.h" #include "bot.h" +#include "codebar.h" #include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" @@ -53,7 +54,9 @@ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; static uint8_t pawn_sensor_get_type (uint8_t direction) { - uint8_t element_type = IO_GET (CONTACT_STRAT) ? ELEMENT_PAWN : ELEMENT_KING; + uint8_t element_type = codebar_get (direction); + if (!element_type) + element_type = ELEMENT_PAWN; return element_type; } diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 8d758382..9b72ccde 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,46 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class Codebar (Observable): + """Codebar stub. + + - element_type: 'queen', 'king', or anything else. + + """ + + def __init__ (self, pack, index): + Observable.__init__ (self) + self.pack = pack + self.index = index + self.element_type = None + self.register (self.__notified) + + def __notified (self): + self.pack.set (self.index, self.element_type) + + class Pack: + """Handle emission of several codebar for one message.""" + + def __init__ (self, node, instance): + self.node = node + self.codebars = [0, 0] + self.mtype = node.reserve (instance + ':codebar') + + def set (self, index, element_type): + if element_type == 'queen': + self.codebars[index] = 4 + elif element_type == 'king': + self.codebars[index] = 8 + else: + self.codebars[index] = 0 + self.__send () + + def __send (self): + m = simu.mex.msg.Msg (self.mtype) + for c in self.codebars: + m.push ('b', c) + self.node.send (m) + class Path (Observable): """Path finding algorithm report. @@ -167,6 +207,9 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.__codebar_pack = self.Codebar.Pack (node, instance) + self.codebar = tuple (self.Codebar (self.__codebar_pack, i) + for i in (0, 1)) self.path = self.Path (node, instance) self.pos_report = self.PosReport (node, instance) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 284e5bb8..b9d3bc3f 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -44,7 +44,7 @@ class Bag: 8 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ] self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0], link_bag.mimot.aux[1], self.clamping_motor, self.door_motors, - self.contact[0:7]) + self.contact[0:7], link_bag.io_hub.codebar) self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index 0af4ccf3..dbcab5f2 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -30,13 +30,14 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side, door_motor, contact): + def __init__ (self, x, y, z, side, door_motor, contact, codebar = None): self.x = x self.y = y self.z = z self.side = side self.door_motor = door_motor self.contact = contact + self.codebar = codebar self.pawn = None class Clamp (Observable): @@ -62,7 +63,8 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor, door_motors, slot_contacts): + rotation_motor, clamping_motor, door_motors, slot_contacts, + codebars): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -75,13 +77,13 @@ class Clamp (Observable): door_motors[2], None, door_motors[3], None) self.slots = ( Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, - door_motors[0], slot_contacts[0]), + door_motors[0], slot_contacts[0], codebars[0]), Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, None, slot_contacts[1]), Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, door_motors[1], slot_contacts[2]), Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, - door_motors[2], slot_contacts[3]), + door_motors[2], slot_contacts[3], codebars[1]), Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, None, slot_contacts[4]), Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, @@ -203,6 +205,11 @@ class Clamp (Observable): and slots[0].pawn.kind == 'tower')) # This one is really high. slots[2].contact.state = not (slots[2].pawn is not None) + if slots[0].pawn: + slots[0].codebar.element_type = slots[0].pawn.kind + else: + slots[0].codebar.element_type = None + slots[0].codebar.notify () slot_side = self.slots[self.SLOT_SIDE] slot_side.contact.state = slot_side.pawn is None clamp_slot = self.__get_clamp_slot () -- cgit v1.2.3 From 4bf3121c81de96a6c67d7e5d4bb53f0c33fc5e74 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 03:36:20 +0200 Subject: digital/io-hub: near the wall, take the element --- digital/io-hub/src/robospierre/pawn_sensor.c | 11 +++++++++++ 1 file changed, 11 insertions(+) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index cb4cfe4a..ccb5eb5b 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -31,6 +31,7 @@ #include "element.h" #include "clamp.h" #include "bot.h" +#include "playground.h" #include "codebar.h" #include "modules/utils/utils.h" @@ -91,6 +92,16 @@ pawn_sensor_get (uint8_t direction) asserv_get_position (&robot_position); if (ctx->active) { + /* In green zone, take it when near enougth from the wall. */ + if (robot_position.v.x < BOT_GREEN_ELEMENT_DISTANCE_MM + 10 + || (robot_position.v.x > PG_WIDTH + - BOT_GREEN_ELEMENT_DISTANCE_MM - 10)) + { + ctx->active = 0; + return pawn_sensor_get_type (direction); + } + /* Else, take it if near enough from the supposed element + * position. */ int32_t d = distance_point_point (&ctx->active_position, &robot_position.v); if (d < BOT_PAWN_TAKING_DISTANCE_MM) -- cgit v1.2.3 From d5ca3805ed0eb4829b491352a31165b9fd58b15e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 08:24:48 +0200 Subject: digital/io-hub: handle bumpers --- digital/ai/src/twi_master/mimot.c | 6 +++ digital/ai/src/twi_master/mimot.h | 4 ++ digital/io-hub/src/robospierre/main.c | 4 +- digital/io-hub/src/robospierre/pawn_sensor.c | 77 ++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 8 +++ digital/io-hub/src/robospierre/top.c | 28 ++++++++-- digital/mimot/src/dirty/simu.host.c | 2 +- 7 files changed, 123 insertions(+), 6 deletions(-) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index 6159e6c9..8df84538 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -97,6 +97,12 @@ mimot_motor1_cmd_status (void) return none; } +uint8_t +mimot_get_input (void) +{ + return mimot_status.input_port; +} + uint16_t mimot_get_motor0_position (void) { diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index c59c1608..c9de7090 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -56,6 +56,10 @@ mimot_motor0_cmd_status (void); asserv_status_e mimot_motor1_cmd_status (void); +/** Return input port state. */ +uint8_t +mimot_get_input (void); + /** Get motor0 position in steps. */ uint16_t mimot_get_motor0_position (void); 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) diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c index 8afd73ca..d27ffe5f 100644 --- a/digital/mimot/src/dirty/simu.host.c +++ b/digital/mimot/src/dirty/simu.host.c @@ -156,7 +156,7 @@ simu_sensor_update_marcel (void) void simu_sensor_update_robospierre (void) { - PINC = 0; + PINC = 0xf0; if (simu_aux_model[0].th < 120.0 * 5.0 / 6.0 * simu_aux_model[0].m.i_G) PINC |= IO_BV (CONTACT_AUX0_ZERO_IO); } -- cgit v1.2.3 From 618872c7705a7f71afe77cd13b694a07660ebcb8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 02:48:28 +0200 Subject: digital/io-hub: conditionaly enable bumpers --- digital/io-hub/src/robospierre/pawn_sensor.c | 27 +++++++++++++++++++-------- digital/io-hub/src/robospierre/pawn_sensor.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 7 +++++++ 3 files changed, 30 insertions(+), 8 deletions(-) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index 5ca18759..cc9e6900 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -59,6 +59,8 @@ struct pawn_sensor_t /** Pawn sensor general context. */ struct pawn_sensor_general_t { + /** Activate bumpers. */ + uint8_t bumper_enabled; /** Last bumped element position. */ vect_t last_bumped; /** Bumper triggered, wait until the next one. */ @@ -191,18 +193,27 @@ pawn_sensor_update (void) #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 + if (pawn_sensor_global.bumper_enabled) { - 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); + 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); + } } } +void +pawn_sensor_bumper_enable (uint8_t enabled) +{ + pawn_sensor_global.bumper_enabled = enabled; +} + vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 04ea1a2f..8d6d2651 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -34,6 +34,10 @@ pawn_sensor_get (uint8_t direction); void pawn_sensor_update (void); +/** Enable bumpers. */ +void +pawn_sensor_bumper_enable (uint8_t enabled); + /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7e3f4413..f4afaa17 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -153,9 +153,15 @@ top_go_element (void) if (!ctx.broken) { if (e.attr & ELEMENT_GREEN) + { logistic_global.prepare = 0; + pawn_sensor_bumper_enable (0); + } else + { logistic_global.prepare = top_prepare_level (); + pawn_sensor_bumper_enable (1); + } } vect_t element_pos = element_get_pos (ctx.target_element_id); top_go_this_element (element_pos, 0); @@ -172,6 +178,7 @@ top_go_drop (void) drop_pos.v = element_get_pos (ctx.target_element_id); if (!ctx.broken) logistic_global.prepare = top_prepare_level (); + pawn_sensor_bumper_enable (0); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; /* Go above or below the drop point. */ -- cgit v1.2.3 From 0b9ae480b928a24664bfd30edcd17d760db593d1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 14:43:16 +0200 Subject: digital/io-hub/src/robospierre: disable pawn sensors when tower dropped --- digital/io-hub/src/robospierre/pawn_sensor.c | 6 ++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 2 ++ 3 files changed, 12 insertions(+) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index cc9e6900..faeee879 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -214,6 +214,12 @@ pawn_sensor_bumper_enable (uint8_t enabled) pawn_sensor_global.bumper_enabled = enabled; } +void +pawn_sensor_bumper_wait (uint16_t wait) +{ + pawn_sensor_global.bump_wait = wait; +} + vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 8d6d2651..9c76f77a 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -38,6 +38,10 @@ pawn_sensor_update (void); void pawn_sensor_bumper_enable (uint8_t enabled); +/** Temporarily disable bumpers. */ +void +pawn_sensor_bumper_wait (uint16_t wait); + /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7732fb50..3d39f60a 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -434,6 +434,7 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); + pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); @@ -446,6 +447,7 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); + pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop); -- cgit v1.2.3 From a42a9f908fc0cad8760ecda5670a6b1322d56a83 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 16:39:27 +0200 Subject: Revert "digital/io-hub/src/robospierre: disable pawn sensors when tower dropped" This reverts commit 0b9ae480b928a24664bfd30edcd17d760db593d1. --- digital/io-hub/src/robospierre/pawn_sensor.c | 6 ------ digital/io-hub/src/robospierre/pawn_sensor.h | 4 ---- digital/io-hub/src/robospierre/top.c | 2 -- 3 files changed, 12 deletions(-) (limited to 'digital/io-hub/src/robospierre/pawn_sensor.c') diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index faeee879..cc9e6900 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -214,12 +214,6 @@ pawn_sensor_bumper_enable (uint8_t enabled) pawn_sensor_global.bumper_enabled = enabled; } -void -pawn_sensor_bumper_wait (uint16_t wait) -{ - pawn_sensor_global.bump_wait = wait; -} - vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 9c76f77a..8d6d2651 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -38,10 +38,6 @@ pawn_sensor_update (void); void pawn_sensor_bumper_enable (uint8_t enabled); -/** Temporarily disable bumpers. */ -void -pawn_sensor_bumper_wait (uint16_t wait); - /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 3d39f60a..7732fb50 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -434,7 +434,6 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); - pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); @@ -447,7 +446,6 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); - pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop); -- cgit v1.2.3