summaryrefslogtreecommitdiffhomepage
path: root/digital/io
diff options
context:
space:
mode:
Diffstat (limited to 'digital/io')
-rw-r--r--digital/io/src/debug.host.h41
-rw-r--r--digital/io/src/move_cb.c6
2 files changed, 47 insertions, 0 deletions
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 <stdio.h>
+#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.