From 361fd769a389650ba5bf3306238eae27bad44aee Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 1 Jun 2012 23:48:04 +0200 Subject: digital/io-hub/src/guybrush: add robot and obstacles position stats --- digital/io-hub/src/guybrush/avrconfig.h | 2 +- digital/io-hub/src/guybrush/main.c | 34 +++++++++++++++++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/guybrush/avrconfig.h b/digital/io-hub/src/guybrush/avrconfig.h index ea058227..0dc9a04a 100644 --- a/digital/io-hub/src/guybrush/avrconfig.h +++ b/digital/io-hub/src/guybrush/avrconfig.h @@ -49,7 +49,7 @@ /** Stop bits : 1, 2. */ #define AC_UART0_STOP_BITS 1 /** Send buffer size, should be power of 2 for RING mode. */ -#define AC_UART0_SEND_BUFFER_SIZE 32 +#define AC_UART0_SEND_BUFFER_SIZE 64 /** Recv buffer size, should be power of 2 for RING mode. */ #define AC_UART0_RECV_BUFFER_SIZE 32 /** If the send buffer is full when putc: diff --git a/digital/io-hub/src/guybrush/main.c b/digital/io-hub/src/guybrush/main.c index b366e54b..b942b5ea 100644 --- a/digital/io-hub/src/guybrush/main.c +++ b/digital/io-hub/src/guybrush/main.c @@ -97,6 +97,12 @@ enum /** Current FSM debug mode. */ uint8_t main_fsm_debug_mode; +/** Robot position stats. */ +static uint8_t main_stats_pos_ = 75, main_stats_pos_cpt_ = 1; + +/** Obstacles stats. */ +static uint8_t main_stats_obstacles_ = 75, main_stats_obstacles_cpt_ = 1; + /** Asserv stats counters. */ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; @@ -392,6 +398,34 @@ main_loop (void) /* Set as move obstacles. */ move_obstacles_update (); simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); + /* Send stats. */ + if (main_usdist && main_stats_pos_ && !--main_stats_pos_cpt_) + { + proto_send2w ('R', robot_pos.v.x, robot_pos.v.y); + logger_log (0xef, + v16_to_v8 (robot_pos.v.x, 1), + v16_to_v8 (robot_pos.v.x, 0), + v16_to_v8 (robot_pos.v.y, 1), + v16_to_v8 (robot_pos.v.y, 0)); + main_stats_pos_cpt_ = main_stats_pos_; + } + if (main_obstacles_nb + && main_stats_obstacles_ && !--main_stats_obstacles_cpt_) + { + uint8_t i; + for (i = 0; i < main_obstacles_nb; i++) + { + proto_send2w ('O', main_obstacles_pos[i].x, main_obstacles_pos[i].y); + logger_log (0xe0 | i, + v16_to_v8 (main_obstacles_pos[i].x, 1), + v16_to_v8 (main_obstacles_pos[i].x, 0), + v16_to_v8 (main_obstacles_pos[i].y, 1), + v16_to_v8 (main_obstacles_pos[i].y, 0)); + } + main_stats_obstacles_cpt_ = main_stats_obstacles_; + } + else if (main_stats_obstacles_cpt_ > 1) + main_stats_obstacles_cpt_--; } pressure_update (); logger_update (); -- cgit v1.2.3