From fb29b57fece6c53492aca06931ca866dd8e0b1d2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 28 Apr 2008 15:31:01 +0200 Subject: * digital/asserv/src/asserv: - added generic system for different deadzones per PWM. - cleaned up pwm setting. --- digital/asserv/src/asserv/main.c | 9 +++------ digital/asserv/src/asserv/pos.c | 10 +++------- digital/asserv/src/asserv/pwm.avr.c | 9 ++++++--- digital/asserv/src/asserv/pwm.h | 23 +++++++++++++++++++++++ digital/asserv/src/asserv/pwm_mp.avr.c | 11 ++++++++--- digital/asserv/src/asserv/simu.host.c | 9 ++++++--- 6 files changed, 49 insertions(+), 22 deletions(-) (limited to 'digital/asserv') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 19fe2056..9516fd43 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -239,18 +239,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pos_reset (&pos_theta); pos_reset (&pos_alpha); state_main.mode = MODE_PWM; - pwm_left = v8_to_v16 (args[0], args[1]); - UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); - pwm_right = v8_to_v16 (args[2], args[3]); - UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX); + PWM_SET (pwm_left, v8_to_v16 (args[0], args[1])); + PWM_SET (pwm_right, v8_to_v16 (args[2], args[3])); break; case c ('w', 2): /* Set auxiliary pwm. * - w: pwm. */ pos_reset (&pos_aux0); state_aux0.mode = MODE_PWM; - pwm_aux0 = v8_to_v16 (args[0], args[1]); - UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX); + PWM_SET (pwm_aux0, v8_to_v16 (args[0], args[1])); break; case c ('c', 4): /* Add to position consign. diff --git a/digital/asserv/src/asserv/pos.c b/digital/asserv/src/asserv/pos.c index d47694c2..9109e165 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -123,10 +123,8 @@ pos_update (void) pid_theta = pos_compute_pid (diff_theta, &pos_theta); pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha); /* Update PWM. */ - pwm_left = pid_theta - pid_alpha; - UTILS_BOUND (pwm_left, -PWM_MAX, PWM_MAX); - pwm_right = pid_theta + pid_alpha; - UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX); + PWM_SET (pwm_left, pid_theta - pid_alpha); + PWM_SET (pwm_right, pid_theta + pid_alpha); } } if (state_aux0.mode >= MODE_POS) @@ -148,9 +146,7 @@ pos_update (void) { pid = pos_compute_pid (diff, &pos_aux0); /* Update PWM. */ - pwm_aux0 = pid; - /* WARNING: crude way to limit PWM for this 12V motor. */ - UTILS_BOUND (pwm_aux0, -(PWM_MAX / 2), (PWM_MAX / 2)); + PWM_SET (pwm_aux0, pid); } } } diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c index 8d12cc8a..932e5a7f 100644 --- a/digital/asserv/src/asserv/pwm.avr.c +++ b/digital/asserv/src/asserv/pwm.avr.c @@ -46,9 +46,12 @@ void pwm_update (void) { /* Some assumption checks. */ - assert (pwm_left > -PWM_MAX && pwm_left < PWM_MAX); - assert (pwm_right > -PWM_MAX && pwm_right < PWM_MAX); - assert (pwm_aux0 > -PWM_MAX && pwm_aux0 < PWM_MAX); + assert (pwm_left >= -PWM_MAX_FOR (pwm_left) + && pwm_left <= PWM_MAX_FOR (pwm_left)); + assert (pwm_right >= -PWM_MAX_FOR (pwm_right) + && pwm_right <= PWM_MAX_FOR (pwm_right)); + assert (pwm_aux0 >= -PWM_MAX_FOR (pwm_aux0) + && pwm_aux0 <= PWM_MAX_FOR (pwm_aux0)); pwm_mp_update (); pwm_ocr_update (); } diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h index d980a6a6..243ea213 100644 --- a/digital/asserv/src/asserv/pwm.h +++ b/digital/asserv/src/asserv/pwm.h @@ -33,12 +33,35 @@ extern int16_t pwm_left, pwm_right, pwm_aux0; extern uint8_t pwm_reverse; +/** Define maximum PWM value for each output. */ +#define PWM_MAX_FOR(x) PWM_MAX_FOR_ (x) +#define PWM_MAX_FOR_(x) PWM_MAX_FOR_ ## x +#define PWM_MAX_FOR_pwm_left PWM_MAX +#define PWM_MAX_FOR_pwm_right PWM_MAX +#define PWM_MAX_FOR_pwm_aux0 (PWM_MAX / 2) + +/** Define minimum PWM value for each output, if the value is less than the + * minimum, use 0. */ +#define PWM_MIN_FOR(x) PWM_MIN_FOR_ (x) +#define PWM_MIN_FOR_(x) PWM_MIN_FOR_ ## x +#define PWM_MIN_FOR_pwm_left 0x8 +#define PWM_MIN_FOR_pwm_right 0x8 +#define PWM_MIN_FOR_pwm_aux0 0x20 + +/** Define which bit controls the PWM inversion. */ #define PWM_REVERSE_BIT(x) PWM_REVERSE_BIT_ (x) #define PWM_REVERSE_BIT_(x) PWM_REVERSE_BIT_ ## x #define PWM_REVERSE_BIT_pwm_left _BV (0) #define PWM_REVERSE_BIT_pwm_right _BV (1) #define PWM_REVERSE_BIT_pwm_aux0 _BV (2) +/** Set pwm value and saturate. */ +#define PWM_SET(pwm, value) \ + do { \ + (pwm) = (value); \ + UTILS_BOUND ((pwm), -PWM_MAX_FOR (pwm), PWM_MAX_FOR (pwm)); \ + } while (0) + void pwm_init (void); diff --git a/digital/asserv/src/asserv/pwm_mp.avr.c b/digital/asserv/src/asserv/pwm_mp.avr.c index 4ad7bd4a..0602c40a 100644 --- a/digital/asserv/src/asserv/pwm_mp.avr.c +++ b/digital/asserv/src/asserv/pwm_mp.avr.c @@ -65,7 +65,8 @@ pwm_mp_init (void) /** Send command using SPI. */ static void -pwm_mp_send (int16_t pwm1, int16_t pwm2, uint8_t invert1, uint8_t invert2) +pwm_mp_send (int16_t pwm1, int16_t pwm2, uint16_t min1, uint16_t min2, + uint8_t invert1, uint8_t invert2) { uint8_t v; uint8_t cks; @@ -76,9 +77,9 @@ pwm_mp_send (int16_t pwm1, int16_t pwm2, uint8_t invert1, uint8_t invert2) pwm1c = -pwm1c; if (invert2) pwm2c = -pwm2c; - if (UTILS_ABS (pwm1c) < 0x10) + if (UTILS_ABS (pwm1c) < min1 * 2) pwm1c = 0; - if (UTILS_ABS (pwm2c) < 0x10) + if (UTILS_ABS (pwm2c) < min2 * 2) pwm2c = 0; /* Send, computing checksum on the way. */ cks = 0x42; @@ -110,6 +111,8 @@ pwm_mp_update (void) /* Chip enable. */ PORTB &= ~_BV (0); pwm_mp_send (PWM1c (PWM1), PWM2c (PWM2), + PWM1c (PWM_MIN_FOR (PWM1)), + PWM2c (PWM_MIN_FOR (PWM2)), PWM1c (pwm_reverse & PWM_REVERSE_BIT (PWM1)), PWM2c (pwm_reverse & PWM_REVERSE_BIT (PWM2))); /* Chip disable. */ @@ -119,6 +122,8 @@ pwm_mp_update (void) /* Chip enable. */ PORTE &= ~_BV (4); pwm_mp_send (PWM3c (PWM3), PWM4c (PWM4), + PWM3c (PWM_MIN_FOR (PWM3)), + PWM4c (PWM_MIN_FOR (PWM4)), PWM3c (pwm_reverse & PWM_REVERSE_BIT (PWM3)), PWM4c (pwm_reverse & PWM_REVERSE_BIT (PWM4))); /* Chip disable. */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 64fc38db..795ee6a7 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -167,9 +167,12 @@ simu_step (void) { double old_left_th, old_right_th, old_aux0_th; /* Convert pwm value into voltage. */ - assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX); - assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX); - assert (pwm_aux0 >= -PWM_MAX && pwm_aux0 <= PWM_MAX); + assert (pwm_left >= -PWM_MAX_FOR (pwm_left) + && pwm_left <= PWM_MAX_FOR (pwm_left)); + assert (pwm_right >= -PWM_MAX_FOR (pwm_right) + && pwm_right <= PWM_MAX_FOR (pwm_right)); + assert (pwm_aux0 >= -PWM_MAX_FOR (pwm_aux0) + && pwm_aux0 <= PWM_MAX_FOR (pwm_aux0)); simu_left_model.u = simu_left_model.m.u_max * ((double) pwm_left / (PWM_MAX + 1)); simu_right_model.u = simu_right_model.m.u_max -- cgit v1.2.3