summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorNicolas Schodet2013-04-01 18:30:49 +0200
committerNicolas Schodet2013-04-01 18:30:49 +0200
commit40bcff0eb45ffae6790a86344e4a16b654e01d79 (patch)
treec848e094d8eb65348bde6ef8dea9deb4a9dacdc1
parentaaa32bad8b1cfaa6c2334e2be0d5f34450c07f53 (diff)
digital/io-hub/src/apbirthday: add asserv position stats
-rw-r--r--digital/io-hub/src/apbirthday/robot.cc15
-rw-r--r--digital/io-hub/src/apbirthday/robot.hh2
2 files changed, 16 insertions, 1 deletions
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.