From 26b72c6065f73d1c50d4b9d5bcee82bed058148e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 10 Mar 2008 23:38:30 +0100 Subject: * digital/asserv/src/asserv: - added auxiliary motor state and PWM. --- digital/asserv/src/asserv/eeprom.avr.c | 4 +- digital/asserv/src/asserv/main.c | 22 ++-- digital/asserv/src/asserv/pwm.avr.c | 221 ++++++++++++++++++++++++++------- digital/asserv/src/asserv/pwm.h | 6 +- digital/asserv/src/asserv/simu.host.c | 10 +- digital/asserv/src/asserv/state.h | 3 + 6 files changed, 206 insertions(+), 60 deletions(-) diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index 05402154..6b54a02b 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -46,7 +46,7 @@ eeprom_read_params (void) speed_alpha_max = eeprom_read_byte (p8++); speed_theta_slow = eeprom_read_byte (p8++); speed_alpha_slow = eeprom_read_byte (p8++); - pwm_dir = eeprom_read_byte (p8++); + pwm_set_reverse (eeprom_read_byte (p8++)); p16 = (uint16_t *) p8; postrack_set_footing (eeprom_read_word (p16++)); speed_theta_acc = eeprom_read_word (p16++); @@ -73,7 +73,7 @@ eeprom_write_params (void) eeprom_write_byte (p8++, speed_alpha_max); eeprom_write_byte (p8++, speed_theta_slow); eeprom_write_byte (p8++, speed_alpha_slow); - eeprom_write_byte (p8++, pwm_dir); + eeprom_write_byte (p8++, pwm_reverse); p16 = (uint16_t *) p8; eeprom_write_word (p16++, postrack_footing); eeprom_write_word (p16++, speed_theta_acc); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 4a4d2aee..e8f43c4a 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -142,7 +142,8 @@ main_loop (void) && state_main.sequence_ack != state_main.sequence_finish && !--main_sequence_ack_cpt) { - proto_send1b ('A', state_main.sequence_finish); + proto_send2b ('A', state_main.sequence_finish, + state_aux0.sequence_finish); main_sequence_ack_cpt = main_sequence_ack; } if (main_stat_counter && !--main_stat_counter_cpt) @@ -178,7 +179,7 @@ main_loop (void) #endif /* HOST */ if (main_stat_pwm && !--main_stat_pwm_cpt) { - proto_send2w ('W', pwm_left, pwm_right); + proto_send3w ('W', pwm_left, pwm_right, pwm_aux0); main_stat_pwm_cpt = main_stat_pwm; } if (main_stat_timer && !--main_stat_timer_cpt) @@ -229,6 +230,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_right = v8_to_v16 (args[2], args[3]); UTILS_BOUND (pwm_right, -PWM_MAX, PWM_MAX); break; + case c ('w', 2): + /* Set auxiliary pwm. + * - w: pwm. */ + pos_reset (); // TODO + state_aux0.mode = MODE_PWM; + pwm_aux0 = v8_to_v16 (args[0], args[1]); + UTILS_BOUND (pwm_aux0, -PWM_MAX, PWM_MAX); + break; case c ('c', 4): /* Add to position consign. * - w: theta consign offset. @@ -405,11 +414,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('b', 3): pos_blocked = v8_to_v16 (args[1], args[2]); break; - case c ('w', 3): + case c ('w', 2): /* Set PWM direction. - * - b: inverse left direction. - * - b: inverse right direction. */ - pwm_reverse (args[1], args[2]); + * - b: bits: 0000[aux0][right][left]. */ + pwm_set_reverse (args[1]); break; case c ('E', 2): /* Write to eeprom. @@ -430,7 +438,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) proto_send2w ('d', pos_theta_kd, pos_alpha_kd); proto_send1w ('E', pos_e_sat); proto_send1w ('I', pos_int_sat); - proto_send1b ('w', pwm_dir); + proto_send1b ('w', pwm_reverse); break; default: proto_send0 ('?'); diff --git a/digital/asserv/src/asserv/pwm.avr.c b/digital/asserv/src/asserv/pwm.avr.c index 673606bd..f2caa220 100644 --- a/digital/asserv/src/asserv/pwm.avr.c +++ b/digital/asserv/src/asserv/pwm.avr.c @@ -28,96 +28,231 @@ #include "modules/utils/utils.h" #include "io.h" -/** Define the PWM output used for left motor. */ -#define PWM_LEFT_OCR OCR1B -/** Define the PWM output used for right motor. */ -#define PWM_RIGHT_OCR OCR1C -/** Define the direction output for left motor. */ -#define PWM_LEFT_DIR 4 -/** Define the direction output for right motor. */ -#define PWM_RIGHT_DIR 5 +/** Assign PWM outputs. */ +#define PWM1 pwm_left +#define PWM2 pwm_right +#define PWM3 pwm_aux0 +#undef PWM4 + +#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) + +#define PWM1_OCR OCR1B +#define PWM1_OCR_BIT 6 +#define PWM1_DIR 4 +#define PWM2_OCR OCR1C +#define PWM2_OCR_BIT 7 +#define PWM2_DIR 5 +#define PWM3_OCR OCR3B +#define PWM3_OCR_BIT 4 +#define PWM3_DIR 2 +#define PWM4_OCR OCR3C +#define PWM4_OCR_BIT 5 +#define PWM4_DIR 3 + +/* Simplify conditionnal compilation. */ +#define PWM1or2 (defined (PWM1) || defined (PWM2)) +#define PWM3or4 (defined (PWM3) || defined (PWM4)) +#ifdef PWM1 +# define PWM1c(x) x +#else +# define PWM1c(x) 0 +#endif +#ifdef PWM2 +# define PWM2c(x) x +#else +# define PWM2c(x) 0 +#endif +#ifdef PWM3 +# define PWM3c(x) x +#else +# define PWM3c(x) 0 +#endif +#ifdef PWM4 +# define PWM4c(x) x +#else +# define PWM4c(x) 0 +#endif /** PWM values, this is an error if absolute value is greater than the * maximum. */ -int16_t pwm_left, pwm_right; -/** PWM reverse direction, only set pwm dir bits or you will get weird results - * on port B. */ -uint8_t pwm_dir = _BV (PWM_LEFT_DIR); +int16_t pwm_left, pwm_right, pwm_aux0; +/** PWM reverse directions. */ +uint8_t pwm_reverse; +/** PWM reverse direction, port B. */ +static uint8_t pwm_dir_reverse_b; +/** PWM reverse direction, port E. */ +static uint8_t pwm_dir_reverse_e; /** Initialise PWM generator. */ void pwm_init (void) { - /* Fast PWM, TOP = 0x3ff, OC1B & OC1C with positive logic. + /* Fast PWM, TOP = 0x3ff, OCnB & OCnC with positive logic. f_IO without prescaler. Fpwm = f_IO / (prescaler * (1 + TOP)) = 14400 Hz. */ +#if PWM1or2 TCCR1A = regv (COM1A1, COM1A0, COM1B1, COM1B0, COM1C1, COM1C0, WGM11, WGM10, 0, 0, 1, 0, 1, 0, 1, 1); TCCR1B = regv (ICNC1, ICES1, 5, WGM13, WGM12, CS12, CS11, CS10, 0, 0, 0, 0, 1, 0, 0, 1); /* Enable pwm and direction outputs in DDRB. */ - DDRB |= _BV (7) | _BV (6) | _BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR); + DDRB |= PWM1c (_BV (PWM1_OCR_BIT) | _BV (PWM1_DIR)) + | PWM2c (_BV (PWM2_OCR_BIT) | _BV (PWM2_DIR)); +#endif +#if PWM3or4 + TCCR3A = + regv (COM3A1, COM3A0, COM3B1, COM3B0, COM3C1, COM3C0, WGM31, WGM30, + 0, 0, 1, 0, 1, 0, 1, 1); + TCCR3B = regv (ICNC3, ICES3, 5, WGM33, WGM32, CS32, CS31, CS30, + 0, 0, 0, 0, 1, 0, 0, 1); + /* Enable pwm and direction outputs in DDRE. */ + DDRE |= PWM3c (_BV (PWM3_OCR_BIT) | _BV (PWM3_DIR)) + | PWM4c (_BV (PWM4_OCR_BIT) | _BV (PWM4_DIR)); +#endif } /** Update the hardware PWM values. */ void pwm_update (void) { - uint16_t left, right; - uint8_t dir; /* Some assumption checks. */ - assert (pwm_left >= -PWM_MAX && pwm_left <= PWM_MAX); - assert (pwm_right >= -PWM_MAX && pwm_right <= PWM_MAX); - assert ((pwm_dir & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR))) == 0); + assert (!PWM1c (PWM1 < -PWM_MAX || PWM1 > PWM_MAX)); + assert (!PWM2c (PWM2 < -PWM_MAX || PWM2 > PWM_MAX)); + assert (!PWM3c (PWM3 < -PWM_MAX || PWM3 > PWM_MAX)); + assert (!PWM4c (PWM4 < -PWM_MAX || PWM4 > PWM_MAX)); +#if PWM1or2 + uint8_t dir_b; /* Sample port B. */ - dir = PORTB & ~(_BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR)); - /* Set left PWM. */ - if (pwm_left == 0) + dir_b = PORTB & ~(PWM1c (_BV (PWM1_DIR)) | PWM2c (_BV (PWM2_DIR))); +# ifdef PWM1 + uint16_t pwm1; + /* Set PWM1. */ + if (PWM1 == 0) + { + pwm1 = 0; + } + else if (PWM1 < 0) + { + pwm1 = -PWM1; + } + else + { + dir_b |= _BV (PWM1_DIR); + pwm1 = PWM1; + } +# endif /* PWM1 */ +# ifdef PWM2 + uint16_t pwm2; + /* Set PWM2. */ + if (PWM2 == 0) + { + pwm2 = 0; + } + else if (PWM2 < 0) + { + pwm2 = -PWM2; + } + else + { + dir_b |= _BV (PWM2_DIR); + pwm2 = PWM2; + } +# endif /* PWM2 */ +#endif /* PWM1or2 */ +#if PWM3or4 + uint8_t dir_e; + /* Sample port E. */ + dir_e = PORTE & ~(PWM3c (_BV (PWM3_DIR)) | PWM4c (_BV (PWM4_DIR))); +# ifdef PWM3 + uint16_t pwm3; + /* Set PWM3. */ + if (PWM3 == 0) { - left = 0; + pwm3 = 0; } - else if (pwm_left < 0) + else if (PWM3 < 0) { - left = -pwm_left; + pwm3 = -PWM3; } else { - dir |= _BV (PWM_LEFT_DIR); - left = pwm_left; + dir_e |= _BV (PWM3_DIR); + pwm3 = PWM3; } - /* Set right PWM. */ - if (pwm_right == 0) +# endif /* PWM3 */ +# ifdef PWM4 + uint16_t pwm4; + /* Set PWM4. */ + if (PWM4 == 0) { - right = 0; + pwm4 = 0; } - else if (pwm_right < 0) + else if (PWM4 < 0) { - right = -pwm_right; + pwm4 = -PWM4; } else { - dir |= _BV (PWM_RIGHT_DIR); - right = pwm_right; + dir_e |= _BV (PWM4_DIR); + pwm4 = PWM4; } +# endif /* PWM4 */ +#endif /* PWM3or4 */ /* Setup registers. */ /* Here, there could be a problem because OCRx are double buffered, not - * PORTB! */ + * PORTx! */ /* Another problem arise if the OCR sampling is done between left and * right OCR: the right PWM is one cycle late. */ /* A solution could be to use interrupts to update PWM or to synchronise * general timer with PWM. */ - dir ^= pwm_dir; - PORTB = dir; - PWM_LEFT_OCR = left; - PWM_RIGHT_OCR = right; +#if PWM1or2 + dir_b ^= pwm_dir_reverse_b; + PORTB = dir_b; +# ifdef PWM1 + PWM1_OCR = pwm1; +# endif +# ifdef PWM2 + PWM2_OCR = pwm2; +# endif +#endif /* PWM1or2 */ +#if PWM3or4 + dir_e ^= pwm_dir_reverse_e; + PORTE = dir_e; +# ifdef PWM3 + PWM3_OCR = pwm3; +# endif +# ifdef PWM4 + PWM4_OCR = pwm4; +# endif +#endif /* PWM3or4 */ } void -pwm_reverse (uint8_t left, uint8_t right) +pwm_set_reverse (uint8_t reverse) { - pwm_dir = 0; - if (left) pwm_dir |= _BV (PWM_LEFT_DIR); - if (right) pwm_dir |= _BV (PWM_RIGHT_DIR); + pwm_reverse = reverse; + pwm_dir_reverse_b = 0; + pwm_dir_reverse_e = 0; +#ifdef PWM1 + if (reverse & PWM_REVERSE_BIT (PWM1)) + pwm_dir_reverse_b |= _BV (PWM1_DIR); +#endif +#ifdef PWM2 + if (reverse & PWM_REVERSE_BIT (PWM2)) + pwm_dir_reverse_b |= _BV (PWM2_DIR); +#endif +#ifdef PWM3 + if (reverse & PWM_REVERSE_BIT (PWM3)) + pwm_dir_reverse_e |= _BV (PWM3_DIR); +#endif +#ifdef PWM4 + if (reverse & PWM_REVERSE_BIT (PWM4)) + pwm_dir_reverse_e |= _BV (PWM4_DIR); +#endif } diff --git a/digital/asserv/src/asserv/pwm.h b/digital/asserv/src/asserv/pwm.h index 2480f7eb..36b2bb5a 100644 --- a/digital/asserv/src/asserv/pwm.h +++ b/digital/asserv/src/asserv/pwm.h @@ -28,8 +28,8 @@ /** Define the absolute maximum PWM value. */ #define PWM_MAX 0x3ff -extern int16_t pwm_left, pwm_right; -extern uint8_t pwm_dir; +extern int16_t pwm_left, pwm_right, pwm_aux0; +extern uint8_t pwm_reverse; void pwm_init (void); @@ -38,6 +38,6 @@ void pwm_update (void); void -pwm_reverse (uint8_t left, uint8_t right); +pwm_set_reverse (uint8_t reverse); #endif /* pwm_h */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index d572d24b..ca5f18dc 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -48,10 +48,9 @@ int16_t counter_left_diff, counter_right_diff; /** PWM values, this is an error if absolute value is greater than the * maximum. */ -int16_t pwm_left, pwm_right; -/** PWM reverse direction, only set pwm dir bits or you will get weird results - * on port B. */ -uint8_t pwm_dir; +int16_t pwm_left, pwm_right, pwm_aux0; +/** PWM reverse directions. */ +uint8_t pwm_reverse; struct motor_t simu_left_model, simu_right_model; @@ -199,7 +198,8 @@ eeprom_clear_params (void) } void -pwm_reverse (uint8_t left, uint8_t right) +pwm_set_reverse (uint8_t reverse) { + pwm_reverse = reverse; } diff --git a/digital/asserv/src/asserv/state.h b/digital/asserv/src/asserv/state.h index 20000943..49ae1e60 100644 --- a/digital/asserv/src/asserv/state.h +++ b/digital/asserv/src/asserv/state.h @@ -74,6 +74,9 @@ struct state_t /** Main motors state. */ struct state_t state_main; +/** First auxiliary motor state. */ +struct state_t state_aux0; + /** Start a new command execution. */ static inline void state_start (struct state_t *motor, uint8_t sequence) -- cgit v1.2.3