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/apbirthday/Makefile | 2 +- digital/io-hub/src/apbirthday/hardware.hh | 4 +- digital/io-hub/src/apbirthday/hardware.host.cc | 2 +- digital/io-hub/src/apbirthday/hardware.stm32.cc | 8 +++- digital/io-hub/src/apbirthday/robot.cc | 31 +++++++++++- digital/io-hub/src/apbirthday/robot.hh | 9 +++- digital/io-hub/src/common-cc/beacon.cc | 64 +++++++++++++++++++++++++ digital/io-hub/src/common-cc/beacon.hh | 64 +++++++++++++++++++++++++ 8 files changed, 175 insertions(+), 9 deletions(-) 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') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 5611319d..910f1af3 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -4,7 +4,7 @@ TARGETS = host stm32f4 PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ simu_report.host.cc zb_avrisp.stm32.cc \ - i2c_queue.cc asserv.cc mimot.cc \ + i2c_queue.cc asserv.cc mimot.cc beacon.cc \ pressure.cc chrono.host.cc chrono.stm32.cc \ radar.cc radar_2013.cc obstacles.cc \ outputs.cc \ diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index dbf98e62..577d5c03 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -58,10 +58,10 @@ struct Hardware #ifdef TARGET_stm32 ucoo::UsbStreamControl usb_control; ucoo::UsbStream usb, zb_usb_avrisp; - ucoo::I2cHard main_i2c; + ucoo::I2cHard main_i2c, zb_i2c; #else ucoo::HostStream usb; - ucoo::I2cHost main_i2c; + ucoo::I2cHost main_i2c, zb_i2c; #endif ucoo::Gpio raw_jack, diff --git a/digital/io-hub/src/apbirthday/hardware.host.cc b/digital/io-hub/src/apbirthday/hardware.host.cc index 2a906864..4a2afbb0 100644 --- a/digital/io-hub/src/apbirthday/hardware.host.cc +++ b/digital/io-hub/src/apbirthday/hardware.host.cc @@ -31,7 +31,7 @@ HardwareHost::HardwareHost () Hardware::Hardware () : dev_uart (), zb_uart ("zb_uart"), usb ("usb"), - main_i2c (host, 0), + main_i2c (host, 0), zb_i2c (host, 1), raw_jack (host, "raw_jack"), ihm_color (host, "ihm_color"), ihm_strat (host, "ihm_strat"), diff --git a/digital/io-hub/src/apbirthday/hardware.stm32.cc b/digital/io-hub/src/apbirthday/hardware.stm32.cc index be0c9729..abbfa477 100644 --- a/digital/io-hub/src/apbirthday/hardware.stm32.cc +++ b/digital/io-hub/src/apbirthday/hardware.stm32.cc @@ -33,7 +33,7 @@ Hardware::Hardware () : dev_uart (4), zb_uart (2), usb_control ("APBTeam", "APBirthday"), usb (usb_control, 0), zb_usb_avrisp (usb_control, 1), - main_i2c (2), + main_i2c (2), zb_i2c (1), raw_jack (GPIOD, 12), ihm_color (GPIOD, 14), ihm_strat (GPIOD, 13), @@ -91,6 +91,12 @@ Hardware::Hardware () gpio_set_af (GPIOA, GPIO_AF4, GPIO8); gpio_set_af (GPIOC, GPIO_AF4, GPIO9); main_i2c.enable (); + // zb_i2c + gpio_mode_setup (GPIOB, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO10 | GPIO11); + gpio_set_output_options (GPIOB, GPIO_OTYPE_OD, GPIO_OSPEED_2MHZ, + GPIO10 | GPIO11); + gpio_set_af (GPIOB, GPIO_AF4, GPIO10 | GPIO11); + zb_i2c.enable (); // ADC. gpio_mode_setup (GPIOA, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, GPIO0 | GPIO1 | GPIO2 | GPIO3 | GPIO6 | GPIO7); diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index f76903f9..ced1b0c8 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -31,9 +31,10 @@ Robot *robot; Robot::Robot () - : main_i2c_queue_ (hardware.main_i2c), + : main_i2c_queue_ (hardware.main_i2c), zb_i2c_queue_ (hardware.zb_i2c), asserv (main_i2c_queue_, BOT_SCALE), mimot (main_i2c_queue_), + beacon (zb_i2c_queue_), dev_proto (*this, hardware.dev_uart), zb_proto (*this, hardware.zb_uart), usb_proto (*this, hardware.usb), @@ -49,7 +50,8 @@ Robot::Robot () fsm_debug_state_ (FSM_DEBUG_RUN), outputs_set_ (outputs_, lengthof (outputs_)), stats_proto_ (0), - stats_asserv_ (0), stats_chrono_ (false), stats_chrono_last_s_ (-1), + stats_asserv_ (0), stats_beacon_ (0), + stats_chrono_ (false), stats_chrono_last_s_ (-1), stats_inputs_ (0), stats_usdist_ (0), stats_cake_ (0), stats_pressure_ (0) { robot = this; @@ -109,6 +111,12 @@ Robot::main_loop () // Wait until next cycle. hardware.wait (); // Update IO modules. + if (zb_i2c_queue_.sync ()) + { + Position robot_pos; + asserv.get_position (robot_pos); + beacon.send_position (robot_pos.v); + } if (usdist_control_.update ()) { Position robot_pos; @@ -239,6 +247,14 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size stats_asserv_cpt_ = stats_asserv_ = args[0]; stats_proto_ = &proto; break; + case c ('B', 1): + // Beacon stats. + // 1B: stat interval. + stats_beacon_cpt_ = stats_beacon_ = args[0]; + // TODO: remove this test hack: + beacon.on (args[0] & 1); + stats_proto_ = &proto; + break; case c ('C', 1): // Chrono stats. // 1B: start chrono if non-zero. @@ -305,6 +321,17 @@ Robot::proto_stats () stats_proto_->send ('A', "hhH", pos.v.x, pos.v.y, pos.a); stats_asserv_cpt_ = stats_asserv_; } + if (stats_beacon_ && !--stats_beacon_cpt_) + { + for (int i = 0; i < beacon.pos_nb; i++) + { + vect_t pos; + int trust; + trust = beacon.get_position (i, pos); + stats_proto_->send ('B', "BhhB", i, pos.x, pos.y, trust); + } + stats_beacon_cpt_ = stats_beacon_; + } if (stats_chrono_) { int s = chrono.remaining_time_ms () / 1000; diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index fa2a2185..b9a6d8a6 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -25,6 +25,7 @@ // }}} #include "hardware.hh" #include "asserv.hh" +#include "beacon.hh" #include "fsm_queue.hh" #include "chrono.hh" #include "pressure.hh" @@ -56,13 +57,15 @@ class Robot : public ucoo::Proto::Handler /// Public access to hardware class. Hardware hardware; private: - /// Main I2C queue. - I2cQueue main_i2c_queue_; + /// I2C queues. + I2cQueue main_i2c_queue_, zb_i2c_queue_; public: /// Public access to asserv class. Asserv asserv; /// Public access to mimot class. Mimot mimot; + /// Public access to beacon class. + Beacon beacon; private: /// Proto associated to each serial interface. ucoo::Proto dev_proto, zb_proto, usb_proto; @@ -111,6 +114,8 @@ class Robot : public ucoo::Proto::Handler ucoo::Proto *stats_proto_; /// Asserv stats interval and counter. int stats_asserv_, stats_asserv_cpt_; + /// Beacon stats interval and counter. + int stats_beacon_, stats_beacon_cpt_; /// Enable chrono stats. bool stats_chrono_; /// Last stated second. 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