From 2f3cb1d7ea6addc9e98b3e324281b2bc17459ef9 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 30 Apr 2008 18:28:25 +0200 Subject: * digital/io/src - add a command to get statistics on the position of the bot. --- digital/io/src/main.c | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) (limited to 'digital/io/src') diff --git a/digital/io/src/main.c b/digital/io/src/main.c index d1be0dfd..fbb7ce6c 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -81,6 +81,11 @@ uint16_t main_sharp_ignore_event; uint8_t main_stats_sharps, main_stats_sharps_cpt; uint8_t main_stats_sharps_interpreted_, main_stats_sharps_interpreted_cpt_; +/** + * Asserv stats counters. + */ +static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; + /** * Update frequency of sharps. */ @@ -324,6 +329,18 @@ main_loop (void) } proto_send ('I', SHARP_NUMBER, cache); } + + /* Send asserv stats if needed */ + if (main_stats_asserv_ && !--main_stats_asserv_cpt_) + { + /* Get current position */ + asserv_position_t cur_pos; + asserv_get_position (&cur_pos); + /* Send stats */ + proto_send3w ('A', cur_pos.x, cur_pos.y, cur_pos.a); + /* Reset stats counter */ + main_stats_asserv_cpt_ = main_stats_asserv_; + } } } @@ -465,6 +482,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) getsamples_start (args[0] << 8, args[1]); break; + case c ('A', 1): + { + /* Get position stats + * - 1b: frequency. + */ + main_stats_asserv_ = main_stats_asserv_cpt_ = args[0]; + } + break; + /* Asserv/arm */ case c ('a', 1): { -- cgit v1.2.3