From 9dbe6192cf66df4c0668ae3c4bc927f39b8be4e3 Mon Sep 17 00:00:00 2001 From: schodet Date: Mon, 18 Apr 2005 22:51:54 +0000 Subject: pwm_dir pwm_filter --- n/asserv/src/main.c | 12 +++++++-- n/asserv/src/pwm.c | 66 ++++++++++++++++++++++++++----------------------- n/asserv/src/taz.c | 3 ++- n/asserv/src/test_pwm.c | 2 ++ 4 files changed, 49 insertions(+), 34 deletions(-) (limited to 'n') diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c index eec0699..1e77b7e 100644 --- a/n/asserv/src/main.c +++ b/n/asserv/src/main.c @@ -212,8 +212,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) reset (); break; /* Commands. */ - case c ('t', 0): - motor_taz = 1; + case c ('t', 1): + motor_taz = args[0]; break; case c ('l', 2): /* Linear move. @@ -368,6 +368,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Set maximum speed for automatic movements. */ speed_max = args[1]; break; + case c ('w', 3): + /* Set PWM direction. + * - b: inverse left direction. + * - b: inverse right direction. */ + pwm_dir = 0; + if (args[1]) pwm_dir |= _BV (PWM_LEFT_DIR); + if (args[2]) pwm_dir |= _BV (PWM_RIGHT_DIR); + break; default: proto_send0 ('?'); return; diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c index 9452077..093e064 100644 --- a/n/asserv/src/pwm.c +++ b/n/asserv/src/pwm.c @@ -31,13 +31,11 @@ #define PWM_LEFT_DIR 3 /** Define the direction output for right motor. */ #define PWM_RIGHT_DIR 2 -/** 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 1 /** PWM values. */ int16_t pwm_left, pwm_right; +/** PWM reverse direction. */ +uint8_t pwm_dir = _BV (PWM_LEFT_DIR); /* +AutoDec */ @@ -68,7 +66,7 @@ pwm_init (void) TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, 0, 0, 0, 0, 0, 0, 0, 1); /* Enable pwm and direction outputs in DDRB. */ - DDRB |= _BV (7) | _BV (6) | _BV (3) | _BV (2); + DDRB |= _BV (7) | _BV (6) | _BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR); } /** Preprocess a PWM value before sending it to hardware. */ @@ -83,55 +81,61 @@ pwm_preproc (uint16_t v) 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) { - PWM_LEFT_OCR = 0; + left = 0; } else if (pwm_left < 0) { -#if PWM_LEFT_DIR_FRW == 1 - PORTB &= ~_BV (PWM_LEFT_DIR); -#else - PORTB |= _BV (PWM_LEFT_DIR); -#endif - PWM_LEFT_OCR = pwm_preproc (-pwm_left); + left = pwm_preproc (-pwm_left); } else { -#if PWM_LEFT_DIR_FRW == 1 - PORTB |= _BV (PWM_LEFT_DIR); -#else - PORTB &= ~_BV (PWM_LEFT_DIR); -#endif - PWM_LEFT_OCR = pwm_preproc (pwm_left); + dir |= _BV (PWM_LEFT_DIR); + left = pwm_preproc (pwm_left); } /* Set right PWM. */ if (pwm_right == 0) { - PWM_RIGHT_OCR = 0; + right = 0; } else if (pwm_right < 0) { -#if PWM_RIGHT_DIR_FRW == 1 - PORTB &= ~_BV (PWM_RIGHT_DIR); -#else - PORTB |= _BV (PWM_RIGHT_DIR); -#endif - PWM_RIGHT_OCR = pwm_preproc (-pwm_right); + right = pwm_preproc (-pwm_right); } else { -#if PWM_RIGHT_DIR_FRW == 1 - PORTB |= _BV (PWM_RIGHT_DIR); -#else - PORTB &= ~_BV (PWM_RIGHT_DIR); -#endif - PWM_RIGHT_OCR = pwm_preproc (pwm_right); + dir |= _BV (PWM_RIGHT_DIR); + right = pwm_preproc (pwm_right); } + /* Setup registers. */ + dir ^= pwm_dir; + PORTB = dir; + PWM_LEFT_OCR = left; + PWM_RIGHT_OCR = right; } diff --git a/n/asserv/src/taz.c b/n/asserv/src/taz.c index 7c0d775..3980ca5 100644 --- a/n/asserv/src/taz.c +++ b/n/asserv/src/taz.c @@ -28,7 +28,8 @@ uint8_t taz_substate; uint8_t taz_tempo; /** Positions. */ //#define taz_scale (1.0/0.05026548245743669181) -#define taz_scale (1.0/.05090591755319148936) +//#define taz_scale (1.0/.05090591755319148936) +#define taz_scale (1.0/.05132374757503138194) 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; diff --git a/n/asserv/src/test_pwm.c b/n/asserv/src/test_pwm.c index ef3b413..d92150c 100644 --- a/n/asserv/src/test_pwm.c +++ b/n/asserv/src/test_pwm.c @@ -30,6 +30,8 @@ /* +AutoDec */ /* -AutoDec */ +int8_t speed_left, speed_right; + #include "pwm.c" void -- cgit v1.2.3