From 2e0727532fdc1e9eb5b72188182c7a8a7c4ab3fc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 Mar 2009 00:20:05 +0200 Subject: * digital/asserv/src/asserv: - use a structure for PWM state. --- digital/asserv/src/asserv/main.c | 12 +++++------ digital/asserv/src/asserv/pos.c | 12 +++++------ digital/asserv/src/asserv/pwm.avr.c | 15 +++++-------- digital/asserv/src/asserv/pwm.h | 37 ++++++++++++++++++++++++++------- digital/asserv/src/asserv/pwm_mp.avr.c | 17 +++++---------- digital/asserv/src/asserv/pwm_ocr.avr.c | 32 ++++++++++++++-------------- digital/asserv/src/asserv/simu.host.c | 21 +++++++------------ digital/asserv/src/asserv/twi_proto.c | 4 ++-- 8 files changed, 78 insertions(+), 72 deletions(-) (limited to 'digital') diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index db3064da..1ba1f956 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -193,7 +193,7 @@ main_loop (void) #endif /* HOST */ if (main_stat_pwm && !--main_stat_pwm_cpt) { - proto_send3w ('W', pwm_left, pwm_right, pwm_aux0); + proto_send3w ('W', pwm_left.cur, pwm_right.cur, pwm_aux0.cur); main_stat_pwm_cpt = main_stat_pwm; } if (main_stat_timer && !--main_stat_timer_cpt) @@ -231,8 +231,8 @@ 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 = 0; - pwm_right = 0; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); break; case c ('w', 4): /* Set pwm. @@ -241,15 +241,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_SET (pwm_left, v8_to_v16 (args[0], args[1])); - PWM_SET (pwm_right, v8_to_v16 (args[2], args[3])); + 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_SET (pwm_aux0, v8_to_v16 (args[0], args[1])); + 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 af3d0315..e84a0905 100644 --- a/digital/asserv/src/asserv/pos.c +++ b/digital/asserv/src/asserv/pos.c @@ -133,16 +133,16 @@ pos_update (void) pos_reset (&pos_theta); pos_reset (&pos_alpha); state_blocked (&state_main); - pwm_left = 0; - pwm_right = 0; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); } else { pid_theta = pos_compute_pid (diff_theta, &pos_theta); pid_alpha = pos_compute_pid (diff_alpha, &pos_alpha); /* Update PWM. */ - PWM_SET (pwm_left, pid_theta - pid_alpha); - PWM_SET (pwm_right, pid_theta + pid_alpha); + pwm_set (&pwm_left, pid_theta - pid_alpha); + pwm_set (&pwm_right, pid_theta + pid_alpha); } } if (state_aux0.mode >= MODE_POS) @@ -158,13 +158,13 @@ pos_update (void) /* Blocked. */ pos_reset (&pos_aux0); state_blocked (&state_aux0); - pwm_aux0 = 0; + pwm_set (&pwm_aux0, 0); } else { pid = pos_compute_pid (diff, &pos_aux0); /* Update PWM. */ - PWM_SET (pwm_aux0, pid); + pwm_set (&pwm_aux0, pid); } } } diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c index 932e5a7f..6a5b0ff4 100644 --- a/digital/asserv/src/asserv/pwm.avr.c +++ b/digital/asserv/src/asserv/pwm.avr.c @@ -27,9 +27,10 @@ #include "pwm_mp.avr.h" #include "pwm_ocr.avr.h" -/** PWM values, this is an error if absolute value is greater than the - * maximum. */ -int16_t pwm_left, pwm_right, pwm_aux0; +/** PWM control states. */ +struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left); +struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right); +struct pwm_t pwm_aux0 = PWM_INIT_FOR (pwm_aux0); /** PWM reverse directions. */ uint8_t pwm_reverse; @@ -45,17 +46,11 @@ pwm_init (void) void pwm_update (void) { - /* Some assumption checks. */ - 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 (); } +/** Set which PWM is reversed. */ void pwm_set_reverse (uint8_t reverse) { diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h index fd141df7..7877d4eb 100644 --- a/digital/asserv/src/asserv/pwm.h +++ b/digital/asserv/src/asserv/pwm.h @@ -30,7 +30,19 @@ * (rounding after shifting bug). */ #define PWM_MAX 0x3f0 -extern int16_t pwm_left, pwm_right, pwm_aux0; +/** PWM control state. */ +struct pwm_t +{ + /** Current PWM value. */ + int16_t cur; + /** Maximum value. */ + int16_t max; + /** Minimum value (dead zone). */ + int16_t min; +}; + +extern struct pwm_t pwm_left, pwm_right, pwm_aux0; + extern uint8_t pwm_reverse; /** Define maximum PWM value for each output. */ @@ -55,12 +67,23 @@ extern uint8_t pwm_reverse; #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) +/** State init macro. */ +#define PWM_INIT_FOR(x) \ + { 0, PWM_MAX_FOR (x), PWM_MIN_FOR (x) } + +/** Set PWM value. */ +static inline void +pwm_set (struct pwm_t *pwm, int16_t value) +{ + if (value > pwm->max) + pwm->cur = pwm->max; + else if (value < -pwm->max) + pwm->cur = -pwm->max; + else if (value > -pwm->min && value < pwm->min) + pwm->cur = 0; + else + pwm->cur = value; +} 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 0602c40a..47b50763 100644 --- a/digital/asserv/src/asserv/pwm_mp.avr.c +++ b/digital/asserv/src/asserv/pwm_mp.avr.c @@ -65,7 +65,7 @@ pwm_mp_init (void) /** Send command using SPI. */ static void -pwm_mp_send (int16_t pwm1, int16_t pwm2, uint16_t min1, uint16_t min2, +pwm_mp_send (int16_t pwm1, int16_t pwm2, uint8_t invert1, uint8_t invert2) { uint8_t v; @@ -77,10 +77,6 @@ pwm_mp_send (int16_t pwm1, int16_t pwm2, uint16_t min1, uint16_t min2, pwm1c = -pwm1c; if (invert2) pwm2c = -pwm2c; - if (UTILS_ABS (pwm1c) < min1 * 2) - pwm1c = 0; - if (UTILS_ABS (pwm2c) < min2 * 2) - pwm2c = 0; /* Send, computing checksum on the way. */ cks = 0x42; v = ((pwm1c >> 4) & 0xf0) | ((pwm2c >> 8) & 0x0f); @@ -102,7 +98,8 @@ void pwm_mp_update (void) { #if PWM1or2 || PWM3or4 - if (PWM1c (PWM1) || PWM2c (PWM2) || PWM3c (PWM3) || PWM4c (PWM4)) + if (PWM1c (PWM1.cur) || PWM2c (PWM2.cur) + || PWM3c (PWM3.cur) || PWM4c (PWM4.cur)) pwm_mp_go = 1; if (!pwm_mp_go) return; @@ -110,9 +107,7 @@ pwm_mp_update (void) #if PWM1or2 /* Chip enable. */ PORTB &= ~_BV (0); - pwm_mp_send (PWM1c (PWM1), PWM2c (PWM2), - PWM1c (PWM_MIN_FOR (PWM1)), - PWM2c (PWM_MIN_FOR (PWM2)), + pwm_mp_send (PWM1c (PWM1.cur), PWM2c (PWM2.cur), PWM1c (pwm_reverse & PWM_REVERSE_BIT (PWM1)), PWM2c (pwm_reverse & PWM_REVERSE_BIT (PWM2))); /* Chip disable. */ @@ -121,9 +116,7 @@ pwm_mp_update (void) #if PWM3or4 /* Chip enable. */ PORTE &= ~_BV (4); - pwm_mp_send (PWM3c (PWM3), PWM4c (PWM4), - PWM3c (PWM_MIN_FOR (PWM3)), - PWM4c (PWM_MIN_FOR (PWM4)), + pwm_mp_send (PWM3c (PWM3.cur), PWM4c (PWM4.cur), PWM3c (pwm_reverse & PWM_REVERSE_BIT (PWM3)), PWM4c (pwm_reverse & PWM_REVERSE_BIT (PWM4))); /* Chip disable. */ diff --git a/digital/asserv/src/asserv/pwm_ocr.avr.c b/digital/asserv/src/asserv/pwm_ocr.avr.c index 1c0dfce0..75857f10 100644 --- a/digital/asserv/src/asserv/pwm_ocr.avr.c +++ b/digital/asserv/src/asserv/pwm_ocr.avr.c @@ -95,35 +95,35 @@ pwm_ocr_update (void) # ifdef PWM1 uint16_t pwm1; /* Set PWM1. */ - if (PWM1 == 0) + if (PWM1.cur == 0) { pwm1 = 0; } - else if (PWM1 < 0) + else if (PWM1.cur < 0) { - pwm1 = -PWM1; + pwm1 = -PWM1.cur; } else { dir_b |= _BV (PWM1_DIR); - pwm1 = PWM1; + pwm1 = PWM1.cur; } # endif /* PWM1 */ # ifdef PWM2 uint16_t pwm2; /* Set PWM2. */ - if (PWM2 == 0) + if (PWM2.cur == 0) { pwm2 = 0; } - else if (PWM2 < 0) + else if (PWM2.cur < 0) { - pwm2 = -PWM2; + pwm2 = -PWM2.cur; } else { dir_b |= _BV (PWM2_DIR); - pwm2 = PWM2; + pwm2 = PWM2.cur; } # endif /* PWM2 */ #endif /* PWM1or2 */ @@ -134,35 +134,35 @@ pwm_ocr_update (void) # ifdef PWM3 uint16_t pwm3; /* Set PWM3. */ - if (PWM3 == 0) + if (PWM3.cur == 0) { pwm3 = 0; } - else if (PWM3 < 0) + else if (PWM3.cur < 0) { - pwm3 = -PWM3; + pwm3 = -PWM3.cur; } else { dir_e |= _BV (PWM3_DIR); - pwm3 = PWM3; + pwm3 = PWM3.cur; } # endif /* PWM3 */ # ifdef PWM4 uint16_t pwm4; /* Set PWM4. */ - if (PWM4 == 0) + if (PWM4.cur == 0) { pwm4 = 0; } - else if (PWM4 < 0) + else if (PWM4.cur < 0) { - pwm4 = -PWM4; + pwm4 = -PWM4.cur; } else { dir_e |= _BV (PWM4_DIR); - pwm4 = PWM4; + pwm4 = PWM4.cur; } # endif /* PWM4 */ #endif /* PWM3or4 */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index eec42f61..d496b61e 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -55,9 +55,10 @@ static int32_t counter_right_raw; /** Correction factor (f8.24). */ uint32_t counter_right_correction = 1L << 24; -/** PWM values, this is an error if absolute value is greater than the - * maximum. */ -int16_t pwm_left, pwm_right, pwm_aux0; +/** PWM control states. */ +struct pwm_t pwm_left = PWM_INIT_FOR (pwm_left); +struct pwm_t pwm_right = PWM_INIT_FOR (pwm_right); +struct pwm_t pwm_aux0 = PWM_INIT_FOR (pwm_aux0); /** PWM reverse directions. */ uint8_t pwm_reverse; @@ -168,18 +169,12 @@ simu_step (void) { double old_left_th, old_right_th, old_aux0_th; /* Convert pwm value into voltage. */ - 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)); + * ((double) pwm_left.cur / (PWM_MAX + 1)); simu_right_model.u = simu_right_model.m.u_max - * ((double) pwm_right / (PWM_MAX + 1)); + * ((double) pwm_right.cur / (PWM_MAX + 1)); simu_aux0_model.u = simu_aux0_model.m.u_max - * ((double) pwm_aux0 / (PWM_MAX + 1)); + * ((double) pwm_aux0.cur / (PWM_MAX + 1)); /* Make one step. */ old_left_th = simu_left_model.th; old_right_th = simu_right_model.th; @@ -259,7 +254,7 @@ simu_send (void) mex_node_send (m); /* Send PWM. */ m = mex_msg_new (0xa1); - mex_msg_push (m, "hhh", pwm_left, pwm_right, pwm_aux0); + mex_msg_push (m, "hhh", pwm_left.cur, pwm_right.cur, pwm_aux0.cur); mex_node_send (m); /* Send Arm position. */ m = mex_msg_new (0xa8); diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 745a1a70..c235a626 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -109,8 +109,8 @@ twi_proto_callback (u8 *buf, u8 size) pos_reset (&pos_theta); pos_reset (&pos_alpha); state_main.mode = MODE_PWM; - pwm_left = 0; - pwm_right = 0; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); break; case c ('s', 0): /* Stop (set zero speed). */ -- cgit v1.2.3