From bcee0d3219e9034d8cb6fdfafbe483d684f11016 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 10 May 2013 00:53:51 +0200 Subject: digital/io-hub/src/apbirthday: send informations to LCD --- digital/io-hub/src/apbirthday/robot.cc | 9 +++- digital/io-hub/src/apbirthday/robot.hh | 2 + digital/io-hub/src/apbirthday/top.cc | 7 +++ digital/io-hub/src/common-cc/lcd.cc | 72 ++++++++++++++++++++++++++++++- digital/io-hub/src/common-cc/lcd.hh | 14 ++++++ digital/io-hub/src/common-cc/obstacles.cc | 9 ++-- 6 files changed, 108 insertions(+), 5 deletions(-) (limited to 'digital/io-hub') 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 + 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; } -- cgit v1.2.3