summaryrefslogtreecommitdiffhomepage
path: root/digital
diff options
context:
space:
mode:
Diffstat (limited to 'digital')
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc9
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh2
-rw-r--r--digital/io-hub/src/apbirthday/top.cc7
-rw-r--r--digital/io-hub/src/common-cc/lcd.cc72
-rw-r--r--digital/io-hub/src/common-cc/lcd.hh14
-rw-r--r--digital/io-hub/src/common-cc/obstacles.cc9
6 files changed, 108 insertions, 5 deletions
diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc
index 64f2c93f..f99351dc 100644
--- a/digital/io-hub/src/apbirthday/robot.cc
+++ b/digital/io-hub/src/apbirthday/robot.cc
@@ -72,7 +72,8 @@ Robot::Robot ()
stats_proto_ (0),
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), stats_rgb_ (0)
+ stats_inputs_ (0), stats_usdist_ (0), stats_cake_ (0), stats_pressure_ (0), stats_rgb_ (0),
+ lcd_send_position_cpt_ (1)
{
robot = this;
// Fill I/O arrays.
@@ -169,8 +170,14 @@ Robot::main_loop ()
zb_i2c_queue_.sync ();
if (!ended)
{
+ lcd.chrono (chrono.remaining_time_ms () / 1000);
Position robot_pos = asserv.get_position ();
beacon.send_position (robot_pos.v);
+ if (!--lcd_send_position_cpt_)
+ {
+ lcd.robot_position (robot_pos);
+ lcd_send_position_cpt_ = 125;
+ }
// Look for obstacles.
for (int i = 0; i < Beacon::pos_nb; i++)
{
diff --git a/digital/io-hub/src/apbirthday/robot.hh b/digital/io-hub/src/apbirthday/robot.hh
index 85b67112..0b585b6d 100644
--- a/digital/io-hub/src/apbirthday/robot.hh
+++ b/digital/io-hub/src/apbirthday/robot.hh
@@ -172,6 +172,8 @@ class Robot : public ucoo::Proto::Handler
int stats_pressure_, stats_pressure_cpt_;
/// RGB stats interval, counter and type.
int stats_rgb_, stats_rgb_cpt_, stats_rgb_type_;
+ /// LCD position counter.
+ int lcd_send_position_cpt_;
};
/// Global instance pointer.
diff --git a/digital/io-hub/src/apbirthday/top.cc b/digital/io-hub/src/apbirthday/top.cc
index fef923f3..e0be0bc0 100644
--- a/digital/io-hub/src/apbirthday/top.cc
+++ b/digital/io-hub/src/apbirthday/top.cc
@@ -386,6 +386,8 @@ FSM_TRANS (TOP_START, init_actuators, TOP_INIT_ACTUATORS)
{
// TODO: make sure the operator do not forget this is demo mode!
robot->demo = !robot->hardware.ihm_strat.get ();
+ if (robot->demo)
+ robot->lcd.message ("demo mode");
robot->pressure.set (BOT_NORMAL_PRESSURE);
}
@@ -396,6 +398,11 @@ FSM_TRANS (TOP_INIT_ACTUATORS, init_done, TOP_INIT)
// Color dependent init can go here.
robot->gifts.compute_pos ();
robot->strat.color_init ();
+ if (team_color)
+ robot->lcd.team_color (0, 0, 255);
+ else
+ robot->lcd.team_color (255, 0, 0);
+ robot->lcd.message ("ready");
}
FSM_TRANS (TOP_INIT, init_start_round, TOP_DECISION)
diff --git a/digital/io-hub/src/common-cc/lcd.cc b/digital/io-hub/src/common-cc/lcd.cc
index 9ed05691..a04574bf 100644
--- a/digital/io-hub/src/common-cc/lcd.cc
+++ b/digital/io-hub/src/common-cc/lcd.cc
@@ -23,8 +23,12 @@
// }}}
#include "lcd.hh"
+#include "ucoolib/utils/bytes.hh"
+
+#include <cstring>
+
LCD::LCD (I2cQueue &queue)
- : I2cQueue::Slave (queue, 0x20, 1)
+ : I2cQueue::Slave (queue, 0x20, 1), last_chrono_sent_ (-1)
{
}
@@ -40,3 +44,69 @@ LCD::team_color (uint8_t r, uint8_t g, uint8_t b)
send (buf, sizeof (buf));
}
+void
+LCD::chrono (int s)
+{
+ if (s != last_chrono_sent_)
+ {
+ last_chrono_sent_ = s;
+ uint8_t t = s > 0 ? s : 0;
+ uint8_t buf[] = { 't', t };
+ send (buf, sizeof (buf));
+ }
+}
+
+void
+LCD::message (const char *msg)
+{
+ uint8_t buf[16];
+ buf[0] = 'm';
+ unsigned msg_len = std::strlen (msg) + 1;
+ if (msg_len > sizeof (buf) - 1)
+ msg_len = sizeof (buf) - 1;
+ std::memcpy (buf + 1, msg, msg_len);
+ buf[sizeof (buf) - 1] = 0;
+ send (buf, 1 + msg_len);
+}
+
+void
+LCD::robot_position (const Position &pos)
+{
+ uint8_t buf[] = { 'p',
+ ucoo::bytes_unpack (pos.v.x, 1),
+ ucoo::bytes_unpack (pos.v.x, 0),
+ ucoo::bytes_unpack (pos.v.y, 1),
+ ucoo::bytes_unpack (pos.v.y, 0),
+ };
+ send (buf, sizeof (buf));
+}
+
+void
+LCD::obstacles (const vect_t *pos, int pos_nb)
+{
+ uint8_t buf[2 + 4 * 4];
+ int i = 0;
+ buf[i++] = 'o';
+ buf[i++] = pos_nb;
+ for (int o = 0; o < pos_nb; o++)
+ {
+ buf[i++] = ucoo::bytes_unpack (pos[o].x, 1);
+ buf[i++] = ucoo::bytes_unpack (pos[o].x, 0);
+ buf[i++] = ucoo::bytes_unpack (pos[o].y, 1);
+ buf[i++] = ucoo::bytes_unpack (pos[o].y, 0);
+ }
+ send (buf, i);
+}
+
+void
+LCD::target (const vect_t &pos)
+{
+ uint8_t buf[] = { 'n',
+ 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/lcd.hh b/digital/io-hub/src/common-cc/lcd.hh
index 2c46b281..65a104af 100644
--- a/digital/io-hub/src/common-cc/lcd.hh
+++ b/digital/io-hub/src/common-cc/lcd.hh
@@ -25,6 +25,8 @@
// }}}
#include "i2c_queue.hh"
+#include "defs.hh"
+
/// Interface to LCD board.
class LCD : public I2cQueue::Slave
{
@@ -35,6 +37,18 @@ class LCD : public I2cQueue::Slave
void recv_status (const uint8_t *status);
/// Send team color.
void team_color (uint8_t r, uint8_t g, uint8_t b);
+ /// Send chrono if changed.
+ void chrono (int s);
+ /// Send message.
+ void message (const char *msg);
+ /// Send robot position.
+ void robot_position (const Position &pos);
+ /// Send obstacles.
+ void obstacles (const vect_t *pos, int pos_nb);
+ /// Send target position.
+ void target (const vect_t &pos);
+ private:
+ int last_chrono_sent_;
};
#endif // lcd_hh
diff --git a/digital/io-hub/src/common-cc/obstacles.cc b/digital/io-hub/src/common-cc/obstacles.cc
index 3a778141..26945137 100644
--- a/digital/io-hub/src/common-cc/obstacles.cc
+++ b/digital/io-hub/src/common-cc/obstacles.cc
@@ -24,6 +24,8 @@
#include "obstacles.hh"
#include "bot.hh"
+#include "robot.hh"
+
extern "C" {
#include "modules/math/geometry/distance.h"
}
@@ -51,10 +53,8 @@ Obstacles::update ()
changed_ = true;
}
}
-#ifdef TARGET_host
if (changed_)
{
- SimuReport &r = robot->hardware.simu_report;
vect_t o[obstacles_nb_];
int o_nb = 0;
for (int i = 0; i < obstacles_nb_; i++)
@@ -62,9 +62,12 @@ Obstacles::update ()
if (obstacles_[i].valid)
o[o_nb++] = obstacles_[i].pos;
}
+#ifdef TARGET_host
+ SimuReport &r = robot->hardware.simu_report;
r.pos (o, o_nb, 0);
- }
#endif
+ robot->lcd.obstacles (o, o_nb);
+ }
changed_ = false;
}