From f0ccf37dce1373d0e3ca70a68772de98ed67d995 Mon Sep 17 00:00:00 2001 From: schodet Date: Wed, 27 Apr 2005 18:37:57 +0000 Subject: Modif des compteurs. Modif de Taz. --- n/asserv/src/counter.c | 79 ++++++++++++++++++++++++++++++++----------------- n/asserv/src/eeprom.c | 2 +- n/asserv/src/goto.c | 8 +++-- n/asserv/src/main.c | 11 ++++++- n/asserv/src/pwm.c | 18 ----------- n/asserv/src/taz.c | 24 +++++++++++---- n/asserv/src/test_pwm.c | 2 -- 7 files changed, 87 insertions(+), 57 deletions(-) diff --git a/n/asserv/src/counter.c b/n/asserv/src/counter.c index 774a9c8..9f8a5d5 100644 --- a/n/asserv/src/counter.c +++ b/n/asserv/src/counter.c @@ -45,10 +45,6 @@ static int16_t counter_left_diff, counter_right_diff; static inline void counter_init (void); -/** Poll counters, should be called as often as possible. */ -static inline void -counter_poll (void); - /** Update overall counter values and compute diffs. */ static inline void counter_update (void); @@ -75,45 +71,44 @@ counter_init (void) 0, 0, 0, 0, 0, 1, 1, 1); /* Begin with safe values. */ counter_restart (); + /* Interrupts for direction. */ + EICRB = 0x05; + EIFR = _BV (4) | _BV (5); + EIMSK = _BV (4) | _BV (5); } -/** Poll counters, should be called as often as possible. */ -static inline void -counter_poll (void) +/** Left direction change. */ +SIGNAL (SIG_INTERRUPT4) { uint8_t c; - /* Read left counter. */ c = TCNT2; -#if COUNTER_REVERSE_LEFT == 0 if (PINE & _BV (4)) -#else - if (!(PINE & _BV (4))) -#endif { - PORTD &= ~0x40; - counter_left_frw += c - counter_left_old; + counter_left_rev += c - counter_left_old; + PORTD |= 0x40; } else { - PORTD |= 0x40; - counter_left_rev += c - counter_left_old; + counter_left_frw += c - counter_left_old; + PORTD &= ~0x40; } counter_left_old = c; - /* Read right counter. */ +} + +/** Right direction change. */ +SIGNAL (SIG_INTERRUPT5) +{ + uint8_t c; c = TCNT3L; -#if COUNTER_REVERSE_RIGHT == 0 if (PINE & _BV (5)) -#else - if (!(PINE & _BV (5))) -#endif { - PORTD &= ~0x20; - counter_right_frw += c - counter_right_old; + counter_right_rev += c - counter_right_old; + PORTD |= 0x20; } else { - PORTD |= 0x20; - counter_right_rev += c - counter_right_old; + counter_right_frw += c - counter_right_old; + PORTD &= ~0x20; } counter_right_old = c; } @@ -122,16 +117,46 @@ counter_poll (void) static inline void counter_update (void) { + uint8_t c; + /* Disable ints. */ + EIMSK &= ~(_BV (4) | _BV (5)); + /* Read left counter. */ + c = TCNT2; + if (PINE & _BV (4)) + counter_left_frw += c - counter_left_old; + else + counter_left_rev += c - counter_left_old; + counter_left_old = c; + /* Read right counter. */ + c = TCNT3L; + if (PINE & _BV (5)) + counter_right_frw += c - counter_right_old; + else + counter_right_rev += c - counter_right_old; + counter_right_old = c; + /* Update counter values and diffs. */ +#if COUNTER_REVERSE_LEFT == 0 counter_left_diff = counter_left_frw; - counter_left_frw = 0; counter_left_diff -= counter_left_rev; +#else + counter_left_diff = counter_left_rev; + counter_left_diff -= counter_left_frw; +#endif + counter_left_frw = 0; counter_left_rev = 0; counter_left += counter_left_diff; +#if COUNTER_REVERSE_RIGHT == 0 counter_right_diff = counter_right_frw; - counter_right_frw = 0; counter_right_diff -= counter_right_rev; +#else + counter_right_diff = counter_right_rev; + counter_right_diff -= counter_right_frw; +#endif + counter_right_frw = 0; counter_right_rev = 0; counter_right += counter_right_diff; + /* Enable ints. */ + EIMSK |= _BV (4) | _BV (5); } /** Restart counting. */ diff --git a/n/asserv/src/eeprom.c b/n/asserv/src/eeprom.c index 8c98078..3eb0140 100644 --- a/n/asserv/src/eeprom.c +++ b/n/asserv/src/eeprom.c @@ -52,7 +52,7 @@ eeprom_write_params (void) uint8_t *p8 = 0; uint16_t *p16; eeprom_write_byte (p8++, 0xa5); - eeprom_write_byte (p8++, speed_acc_cpt); + eeprom_write_byte (p8++, speed_acc); eeprom_write_byte (p8++, speed_max); eeprom_write_byte (p8++, pwm_dir); p16 = (uint16_t *) p8; diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c index 64d1eac..d833204 100644 --- a/n/asserv/src/goto.c +++ b/n/asserv/src/goto.c @@ -41,7 +41,7 @@ int8_t goto_a; /** Travel speed for fixed speed movements, i8. */ int8_t goto_s; /** Destination epsillon. */ -int32_t goto_eps = 200L << 8; +int32_t goto_eps = 20L << 8; /** Debug values. */ int32_t goto_dx, goto_dy, goto_dl, goto_da; /** Movement finished. */ @@ -65,6 +65,8 @@ goto_linear_mode (void) /* Change speed. */ if (d > goto_d) { + speed_left = 0; + speed_right = 0; speed_left_aim = 0; speed_right_aim = 0; goto_finish = 1; @@ -90,11 +92,11 @@ goto_angular_mode (void) /* Small angles. */ if (0x10000L > angle_diff && angle_diff > -0x10000L) { - goto_finish = 1; speed_left = 0; speed_right = 0; speed_left_aim = 0; speed_right_aim = 0; + goto_finish = 1; } else { @@ -116,6 +118,8 @@ goto_position_mode (void) if (goto_dx < goto_eps && goto_dx > -goto_eps && goto_dy < goto_eps && goto_dy > -goto_eps) { + speed_left = 0; + speed_right = 0; speed_left_aim = 0; speed_right_aim = 0; goto_finish = 1; diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index 7650014..fe99acf 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -30,6 +30,7 @@ #include #include #include +#include #include "pwm.c" #include "timer.c" @@ -118,8 +119,9 @@ main_loop (void) while (1) { motor_timer_0 = timer_read (); + /* Counter update. */ while (!timer_pending ()) - counter_poll (); + ; counter_update (); motor_timer_4 = timer_read (); /* Position tracking. */ @@ -214,9 +216,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) reset (); break; /* Commands. */ + case c ('t', 0): + proto_send2b ('t', taz_state, taz_substate); + break; case c ('t', 1): motor_taz = args[0]; break; + case c ('t', 2): + taz_max_state = args[0]; + taz_max_substate = args[1]; + break; case c ('l', 2): /* Linear move. * - w: distance (negative to go backward). */ diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c index 093e064..4ab8722 100644 --- a/n/asserv/src/pwm.c +++ b/n/asserv/src/pwm.c @@ -73,36 +73,18 @@ pwm_init (void) static inline uint8_t pwm_preproc (uint16_t v) { - // This is a try to correct the optocoupler problem... - //v += 0x10; if (v > 255) return 255; else return v; } -extern int8_t speed_left, speed_right; - -/** Filter PWM values. */ -static inline void -pwm_filter (void) -{ - /* This is a try to correct vibration problem. */ - if ((speed_left > 0 && pwm_left < 0) - || (speed_left < 0 && pwm_left > 0)) - pwm_left = 0; - if ((speed_right > 0 && pwm_right < 0) - || (speed_right < 0 && pwm_right > 0)) - pwm_right = 0; -} - /** Update the hardware PWM values. */ static inline void pwm_update (void) { uint16_t left, right; uint8_t dir; - pwm_filter (); dir = PORTB & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR)); /* Set left PWM. */ if (pwm_left == 0) diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c index 3980ca5..f615ed0 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -22,14 +22,21 @@ * Email: * }}} */ -/** FSM state. */ +/** FSM state & substate. */ uint8_t taz_state; uint8_t taz_substate; +/** FSM tempo. */ uint8_t taz_tempo; +/** FSM max state & substate. */ +uint8_t taz_max_state = 0xff; +uint8_t taz_max_substate; + /** Positions. */ //#define taz_scale (1.0/0.05026548245743669181) //#define taz_scale (1.0/.05090591755319148936) -#define taz_scale (1.0/.05132374757503138194) +//#define taz_scale (1.0/.05132374757503138194) +#define taz_scale (1.0/.05124525981346725427) +static const uint32_t taz_rear_angle = 0x00ff90bf; static const uint16_t taz_rear_16 = taz_scale * 270 / 2; static const uint32_t taz_rear_32 = taz_scale * 270 / 2 * 256; static const uint16_t taz_front_16 = taz_scale * 270 / 2; @@ -37,11 +44,12 @@ static const uint32_t taz_front_32 = taz_scale * 270 / 2 * 256; static const uint16_t taz_side_16 = taz_scale * 340 / 2; static const uint32_t taz_side_32 = taz_scale * 340 / 2 * 256; -static const uint16_t taz_start_y_16 = taz_scale * (450 - 50) / 2; +static const uint16_t taz_start_y_16 = taz_scale * 450 / 2; static const uint16_t taz_before_bridge_16 = taz_scale * 1200; -static const uint16_t taz_brige_interval_16 = taz_scale * 140; -#define OR2 (1500 + 22 + 600 + 22) +static const uint16_t taz_brige_interval_16 = taz_scale * 150; +//#define OR2 (1500 + 22 + 600 + 22) +#define OR2 (1500 + 600) static const uint16_t taz_after_bridge_16[4] = { taz_scale * (OR2), @@ -96,6 +104,7 @@ taz_state_0 (void) { taz_substate = 2; /* FTW. */ + speed_restart (); motor_mode = 2; goto_mode = 10; goto_s = -10; @@ -245,7 +254,7 @@ taz_state_1 (void) taz_substate = 6; /* Recalage. */ speed_restart (); - postrack_y = taz_rear_32; + postrack_y = -taz_rear_32; postrack_a = 0x00c00000; /* On avance juste qu'à l'y de traversée de pont. */ motor_mode = 2; @@ -424,6 +433,9 @@ taz_update (void) uint8_t old_state, old_substate; old_state = taz_state; old_substate = taz_substate; + if (taz_state > taz_max_state + || (taz_state == taz_max_state && taz_substate > taz_max_substate)) + return; if (taz_tempo) { taz_tempo--; diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c index d92150c..ef3b413 100644 --- a/n/asserv/src/test_pwm.c +++ b/n/asserv/src/test_pwm.c @@ -30,8 +30,6 @@ /* +AutoDec */ /* -AutoDec */ -int8_t speed_left, speed_right; - #include "pwm.c" void -- cgit v1.2.3