From f295f6a382110cb03441b4683449621e9ea3660f Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 27 Feb 2005 16:50:20 +0000 Subject: Ajout de code de debug pour goto. --- n/asserv/src/goto.c | 17 ++++++++++------- n/asserv/src/main.c | 26 ++++++++++++++++++++------ 2 files changed, 30 insertions(+), 13 deletions(-) (limited to 'n') 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; -- cgit v1.2.3