From b3392b2c9b5e808e868b092422d7b143a82c4628 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 31 Mar 2013 19:28:24 +0200 Subject: digital/io-hub/src/apbirthday: add radar system and obstacles handling --- digital/io-hub/src/apbirthday/Makefile | 3 +- digital/io-hub/src/apbirthday/hardware.hh | 6 ++++ digital/io-hub/src/apbirthday/hardware.host.cc | 3 +- digital/io-hub/src/apbirthday/radar_2013.cc | 50 ++++++++++++++++++++++++++ digital/io-hub/src/apbirthday/radar_2013.hh | 40 +++++++++++++++++++++ digital/io-hub/src/apbirthday/robot.cc | 9 ++++- digital/io-hub/src/apbirthday/robot.hh | 6 ++++ 7 files changed, 114 insertions(+), 3 deletions(-) create mode 100644 digital/io-hub/src/apbirthday/radar_2013.cc create mode 100644 digital/io-hub/src/apbirthday/radar_2013.hh (limited to 'digital/io-hub/src/apbirthday') diff --git a/digital/io-hub/src/apbirthday/Makefile b/digital/io-hub/src/apbirthday/Makefile index 17254322..5611319d 100644 --- a/digital/io-hub/src/apbirthday/Makefile +++ b/digital/io-hub/src/apbirthday/Makefile @@ -3,9 +3,10 @@ BASE = ../../../ucoolib TARGETS = host stm32f4 PROGS = apbirthday apbirthday_SOURCES = main.cc robot.cc hardware.host.cc hardware.stm32.cc \ - zb_avrisp.stm32.cc \ + simu_report.host.cc zb_avrisp.stm32.cc \ i2c_queue.cc asserv.cc mimot.cc \ pressure.cc chrono.host.cc chrono.stm32.cc \ + radar.cc radar_2013.cc obstacles.cc \ outputs.cc \ top.cc init.cc move.cc candles.cc \ angfsm.host.c angfsm_gen_arm_AI.arm.c fsm_queue.cc \ diff --git a/digital/io-hub/src/apbirthday/hardware.hh b/digital/io-hub/src/apbirthday/hardware.hh index 55caea40..63665222 100644 --- a/digital/io-hub/src/apbirthday/hardware.hh +++ b/digital/io-hub/src/apbirthday/hardware.hh @@ -33,6 +33,9 @@ #else # include "ucoolib/arch/host/host_stream.hh" #endif +#ifdef TARGET_host +# include "simu_report.host.hh" +#endif #ifdef TARGET_host # include "ucoolib/arch/host/host.hh" @@ -94,6 +97,9 @@ struct Hardware #else ucoo::AdcHost adc_dist0, adc_dist1, adc_dist2, adc_dist3; ucoo::AdcHost adc_pressure; +#endif +#ifdef TARGET_host + SimuReport simu_report; #endif Hardware (); // Wait until next cycle. diff --git a/digital/io-hub/src/apbirthday/hardware.host.cc b/digital/io-hub/src/apbirthday/hardware.host.cc index a53962b7..39fe110b 100644 --- a/digital/io-hub/src/apbirthday/hardware.host.cc +++ b/digital/io-hub/src/apbirthday/hardware.host.cc @@ -66,7 +66,8 @@ Hardware::Hardware () adc_dist1 (host, "dist1", 1 << 12), adc_dist2 (host, "dist2", 1 << 12), adc_dist3 (host, "dist3", 1 << 12), - adc_pressure (host, "pressure", 1 << 12) + adc_pressure (host, "pressure", 1 << 12), + simu_report (host) { dev_uart.block (false); zb_uart.block (false); diff --git a/digital/io-hub/src/apbirthday/radar_2013.cc b/digital/io-hub/src/apbirthday/radar_2013.cc new file mode 100644 index 00000000..5cbbe0ca --- /dev/null +++ b/digital/io-hub/src/apbirthday/radar_2013.cc @@ -0,0 +1,50 @@ +// 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 "radar_2013.hh" +#include "playground.hh" + +RadarSensor sensors[] = { + { 0, { 102, 84 }, G_ANGLE_UF016_DEG (0), true }, + { 0, { 102, -84 }, G_ANGLE_UF016_DEG (0), false }, + { 0, { -78, 104 }, G_ANGLE_UF016_DEG (180), true }, + { 0, { -83, -120 }, G_ANGLE_UF016_DEG (180), false }, +}; + +Radar2013::Radar2013 (ucoo::UsDist &dist0, ucoo::UsDist &dist1, + ucoo::UsDist &dist2, ucoo::UsDist &dist3) + : Radar (150, sensors, lengthof (sensors)) +{ + sensors[0].sensor = &dist0; + sensors[1].sensor = &dist1; + sensors[2].sensor = &dist2; + sensors[3].sensor = &dist3; +} + +bool +Radar2013::valid (int sensor_index, vect_t &p) +{ + return p.x >= margin_mm && p.x < pg_width - margin_mm + && p.y >= margin_mm && p.y < pg_length - margin_mm; +} + diff --git a/digital/io-hub/src/apbirthday/radar_2013.hh b/digital/io-hub/src/apbirthday/radar_2013.hh new file mode 100644 index 00000000..d865fbca --- /dev/null +++ b/digital/io-hub/src/apbirthday/radar_2013.hh @@ -0,0 +1,40 @@ +#ifndef radar_2013_hh +#define radar_2013_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 "radar.hh" + +/// APBirthday specific radar. +class Radar2013 : public Radar +{ + public: + /// Constructor. + Radar2013 (ucoo::UsDist &dist0, ucoo::UsDist &dist1, + ucoo::UsDist &dist2, ucoo::UsDist &dist3); + protected: + /// See Radar::valid. + bool valid (int sensor_index, vect_t &p); +}; + +#endif // radar_2013_hh diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 70b70e98..a40ca160 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -44,6 +44,7 @@ Robot::Robot () usdist1_ (usdist_control_, hardware.adc_dist1, hardware.dist1_sync, 100, 700, 650), usdist2_ (usdist_control_, hardware.adc_dist2, hardware.dist2_sync, 100, 700, 650), usdist3_ (usdist_control_, hardware.adc_dist3, hardware.dist3_sync, 100, 700, 650), + radar_ (usdist0_, usdist1_, usdist2_, usdist3_), candles (1), fsm_debug_state_ (FSM_DEBUG_RUN), outputs_set_ (outputs_, lengthof (outputs_)), @@ -108,7 +109,13 @@ Robot::main_loop () // Wait until next cycle. hardware.wait (); // Update IO modules. - usdist_control_.update (); + if (usdist_control_.update ()) + { + Position robot_pos; + asserv.get_position (robot_pos); + radar_.update (robot_pos, obstacles); + } + obstacles.update (); pressure.update (); outputs_set_.update (); // Handle communications. diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh index aaac185b..35b2266a 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -29,6 +29,8 @@ #include "chrono.hh" #include "pressure.hh" #include "outputs.hh" +#include "radar_2013.hh" +#include "obstacles.hh" #include "candles.hh" #include "ucoolib/base/proto/proto.hh" @@ -74,7 +76,11 @@ class Robot : public ucoo::Proto::Handler ucoo::UsDistControl usdist_control_; /// US distance sensors. ucoo::UsDist usdist0_, usdist1_, usdist2_, usdist3_; + /// Radar. + Radar2013 radar_; public: + /// Obstacle database. + Obstacles obstacles; /// Candles. Candles candles; private: -- cgit v1.2.3