summaryrefslogtreecommitdiff
path: root/n
diff options
context:
space:
mode:
authorschodet2005-02-27 16:50:20 +0000
committerschodet2005-02-27 16:50:20 +0000
commitf295f6a382110cb03441b4683449621e9ea3660f (patch)
tree570e3f4e43afb7770c362c68deacccc610db6238 /n
parent168504854d0c4fdd46d9020f37cba64f6b944bec (diff)
Ajout de code de debug pour goto.
Diffstat (limited to 'n')
-rw-r--r--n/asserv/src/goto.c17
-rw-r--r--n/asserv/src/main.c26
2 files changed, 30 insertions, 13 deletions
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index 1a5e73f..dc8cc85 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -29,6 +29,8 @@ int32_t goto_x, goto_y;
int32_t goto_a;
/** Destination epsillon. */
int32_t goto_eps = 20L << 8;
+/** Debug values. */
+int32_t goto_dx, goto_dy, goto_dl, goto_da;
/* +AutoDec */
/* -AutoDec */
@@ -37,11 +39,12 @@ int32_t goto_eps = 20L << 8;
static inline void
goto_update (void)
{
- int32_t dx, dy, c, s, dl, da;
+ int32_t c, s;
/* Project in the robot base. */
- dx = goto_x - postrack_x; /* f24.8 */
- dy = goto_y - postrack_y;
- if (dx < goto_eps && dx > -goto_eps && dy < goto_eps && dy > -goto_eps)
+ goto_dx = goto_x - postrack_x; /* f24.8 */
+ goto_dy = goto_y - postrack_y;
+ if (goto_dx < goto_eps && goto_dx > -goto_eps
+ && goto_dy < goto_eps && goto_dy > -goto_eps)
{
speed_left_aim = 0;
speed_right_aim = 0;
@@ -50,11 +53,11 @@ goto_update (void)
{
c = dsp_cos (postrack_a);
s = dsp_sin (postrack_a);
- dl = dsp_mul_f824 (dx, c) + dsp_mul_f824 (dy, s);
- da = dsp_mul_f824 (dy, c) - dsp_mul_f824 (dx, s);
+ goto_dl = dsp_mul_f824 (goto_dx, c) + dsp_mul_f824 (goto_dy, s);
+ goto_da = dsp_mul_f824 (goto_dy, c) - dsp_mul_f824 (goto_dx, s);
/* Convert da into a arc. This is a rough aproximation. */
//da = da * postrack_footing / 2 / (dl >> 8);
- speed_distance (dl, da);
+ speed_distance (goto_dl, goto_da);
}
}
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index 9033372..d8ede11 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -45,6 +45,15 @@ int8_t motor_mode;
/** Main loop counter. */
uint8_t motor_loop_cpt;
+/** Report of counters. */
+uint8_t motor_stat_counter, motor_stat_counter_cpt;
+
+/** Report position. */
+uint8_t motor_stat_postrack, motor_stat_postrack_cpt;
+
+/** Goto debug. */
+uint8_t motor_stat_goto_debug, motor_stat_goto_debug_cpt;
+
/** Statistics about speed control. */
uint8_t motor_stat_speed, motor_stat_speed_cpt;
@@ -54,12 +63,6 @@ uint8_t motor_stat_pwm, motor_stat_pwm_cpt;
/** Report of timer. */
uint8_t motor_stat_timer, motor_stat_timer_cpt;
-/** Report of counters. */
-uint8_t motor_stat_counter, motor_stat_counter_cpt;
-
-/** Report position. */
-uint8_t motor_stat_postrack, motor_stat_postrack_cpt;
-
/** Record timer value at different stage of computing. Used for performance
* analisys. */
uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3,
@@ -133,6 +136,11 @@ main_loop (void)
proto_send1d ('A', postrack_a);
motor_stat_postrack_cpt = motor_stat_postrack;
}
+ if (motor_stat_goto_debug && !--motor_stat_goto_debug_cpt)
+ {
+ proto_send4d ('G', goto_dx, goto_dy, goto_dl, goto_da);
+ motor_stat_goto_debug_cpt = motor_stat_goto_debug;
+ }
if (motor_stat_speed && !--motor_stat_speed_cpt)
{
proto_send4w ('S', speed_left_e_old, speed_left_int,
@@ -170,6 +178,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('z', 0):
reset ();
break;
+ /* Commands. */
case c ('g', 8):
motor_mode = 2;
goto_x = (uint32_t) args[0] << 24 | (uint32_t) args[1] << 16
@@ -205,6 +214,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
pwm_left = args[0] << 8 | args[1];
pwm_right = args[2] << 8 | args[3];
break;
+ /* Params. */
case c ('e', 4):
goto_eps = (uint32_t) args[0] << 24 | (uint32_t) args[1] << 16
| args[2] << 8 | args[3];
@@ -221,12 +231,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('f', 2):
postrack_set_footing (args[0] << 8 | args[1]);
break;
+ /* Stats. */
case c ('C', 1):
motor_stat_counter_cpt = motor_stat_counter = args[0];
break;
case c ('X', 1):
motor_stat_postrack_cpt = motor_stat_postrack = args[0];
break;
+ case c ('G', 1):
+ motor_stat_goto_debug_cpt = motor_stat_goto_debug = args[0];
+ break;
case c ('S', 1):
motor_stat_speed_cpt = motor_stat_speed = args[0];
break;