From cbdfded9991dc3e01da3f70cd81b854694c0f769 Mon Sep 17 00:00:00 2001 From: schodet Date: Sat, 9 Apr 2005 21:30:07 +0000 Subject: footing pour Taz, 0.05/256 d'erreur d'angle !! --- n/asserv/src/avrconfig.h | 4 ++-- n/asserv/src/goto.c | 57 ++++++++++++++++++++++++++++++++++++++++++++++++ n/asserv/src/main.c | 22 +++++++++++++++++++ n/asserv/src/postrack.c | 6 ++++- n/asserv/src/pwm.c | 2 +- n/asserv/src/speed.c | 8 +++++++ 6 files changed, 95 insertions(+), 4 deletions(-) (limited to 'n/asserv') diff --git a/n/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h index be35d22..a96d7ef 100644 --- a/n/asserv/src/avrconfig.h +++ b/n/asserv/src/avrconfig.h @@ -37,9 +37,9 @@ /** Send mode : * - POLLING : no interrupts; * - RING : interrupts, ring buffer. */ -#define AC_RS232_SEND_MODE POLLING +#define AC_RS232_SEND_MODE RING /** Recv mode, same as send mode. */ -#define AC_RS232_RECV_MODE POLLING +#define AC_RS232_RECV_MODE RING /** Character size : 5, 6, 7, 8, 9 (only 8 implemented). */ #define AC_RS232_CHAR_SIZE 8 /** Parity : ODD, EVEN, NONE. */ diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index bd57d85..8aa5e91 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -27,6 +27,7 @@ * 0: linear move. * 1: angular move. * 2: position move. + * 10: we fuck the wall. */ uint8_t goto_mode; /** Distance for mode 0, ui16. */ @@ -37,6 +38,8 @@ uint8_t goto_sign; int32_t goto_x, goto_y; /** Destination angle, f0.8. */ int8_t goto_a; +/** Travel speed for fixed speed movements, i8. */ +int8_t goto_s; /** Destination epsillon. */ int32_t goto_eps = 20L << 8; /** Debug values. */ @@ -91,6 +94,32 @@ goto_angular_mode (void) /** Position control mode. */ static inline void goto_position_mode (void) +{ + int32_t c, s; + /* Project in the robot base. */ + 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; + } + else + { + c = dsp_cos_f824 (postrack_a); + s = dsp_sin_f824 (postrack_a); + 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. */ + goto_da = goto_da * (postrack_footing / 2) / (goto_dl >> 8); + speed_distance (goto_dl / 2, goto_da * 2); + } +} + +/** Position control mode, `exact' method. */ +static inline void +goto_position_exact_mode (void) { int32_t c, s; /* Project in the robot base. */ @@ -114,6 +143,31 @@ goto_position_mode (void) } } +/** We fuck the wall mode. */ +static inline void +goto_ftw_mode (void) +{ + /* Change speed. */ + if (PINC & _BV (0)) + { + speed_left_aim = goto_s; + } + else + { + speed_left = 0; + speed_left_aim = 0; + } + if (PINC & _BV (7)) + { + speed_right_aim = goto_s; + } + else + { + speed_right = 0; + speed_right_aim = 0; + } +} + /** Update the speed according to the desired destination. */ static inline void goto_update (void) @@ -129,6 +183,9 @@ goto_update (void) case 2: goto_position_mode (); break; + case 10: + goto_ftw_mode (); + break; } } diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index d119e4b..fb6b2d4 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -64,6 +64,9 @@ uint8_t motor_stat_pwm, motor_stat_pwm_cpt; /** Report of timer. */ uint8_t motor_stat_timer, motor_stat_timer_cpt; +/** Print PINC. */ +uint8_t motor_print_pinc, motor_print_pinc_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, @@ -159,6 +162,11 @@ main_loop (void) motor_timer_1, motor_timer_0); motor_stat_timer_cpt = motor_stat_timer; } + if (motor_print_pinc && !--motor_print_pinc_cpt) + { + proto_send1b ('P', PINC); + motor_print_pinc_cpt = motor_print_pinc; + } /* Misc. */ if ((motor_loop_cpt & 7) == 0) { @@ -200,6 +208,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]); goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]); break; + case c ('f', 1): + motor_mode = 2; + goto_mode = 10; + goto_s = args[0]; + break; case c ('s', 0): motor_mode = 1; speed_left_aim = 0; @@ -246,6 +259,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('T', 1): motor_stat_timer_cpt = motor_stat_timer = args[0]; break; + case c ('P', 1): + motor_print_pinc_cpt = motor_print_pinc = args[0]; + break; default: /* Params. */ if (cmd == 'p') @@ -273,6 +289,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('i', 3): speed_ki = v8_to_v16 (args[1], args[2]); break; + case c ('d', 3): + speed_dead_zone = v8_to_v16 (args[1], args[2]); + break; + case c ('I', 3): + speed_int_max = v8_to_v16 (args[1], args[2]); + break; case c ('a', 2): speed_acc_cpt = speed_acc = args[1]; break; diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c index ae0d0c3..3cca141 100644 --- a/n/asserv/src/postrack.c +++ b/n/asserv/src/postrack.c @@ -60,7 +60,11 @@ postrack_set_footing (uint16_t footing); static inline void postrack_init (void) { - postrack_set_footing (5142); + // Mesured : 2667. + // Correction : / 1.00708 -> 6223 (184F). + // Correction : / 1.00210 -> 6209.9107 (1842). + // Correction : -> 6216 (1848). + postrack_set_footing (0x1842); } #define M_PI 3.14159265358979323846 /* pi */ diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c index 6e90e65..9452077 100644 --- a/n/asserv/src/pwm.c +++ b/n/asserv/src/pwm.c @@ -34,7 +34,7 @@ /** Define the forward direction logic for left motor. */ #define PWM_LEFT_DIR_FRW 0 /** Define the forward direction logic for right motor. */ -#define PWM_RIGHT_DIR_FRW 0 +#define PWM_RIGHT_DIR_FRW 1 /** PWM values. */ int16_t pwm_left, pwm_right; diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c index 38842a1..7fdb0dc 100644 --- a/n/asserv/src/speed.c +++ b/n/asserv/src/speed.c @@ -43,6 +43,8 @@ int16_t speed_left_e_old, speed_right_e_old; uint16_t speed_kp = 5 * 255; /** I coeficients. 4.8 fixed point format. */ uint16_t speed_ki = 1 * 255; +/** Dead zone. */ +uint16_t speed_dead_zone = 0; /* +AutoDec */ @@ -114,6 +116,9 @@ speed_compute_left_pwm (void) int16_t e; int16_t pwm; e = speed_left - counter_left_diff; /* 10b = 8b + 9b */ + /* Dead zone. */ + if (speed_dead_zone > e && e > -speed_dead_zone) + e = 0; /* Integral update. */ speed_left_int += e; /* 12b = 11b + 10b */ if (speed_left_int > speed_int_max) /* 11b */ @@ -135,6 +140,9 @@ speed_compute_right_pwm (void) int16_t e; int16_t pwm; e = speed_right - counter_right_diff; + /* Dead zone. */ + if (speed_dead_zone > e && e > -speed_dead_zone) + e = 0; /* Integral update. */ speed_right_int += e; if (speed_right_int > speed_int_max) -- cgit v1.2.3