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 --- Makefile | 2 + 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 + digital/beacon/src/stub/Makefile | 12 +++ digital/beacon/src/stub/avrconfig.h | 61 ++++++++++++++ digital/beacon/src/stub/simu_stub.host.c | 126 +++++++++++++++++++++++++++++ digital/beacon/tools/beacon/__init__.py | 1 + digital/beacon/tools/beacon/mex.py | 57 +++++++++++++ digital/io-hub/src/guybrush/Makefile | 2 +- digital/io-hub/src/guybrush/avrconfig.h | 8 +- digital/io-hub/src/guybrush/main.c | 4 + digital/io-hub/src/robospierre/avrconfig.h | 4 +- digital/io/src/avrconfig.h | 2 + host/simu/robots/guybrush/link/bag.py | 2 + 17 files changed, 437 insertions(+), 4 deletions(-) create mode 100644 digital/ai/src/twi_master/beacon.c create mode 100644 digital/ai/src/twi_master/beacon.h create mode 100644 digital/beacon/src/stub/Makefile create mode 100644 digital/beacon/src/stub/avrconfig.h create mode 100644 digital/beacon/src/stub/simu_stub.host.c create mode 100644 digital/beacon/tools/beacon/__init__.py create mode 100644 digital/beacon/tools/beacon/mex.py diff --git a/Makefile b/Makefile index 0ab9c52e..a2057a11 100644 --- a/Makefile +++ b/Makefile @@ -18,6 +18,7 @@ DIRS = \ digital/asserv/src/asserv \ digital/mimot/src/asserv \ digital/mimot/src/dirty \ + digital/beacon/src/stub \ digital/io/src \ digital/io-hub/src/robospierre \ digital/io-hub/src/guybrush @@ -26,6 +27,7 @@ PYTHON_DIRS = \ digital/dev2/tools \ digital/asserv/tools \ digital/mimot/tools \ + digital/beacon/tools \ digital/io/tools \ digital/io-hub/tools \ host 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 () diff --git a/digital/beacon/src/stub/Makefile b/digital/beacon/src/stub/Makefile new file mode 100644 index 00000000..2f97d775 --- /dev/null +++ b/digital/beacon/src/stub/Makefile @@ -0,0 +1,12 @@ +BASE = ../../../avr +HOST_PROGS = simu_stub +simu_stub_SOURCES = simu_stub.host.c +MODULES = utils host twi +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 + +include $(BASE)/make/Makefile.gen diff --git a/digital/beacon/src/stub/avrconfig.h b/digital/beacon/src/stub/avrconfig.h new file mode 100644 index 00000000..e88759d5 --- /dev/null +++ b/digital/beacon/src/stub/avrconfig.h @@ -0,0 +1,61 @@ +#ifndef stub_avrconfig_h +#define stub_avrconfig_h +/* stub_avrconfig.h */ +/* lol - Laser Opponent Location finding system. {{{ + * + * 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. + * + * }}} */ + +/* Config for simu_stub. */ + +/* twi - TWI module. */ +/** Driver to implement TWI: HARD, SOFT, or USI. */ +#define AC_TWI_DRIVER HARD +/** Do not use interrupts. */ +#define AC_TWI_NO_INTERRUPT 0 +/** TWI frequency, should really be 100 kHz. */ +#define AC_TWI_FREQ 100000 +/** Enable slave part. */ +#define AC_TWI_SLAVE_ENABLE 1 +/** Enable master part. */ +#define AC_TWI_MASTER_ENABLE 0 +/** Use polled slave mode: received data is stored in a buffer which can be + * polled using twi_slave_poll. */ +#define AC_TWI_SLAVE_POLLED 1 +/** Slave reception callback to be defined by the user when not in polled + * mode. */ +#undef AC_TWI_SLAVE_RECV +/** Master transfer completion callback, optionally defined by the user, called + * at end of master transfer. */ +#undef AC_TWI_MASTER_DONE +/** Use internal pull up. */ +#define AC_TWI_PULL_UP 0 +/** Slave reception buffer size. */ +#define AC_TWI_SLAVE_RECV_BUFFER_SIZE 16 +/** Slave transmission buffer size. */ +#define AC_TWI_SLAVE_SEND_BUFFER_SIZE 16 + +/* beacon. */ +/** TWI address. */ +#define AC_BEACON_TWI_ADDRESS 10 + +#endif /* stub_avrconfig_h */ diff --git a/digital/beacon/src/stub/simu_stub.host.c b/digital/beacon/src/stub/simu_stub.host.c new file mode 100644 index 00000000..cdbacaa3 --- /dev/null +++ b/digital/beacon/src/stub/simu_stub.host.c @@ -0,0 +1,126 @@ +/* simu_stub.c */ +/* lol - Laser Opponent Location finding system. {{{ + * + * 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 "modules/host/host.h" +#include "modules/host/mex.h" +#include "modules/twi/twi.h" +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "modules/utils/crc.h" + +/** This is a stub only used in simulation to bypass all beacon computations + * and directly provide a position via TWI. */ + +struct position_t +{ + uint16_t x, y; +}; + +struct position_t simu_positions[2]; + +uint8_t simu_seq; + +static void +simu_twi_proto_poll (void) +{ + /* Do nothing interresting with the received value, but check it + * anyway. */ + uint8_t recv_buf[AC_TWI_SLAVE_RECV_BUFFER_SIZE]; + uint8_t recv_size; + while ((recv_size = twi_slave_poll (recv_buf, sizeof (recv_buf)))) + { + if (crc_compute (recv_buf + 1, recv_size - 1) != recv_buf[0]) + continue; + if (recv_buf[1] == simu_seq) + continue; + if (recv_size != 3) + continue; + simu_seq = recv_buf[1]; + } +} + +static void +simu_twi_proto_update (void) +{ + /* Update TWI slave buffer after each position update. */ + uint8_t status[4 + 5 * UTILS_COUNT (simu_positions)]; + uint8_t i, index = 1; + status[index++] = 0; + status[index++] = 0; + status[index++] = simu_seq; + for (i = 0; i < UTILS_COUNT (simu_positions); i++) + { + status[index++] = v16_to_v8 (simu_positions[i].x, 1); + status[index++] = v16_to_v8 (simu_positions[i].x, 0); + status[index++] = v16_to_v8 (simu_positions[i].y, 1); + status[index++] = v16_to_v8 (simu_positions[i].y, 0); + status[index++] = simu_positions[i].x == (uint16_t) -1 ? 0 : 100; + } + status[0] = crc_compute (&status[1], sizeof (status) - 1); + twi_slave_update (status, sizeof (status)); +} + +static void +simu_positions_handle (void *user, mex_msg_t *msg) +{ + uint8_t i; + struct position_t pos; + mex_msg_pop (msg, "BHH", &i, &pos.x, &pos.y); + simu_positions[i] = pos; + simu_twi_proto_update (); +} + +static void +simu_init (void) +{ + const char *mex_instance; + mex_node_connect (); + mex_instance = host_get_instance ("beacon0", 0); + uint8_t mtype = mex_node_reservef ("%s:position", mex_instance); + mex_node_register (mtype, simu_positions_handle, 0); + uint8_t i; + for (i = 0; i < UTILS_COUNT (simu_positions); i++) + { + simu_positions[i].x = (uint16_t) -1; + simu_positions[i].y = (uint16_t) -1; + } +} + +int +main (int argc, char ** argv) +{ + avr_init (argc, argv); + simu_init (); + twi_init (AC_BEACON_TWI_ADDRESS); + simu_twi_proto_update (); + while (1) + { + simu_twi_proto_poll (); + mex_node_wait_date (mex_node_date () + 1); + } + return 0; +} + diff --git a/digital/beacon/tools/beacon/__init__.py b/digital/beacon/tools/beacon/__init__.py new file mode 100644 index 00000000..af42b67b --- /dev/null +++ b/digital/beacon/tools/beacon/__init__.py @@ -0,0 +1 @@ +from mex import Mex diff --git a/digital/beacon/tools/beacon/mex.py b/digital/beacon/tools/beacon/mex.py new file mode 100644 index 00000000..fcc27741 --- /dev/null +++ b/digital/beacon/tools/beacon/mex.py @@ -0,0 +1,57 @@ +# lol - Laser Opponent Location finding system. {{{ +# +# 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. +# +# }}} +"""Mex interface to simulated beacon.""" + +import simu.mex.msg + +POSITION_NB = 2 + +class Mex: + """Handle communications with stub.""" + + class Position: + """Send a position to the stub.""" + + def __init__ (self, node, instance, index): + self.__node = node + self.__mtype = node.reserve (instance + ':position') + self.__index = index + self.pos = None + + def register_to (self, pos): + """Register to given observable position.""" + assert self.pos is None + self.pos = pos + self.pos.register (self.__update) + + def __update (self): + """Called on position update.""" + if self.pos.pos is not None: + m = simu.mex.msg.Msg (self.__mtype) + m.push ('BHH', self.__index, int (self.pos.pos[0]), int (self.pos.pos[1])) + self.__node.send (m) + + def __init__ (self, node, instance = 'beacon0'): + self.position = [ self.Position (node, instance, i) + for i in xrange (POSITION_NB) ] diff --git a/digital/io-hub/src/guybrush/Makefile b/digital/io-hub/src/guybrush/Makefile index 74423423..7a0e8bc2 100644 --- a/digital/io-hub/src/guybrush/Makefile +++ b/digital/io-hub/src/guybrush/Makefile @@ -8,7 +8,7 @@ io_hub_SOURCES = main.c top.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ contact.avr.c contact.host.c \ output.c output.host.c \ - twi_master.c asserv.c mimot.c \ + twi_master.c asserv.c mimot.c beacon.c \ chrono.c timer.avr.c simu.host.c # Modules needed for IO. MODULES = proto uart twi utils \ diff --git a/digital/io-hub/src/guybrush/avrconfig.h b/digital/io-hub/src/guybrush/avrconfig.h index 7aba16dc..d846fe6a 100644 --- a/digital/io-hub/src/guybrush/avrconfig.h +++ b/digital/io-hub/src/guybrush/avrconfig.h @@ -128,9 +128,15 @@ /* io-hub - io/ai board. */ /** TWI address of the io board. */ -#define AC_IO_TWI_ADDRESS 10 +#define AC_IO_TWI_ADDRESS 8 /** PWM setting. */ #undef AC_IOHUB_PWM +/** Use beacon board. */ +#define AC_AI_TWI_MASTER_BEACON 1 + +/* beacon. */ +/** Number of beacon positions. */ +#define AC_BEACON_POSITION_NB 2 /* asserv. */ /** Number of auxiliary motors. */ diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c index 924f77e1..c7ef204a 100644 --- a/digital/io-hub/src/guybrush/main.c +++ b/digital/io-hub/src/guybrush/main.c @@ -36,6 +36,7 @@ #include "asserv.h" #include "mimot.h" +#include "beacon.h" #include "twi_master.h" #include "contact.h" @@ -97,6 +98,9 @@ main_init (void) /* TWI communications */ asserv_init (); mimot_init (); +#if AC_AI_TWI_MASTER_BEACON + beacon_init (); +#endif twi_master_init (); /* IO modules. */ contact_init (); diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h index fdd682d0..648c7438 100644 --- a/digital/io-hub/src/robospierre/avrconfig.h +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -128,7 +128,7 @@ /* io-hub - io/ai board. */ /** TWI address of the io board. */ -#define AC_IO_TWI_ADDRESS 10 +#define AC_IO_TWI_ADDRESS 8 /** PWM setting. */ #define AC_IOHUB_PWM \ PWM (1, A, B, 5, D, 4) \ @@ -137,6 +137,8 @@ PWM (3, A, C, 6, C, 7) \ PWM (3, B, C, 5, C, 3) \ PWM (3, C, C, 4, C, 2) +/** Use beacon board. */ +#define AC_AI_TWI_MASTER_BEACON 0 /* asserv. */ /** Number of auxiliary motors. */ diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index 2ad8c129..053b9b56 100644 --- a/digital/io/src/avrconfig.h +++ b/digital/io/src/avrconfig.h @@ -137,6 +137,8 @@ /* io - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 2 +/** Use beacon board. */ +#define AC_AI_TWI_MASTER_BEACON 0 /* asserv. */ /** Number of auxiliary motors. */ diff --git a/host/simu/robots/guybrush/link/bag.py b/host/simu/robots/guybrush/link/bag.py index d706cf15..0603e2d9 100644 --- a/host/simu/robots/guybrush/link/bag.py +++ b/host/simu/robots/guybrush/link/bag.py @@ -25,6 +25,7 @@ import io_hub.mex import asserv.mex import mimot.mex +import beacon.mex class Bag: @@ -34,4 +35,5 @@ class Bag: self.io_hub = io_hub.mex.Mex (node, '%s:io0' % instance, contact_nb = 13, output_nb = 10) self.mimot = mimot.mex.Mex (node, '%s:mimot0' % instance) + self.beacon = beacon.mex.Mex (node, '%s:beacon0' % instance) -- cgit v1.2.3