From e83202bcbb8dc8b3a00731cfdcd935344bba5e32 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 15 Apr 2008 00:30:44 +0200 Subject: * digital/io/src: - fixed bad integer computation: 20 / 1000 = 0. - fixed no support for more than 5 servos. - added zero value support. - added saturation to protect from bad commands. --- digital/io/src/servo.avr.c | 21 ++++++++++++--------- digital/io/src/servo.h | 18 +++++++++++++----- 2 files changed, 25 insertions(+), 14 deletions(-) (limited to 'digital/io') diff --git a/digital/io/src/servo.avr.c b/digital/io/src/servo.avr.c index c2603c6e..39886bc5 100644 --- a/digital/io/src/servo.avr.c +++ b/digital/io/src/servo.avr.c @@ -56,7 +56,7 @@ * We want a time of 20ms (20/1000). * See @a servo_init to know the prescaler value of the timer/counter. */ -static const uint16_t servo_tic_cyle_ = ((20/1000) * AC_FREQ / 256); +static const uint16_t servo_tic_cyle_ = AC_FREQ / 256 * 20 / 1000; /** @} */ @@ -90,11 +90,7 @@ void servo_init (void) { /* Set-up all the pins of the servo to out direction */ - set_bit (SERVO_DDR, 0); - set_bit (SERVO_DDR, 1); - set_bit (SERVO_DDR, 2); - set_bit (SERVO_DDR, 3); - set_bit (SERVO_DDR, 4); + SERVO_DDR = 0xff; /* All pins are at low state by default */ /* Set-up the timer/counter 2: @@ -113,10 +109,13 @@ servo_init (void) void servo_set_high_time (uint8_t servo, uint8_t high_time) { + uint8_t filtered = high_time; + if (filtered != 0) + UTILS_BOUND (filtered, SERVO_HIGH_TIME_MIN, SERVO_HIGH_TIME_MAX); /* Sanity check */ if (servo < SERVO_NUMBER) /* Set new desired position (high value time) */ - servo_high_time_[servo] = high_time; + servo_high_time_[servo] = filtered; } /* Get the high time of the servo. */ @@ -147,14 +146,18 @@ SIGNAL (SIG_OVERFLOW2) case 2: case 3: case 4: + case 5: + case 6: + case 7: /* Servos motor high state mode */ /* Set to low state the previous servo motor pin if needed (not for * the first one) */ if (servo_updating_id_ != 0) SERVO_PORT &= ~_BV (servo_updating_id_ - 1); - /* Set to high state the current servo motor pin */ - set_bit (SERVO_PORT, servo_updating_id_); + /* Set to high state the current servo motor pin, unless is zero */ + if (servo_high_time_[servo_updating_id_]) + set_bit (SERVO_PORT, servo_updating_id_); /* Plan next timer overflow to the TOP minus the current configuration * of the servo motor */ TCNT2 = SERVO_TCNT_TOP - servo_high_time_[servo_updating_id_]; diff --git a/digital/io/src/servo.h b/digital/io/src/servo.h index c6c08ce3..2b01e731 100644 --- a/digital/io/src/servo.h +++ b/digital/io/src/servo.h @@ -36,7 +36,7 @@ * * Servo motors can be controlled by the time the input signal spend at its * high state. For example, if the signal sent to the servo motor only spend - * 0.5ms at the high state, it will have a 0° angle position. If the signal + * 1ms at the high state, it will have a 0° angle position. If the signal * stays for 1.5ms at high state, it will have a 90° angle position. * To manage all servo motors in a "one time shot", we need to use the * timer/counter 2 of the ATmega128 and its overflow. @@ -55,13 +55,20 @@ */ #define SERVO_NUMBER 6 +/** + * Minimum high time for servos. + */ +#define SERVO_HIGH_TIME_MIN 0x24 + +/** + * Maximum high time for servos. + */ +#define SERVO_HIGH_TIME_MAX 0x88 + /** * Initialize servo module. * This functions put the pins of the servos motor in the right direction, * initialize the timer/counter 2 and some internals stuff. - * A important remark: you need to set the default values of each servos motor - * before calling this functions if you do not want to have some strange - * behavior. */ void servo_init (void); @@ -70,7 +77,8 @@ servo_init (void); * Set the high time of the input signal of a servo (and its position). * @param servo the servo to change the position. * @param high_time the high time we want the input signal to spend at the - * high state to set the servo motor to a position. + * high state to set the servo motor to a position. A zero will let the servo + * floating. */ void servo_set_high_time (uint8_t servo, uint8_t high_time); -- cgit v1.2.3