From 40bcff0eb45ffae6790a86344e4a16b654e01d79 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 1 Apr 2013 18:30:49 +0200 Subject: digital/io-hub/src/apbirthday: add asserv position stats --- digital/io-hub/src/apbirthday/robot.cc | 15 ++++++++++++++- digital/io-hub/src/apbirthday/robot.hh | 2 ++ 2 files changed, 16 insertions(+), 1 deletion(-) (limited to 'digital/io-hub/src') diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index a40ca160..35dc891a 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -49,7 +49,7 @@ Robot::Robot () fsm_debug_state_ (FSM_DEBUG_RUN), outputs_set_ (outputs_, lengthof (outputs_)), stats_proto_ (0), - stats_chrono_ (false), stats_chrono_last_s_ (-1), + stats_asserv_ (0), stats_chrono_ (false), stats_chrono_last_s_ (-1), stats_inputs_ (0), stats_usdist_ (0), stats_pressure_ (0) { robot = this; @@ -231,6 +231,12 @@ Robot::proto_handle (ucoo::Proto &proto, char cmd, const uint8_t *args, int size ucoo::bytes_pack (args[4], args[5])); } break; + case c ('A', 1): + // Asserv position stats. + // 1B: stat interval. + stats_asserv_cpt_ = stats_asserv_ = args[0]; + stats_proto_ = &proto; + break; case c ('C', 1): // Chrono stats. // 1B: start chrono if non-zero. @@ -270,6 +276,13 @@ Robot::proto_stats () { if (!stats_proto_) return; + if (stats_asserv_ && !--stats_asserv_cpt_) + { + Position pos; + asserv.get_position (pos); + stats_proto_->send ('A', "hhH", pos.v.x, pos.v.y, pos.a); + stats_asserv_cpt_ = stats_asserv_; + } 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 35b2266a..1e3a1c97 100644 --- a/digital/io-hub/src/apbirthday/robot.hh +++ b/digital/io-hub/src/apbirthday/robot.hh @@ -103,6 +103,8 @@ class Robot : public ucoo::Proto::Handler Outputs outputs_set_; /// Proto used for stats. ucoo::Proto *stats_proto_; + /// Asserv stats interval and counter. + int stats_asserv_, stats_asserv_cpt_; /// Enable chrono stats. bool stats_chrono_; /// Last stated second. -- cgit v1.2.3