From 8314a191a83c606e9555e53d3c310bef05ef7731 Mon Sep 17 00:00:00 2001 From: Jérémy Dufour Date: Wed, 30 Apr 2008 21:13:29 +0200 Subject: * digital/io/src - add debug function to output messages under host ; - add some debug message into the move FSM. --- digital/io/src/debug.host.h | 41 +++++++++++++++++++++++++++++++++++++++++ digital/io/src/move_cb.c | 6 ++++++ 2 files changed, 47 insertions(+) create mode 100644 digital/io/src/debug.host.h (limited to 'digital/io/src') diff --git a/digital/io/src/debug.host.h b/digital/io/src/debug.host.h new file mode 100644 index 00000000..f4c9b72f --- /dev/null +++ b/digital/io/src/debug.host.h @@ -0,0 +1,41 @@ +#ifndef debug_host_h +#define debug_host_h +/* debug.host.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2008 Dufour Jérémy + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ + +/** + * @file Debug printf functions. + * This module include a macro to add some debug functions to printf a message + * under host target. It does nothing under avr target. + */ + +#ifdef HOST +#include +#define DPRINTF(format, args...) do { printf ((format), ## args); } while (0) +#else /* HOST */ +#define DPRINTF(format, args...) +#endif /* HOST */ + +#endif /* debug_host_h */ diff --git a/digital/io/src/move_cb.c b/digital/io/src/move_cb.c index 7254a504..74a303a3 100644 --- a/digital/io/src/move_cb.c +++ b/digital/io/src/move_cb.c @@ -34,6 +34,8 @@ #include "main.h" /* main_post_event_for_top_fsm */ #include "modules/math/fixed/fixed.h" /* fixed_* */ +#include "debug.host.h" + /** * The real radius of the obstacle. */ @@ -91,11 +93,13 @@ move_get_next_position (move_position_t *dst) /* Retrieve next path coordinate */ if (!path_get_next (&dst->x, &dst->y)) { + DPRINTF ("Could not compute any path to avoid obstacle!\n"); /* If it failed, try original destination */ dst->x = move_data.final.x; dst->y = move_data.final.y; } main_sharp_ignore_event = MOVE_MAIN_IGNORE_SHARP_EVENT; + DPRINTF ("Computed path is (%d ; %d)\n", dst->x, dst->y); } /** @@ -113,12 +117,14 @@ move_compute_obstacle_position (asserv_position_t cur, if (move_data.backward_movement_allowed) angle += 0x8000; angle = angle << 8; + DPRINTF ("We are at (%d ; %d ; %x)\n", cur.x, cur.y, cur.a); /* X */ obstacle->x = cur.x + fixed_mul_f824 (fixed_cos_f824 (angle), MOVE_OBSTACLE_DISTANCE); /* Y */ obstacle->y = cur.y + fixed_mul_f824 (fixed_sin_f824 (angle), MOVE_OBSTACLE_DISTANCE); + DPRINTF ("Computed obstacle (%d ; %d)\n", obstacle->x, obstacle->y); } /** * Unique function to compute the obstacle position from here. -- cgit v1.2.3