From 1b0e39a3eeab73de59e5b6fe982bff67021afc78 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 3 Apr 2013 22:13:23 +0200 Subject: digital/io-hub/src/apbirthday: add communication with beacon board --- digital/io-hub/src/common-cc/beacon.cc | 64 ++++++++++++++++++++++++++++++++++ digital/io-hub/src/common-cc/beacon.hh | 64 ++++++++++++++++++++++++++++++++++ 2 files changed, 128 insertions(+) create mode 100644 digital/io-hub/src/common-cc/beacon.cc create mode 100644 digital/io-hub/src/common-cc/beacon.hh (limited to 'digital/io-hub/src/common-cc') diff --git a/digital/io-hub/src/common-cc/beacon.cc b/digital/io-hub/src/common-cc/beacon.cc new file mode 100644 index 00000000..4a1b0f02 --- /dev/null +++ b/digital/io-hub/src/common-cc/beacon.cc @@ -0,0 +1,64 @@ +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 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 "beacon.hh" + +#include "ucoolib/utils/bytes.hh" + +Beacon::Beacon (I2cQueue &queue) + : I2cQueue::Slave (queue, 0x0a, 5 * pos_nb), on_ (false), robots_nb_ (0) +{ + for (int i = 0; i < pos_nb; i++) + trust_[i] = 0; +} + +void +Beacon::recv_status (const uint8_t *status) +{ + int index = 0; + for (int i = 0; i < pos_nb; i++) + { + pos_[i].x = ucoo::bytes_pack (status[index], status[index + 1]); + index += 2; + pos_[i].y = ucoo::bytes_pack (status[index], status[index + 1]); + index += 2; + trust_[i] = status[index]; + index++; + } +} + +void +Beacon::send_position (const vect_t &pos) +{ + uint8_t buf[] = { + on_, + team_color, + (uint8_t) robots_nb_, + ucoo::bytes_unpack (pos.x, 1), + ucoo::bytes_unpack (pos.x, 0), + ucoo::bytes_unpack (pos.y, 1), + ucoo::bytes_unpack (pos.y, 0), + }; + send (buf, sizeof (buf)); +} + diff --git a/digital/io-hub/src/common-cc/beacon.hh b/digital/io-hub/src/common-cc/beacon.hh new file mode 100644 index 00000000..c84e9c8c --- /dev/null +++ b/digital/io-hub/src/common-cc/beacon.hh @@ -0,0 +1,64 @@ +#ifndef beacon_hh +#define beacon_hh +// io-hub - Modular Input/Output. {{{ +// +// Copyright (C) 2013 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 "i2c_queue.hh" +#include "defs.hh" + +/// Interface to beacon board. +class Beacon : public I2cQueue::Slave +{ + public: + /// Number of reported positions. + static const int pos_nb = 2; + public: + /// Constructor. + Beacon (I2cQueue &queue); + /// See I2cQueue::Slave::recv_status. + void recv_status (const uint8_t *status); + /// Turn on (true) or off (false). + void on (bool state = true) { on_ = state; } + /// Give number of opponents robots. + void set_robots_nb (int robots_nb) { robots_nb_ = robots_nb; } + /// Send my position along with other informations. + void send_position (const vect_t &pos); + /// Get a detected position, return trust (0 for invalid, 100 for full + /// trust). + int get_position (int index, vect_t &pos) + { + pos = pos_[index]; + return trust_[index]; + } + private: + /// To send: beacon system powered. + bool on_; + /// To send: number of opponents. + int robots_nb_; + /// Received obstacles position. + vect_t pos_[pos_nb]; + /// Received trust. + int trust_[pos_nb]; +}; + +#endif // beacon_hh -- cgit v1.2.3