From a750fc58646c6f1ab5319d9bcf786be7a525a77c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 Apr 2012 22:07:20 +0200 Subject: digital/{ai,beacon,io-hub}: add beacon simulation stub --- digital/ai/src/twi_master/beacon.c | 84 ++++++++++++++++++++++++++++++++++ digital/ai/src/twi_master/beacon.h | 58 +++++++++++++++++++++++ digital/ai/src/twi_master/twi_master.c | 6 +++ digital/ai/tools/guybrush.py | 9 +++- digital/ai/tools/test_simu.py | 3 ++ 5 files changed, 159 insertions(+), 1 deletion(-) create mode 100644 digital/ai/src/twi_master/beacon.c create mode 100644 digital/ai/src/twi_master/beacon.h (limited to 'digital/ai') diff --git a/digital/ai/src/twi_master/beacon.c b/digital/ai/src/twi_master/beacon.c new file mode 100644 index 00000000..6f3a3b2b --- /dev/null +++ b/digital/ai/src/twi_master/beacon.c @@ -0,0 +1,84 @@ +/* beacon.c */ +/* ai - Robot Artificial Intelligence. {{{ + * + * Copyright (C) 2012 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 "beacon.h" + +#include "twi_master.h" + +#include "modules/utils/byte.h" + +/** Global context. */ +struct beacon_t +{ + /** Obstacles position. */ + vect_t position[AC_BEACON_POSITION_NB]; + /** Obstacles trust. */ + uint8_t trust[AC_BEACON_POSITION_NB]; +}; +struct beacon_t beacon; + +void +beacon_init (void) +{ + uint8_t i; + for (i = 0; i < AC_BEACON_POSITION_NB; i++) + beacon.trust[i] = 0; +} + +void +beacon_status_cb (uint8_t *status) +{ + uint8_t i, index = 3; + for (i = 0; i < AC_BEACON_POSITION_NB; i++) + { + beacon.position[i].x = v8_to_v16 (status[index], status[index + 1]); + index += 2; + beacon.position[i].y = v8_to_v16 (status[index], status[index + 1]); + index += 2; + beacon.trust[i] = status[index]; + index++; + } +} + +void +beacon_on (uint8_t on_off) +{ + uint8_t *buffer = twi_master_get_buffer (BEACON_SLAVE); + buffer[0] = on_off; + twi_master_send_buffer (1); +} + +uint8_t +beacon_get_position (uint8_t index, vect_t *position) +{ + if (index < AC_BEACON_POSITION_NB) + { + *position = beacon.position[index]; + return beacon.trust[index]; + } + else + return 0; +} + diff --git a/digital/ai/src/twi_master/beacon.h b/digital/ai/src/twi_master/beacon.h new file mode 100644 index 00000000..2ff4e635 --- /dev/null +++ b/digital/ai/src/twi_master/beacon.h @@ -0,0 +1,58 @@ +#ifndef beacon_h +#define beacon_h +/* beacon.h */ +/* ai - Robot Artificial Intelligence. {{{ + * + * Copyright (C) 2012 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 "defs.h" + +/** Interface with beacon board using the TWI protocol. */ + +/** Slave number in the twi_master list. */ +#define BEACON_SLAVE 2 + +/** Beacon TWI address. */ +#define BEACON_TWI_ADDRESS 10 + +/** Length of status buffer (not including CRC). */ +#define BEACON_STATUS_LENGTH (3 + 5 * AC_BEACON_POSITION_NB) + +/** Initialise module. */ +void +beacon_init (void); + +/** Called when a new status buffer is received, update the beacon + * information. */ +void +beacon_status_cb (uint8_t *status); + +/** Turn on (1) or off (0). */ +void +beacon_on (uint8_t on_off); + +/** Get a detected position, return trust (0 for invalid, 100 for full + * trust). */ +uint8_t +beacon_get_position (uint8_t index, vect_t *position); + +#endif /* beacon_h */ diff --git a/digital/ai/src/twi_master/twi_master.c b/digital/ai/src/twi_master/twi_master.c index 90977fd3..ee401d7d 100644 --- a/digital/ai/src/twi_master/twi_master.c +++ b/digital/ai/src/twi_master/twi_master.c @@ -27,6 +27,9 @@ #include "asserv.h" #include "mimot.h" +#if AC_AI_TWI_MASTER_BEACON +# include "beacon.h" +#endif #include "modules/twi/twi.h" #include "modules/utils/utils.h" @@ -100,6 +103,9 @@ struct twi_master_slave_t static struct twi_master_slave_t twi_master_slaves[] = { { ASSERV_TWI_ADDRESS, 0, ASSERV_STATUS_LENGTH, asserv_status_cb }, { MIMOT_TWI_ADDRESS, 0, MIMOT_STATUS_LENGTH, mimot_status_cb }, +#if AC_AI_TWI_MASTER_BEACON + { BEACON_TWI_ADDRESS, 0, BEACON_STATUS_LENGTH, beacon_status_cb }, +#endif }; /** Send first pending message if available. */ diff --git a/digital/ai/tools/guybrush.py b/digital/ai/tools/guybrush.py index 0600a494..340d7ef7 100644 --- a/digital/ai/tools/guybrush.py +++ b/digital/ai/tools/guybrush.py @@ -7,6 +7,7 @@ import io_hub.init from proto.popen_io import PopenIO import math +import subprocess class Robot: """Guybrush robot instance.""" @@ -23,24 +24,30 @@ class Robot: True: (3000 - 250, 2000 - 250, math.radians (180)) } - client_nb = 3 + client_nb = 4 def __init__ (self, proto_time, instance = 'robot0'): self.instance = instance def proto (proto_class, cmd, init): cmd = [ s.format (instance = instance) for s in cmd ] return proto_class (PopenIO (cmd), proto_time, **init) + def prog (cmd): + cmd = [ s.format (instance = instance) for s in cmd ] + subprocess.Popen (cmd) asserv_cmd = ('../../mimot/src/asserv/asserv.host', '-i{instance}:asserv0', '-m9', 'guybrush') mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-i{instance}:mimot0', '-m9', 'guybrush') io_hub_cmd = ('../../io-hub/src/guybrush/io_hub.host', '-i{instance}:io0') + beacon_stub_cmd = ('../../beacon/src/stub/simu_stub.host', + '-i{instance}:beacon0') self.asserv = proto (asserv.Proto, asserv_cmd, asserv.init.host['guybrush']) self.mimot = proto (mimot.Proto, mimot_cmd, mimot.init.host['guybrush']) self.io = proto (io_hub.ProtoGuybrush, io_hub_cmd, io_hub.init.host['guybrush']) + self.beacon_stub = prog (beacon_stub_cmd) self.protos = (self.asserv, self.mimot, self.io) diff --git a/digital/ai/tools/test_simu.py b/digital/ai/tools/test_simu.py index fad3ac62..dc8b9481 100644 --- a/digital/ai/tools/test_simu.py +++ b/digital/ai/tools/test_simu.py @@ -93,6 +93,9 @@ class TestSimu (InterNode): if color_switch_set_pos: r.asserv.set_pos (*r.robot_start_pos[i]) r.model.color_switch.register (change_color) + # Beacon system. + if hasattr (r.link, 'beacon'): + r.link.beacon.position[0].register_to (self.obstacle) def close (self): self.forked_hub.kill () -- cgit v1.2.3