summaryrefslogtreecommitdiff
path: root/digital/io-hub/src/apbirthday
diff options
context:
space:
mode:
authorNicolas Schodet2013-03-31 19:28:24 +0200
committerNicolas Schodet2013-03-31 19:28:24 +0200
commitb3392b2c9b5e808e868b092422d7b143a82c4628 (patch)
tree0586a54f57e5b392aae8839acf5e2e5e9698c207 /digital/io-hub/src/apbirthday
parent3adb1db9e53ab8d33f25913376c8172d48253e34 (diff)
digital/io-hub/src/apbirthday: add radar system and obstacles handling
Diffstat (limited to 'digital/io-hub/src/apbirthday')
-rw-r--r--digital/io-hub/src/apbirthday/Makefile3
-rw-r--r--digital/io-hub/src/apbirthday/hardware.hh6
-rw-r--r--digital/io-hub/src/apbirthday/hardware.host.cc3
-rw-r--r--digital/io-hub/src/apbirthday/radar_2013.cc50
-rw-r--r--digital/io-hub/src/apbirthday/radar_2013.hh40
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc9
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh6
7 files changed, 114 insertions, 3 deletions
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"
@@ -95,6 +98,9 @@ struct Hardware
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.
void wait ();
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: