From 46f98001f2ab53850dcf7e4a64cf2cea7e518d86 Mon Sep 17 00:00:00 2001 From: Mathieu Tournier Date: Fri, 22 Apr 2022 00:19:53 +0200 Subject: [PATCH] BugFix : _minUs and _maxUs should not be stored globally in the impl. Each call to setup with a different Pulse range creates a side effect on previously configured servos that results in bad positionning. Remove global variable minUs and maxUS and store these variables inside servo struct, as it can change between each device. --- src/RP2040_ISR_Servo.hpp | 10 +++++----- src/RP2040_ISR_Servo_Impl.h | 26 +++++++++++++------------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/RP2040_ISR_Servo.hpp b/src/RP2040_ISR_Servo.hpp index 7037158..018e98e 100644 --- a/src/RP2040_ISR_Servo.hpp +++ b/src/RP2040_ISR_Servo.hpp @@ -158,7 +158,7 @@ class RP2040_ISR_Servo } // Bind servo to the timer and pin, return servoIndex - int8_t setupServo(const uint8_t& pin, const uint16_t& min = MIN_PULSE_WIDTH, const uint16_t& max = MAX_PULSE_WIDTH, uint16_t value = 0); + int8_t setupServo(const uint8_t& pin, const uint16_t& minPulseUs = MIN_PULSE_WIDTH, const uint16_t& maxPulseUs = MAX_PULSE_WIDTH, uint16_t value = 0); // if value is < MIN_PULSE_WIDTH its treated as an angle, otherwise as pulse width in microseconds void write(const uint8_t& servoIndex, uint16_t& value); @@ -234,9 +234,6 @@ class RP2040_ISR_Servo // find the first available slot int8_t findFirstFreeSlot(); - - uint16_t _minUs; - uint16_t _maxUs; #if defined(ARDUINO_ARCH_MBED) @@ -245,7 +242,8 @@ class RP2040_ISR_Servo uint8_t pin; // pin servo connected to int position; // In degrees bool enabled; // true if enabled - + uint16_t minPulseUs; // The minimum pulse width the servo can handle + uint16_t maxPulseUs; // The maximum pulse width the servo can handle ServoImpl* servoImpl; } servo_t; @@ -259,6 +257,8 @@ class RP2040_ISR_Servo int pgmOffset; // Where in the SM the code lives int position; // In degrees bool enabled; // true if enabled + uint16_t minPulseUs; // The minimum pulse width the servo can handle + uint16_t maxPulseUs; // The maximum pulse width the servo can handle } servo_t; #endif diff --git a/src/RP2040_ISR_Servo_Impl.h b/src/RP2040_ISR_Servo_Impl.h index a973327..48abc0a 100644 --- a/src/RP2040_ISR_Servo_Impl.h +++ b/src/RP2040_ISR_Servo_Impl.h @@ -78,7 +78,7 @@ int8_t RP2040_ISR_Servo::findFirstFreeSlot() ///////////////////////////////////////////////////// -int8_t RP2040_ISR_Servo::setupServo(const uint8_t& pin, const uint16_t& minUs, const uint16_t& maxUs, uint16_t value) +int8_t RP2040_ISR_Servo::setupServo(const uint8_t& pin, const uint16_t& minPulseUs, const uint16_t& maxPulseUs, uint16_t value) { int8_t servoIndex; @@ -97,8 +97,8 @@ int8_t RP2040_ISR_Servo::setupServo(const uint8_t& pin, const uint16_t& minUs, c return -1; servo[servoIndex].pin = pin; - _maxUs = maxUs; - _minUs = minUs; + servo[servoIndex].maxPulseUs = maxPulseUs; + servo[servoIndex].minPulseUs = minPulseUs; servo[servoIndex].position = 0; servo[servoIndex].enabled = true; @@ -139,7 +139,7 @@ int8_t RP2040_ISR_Servo::setupServo(const uint8_t& pin, const uint16_t& minUs, c #endif ISR_SERVO_LOGDEBUG1("Index =", servoIndex); - ISR_SERVO_LOGDEBUG3("min =", _minUs, ", max =", _maxUs); + ISR_SERVO_LOGDEBUG3("min =", servo[servoIndex].minPulseUs, ", max =", servo[servoIndex].maxPulseUs); return servoIndex; } @@ -153,7 +153,7 @@ void RP2040_ISR_Servo::write(const uint8_t& servoIndex, uint16_t& value) { // assumed to be 0-180 degrees servo value = constrain(value, 0, 180); - value = map(value, 0, 180, _minUs, _maxUs); + value = map(value, 0, 180, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs); } writeMicroseconds(servoIndex, value); @@ -163,7 +163,7 @@ void RP2040_ISR_Servo::write(const uint8_t& servoIndex, uint16_t& value) void RP2040_ISR_Servo::writeMicroseconds(const uint8_t& servoIndex, uint16_t value) { - value = constrain(value, _minUs, _maxUs); + value = constrain(value, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs); servo[servoIndex].position = value; if (servo[servoIndex].enabled) @@ -203,7 +203,7 @@ bool RP2040_ISR_Servo::setPosition(const uint8_t& servoIndex, uint16_t position) { // assumed to be 0-180 degrees servo position = constrain(position, 0, 180); - position = map(position, 0, 180, _minUs, _maxUs); + position = map(position, 0, 180, servo[servoIndex].minPulseUs, servo[servoIndex].maxPulseUs); } servo[servoIndex].position = position; @@ -253,10 +253,10 @@ bool RP2040_ISR_Servo::setPulseWidth(const uint8_t& servoIndex, uint16_t& pulseW // Updates interval of existing specified servo if ( servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) ) { - if (pulseWidth < _minUs) - pulseWidth = _minUs; - else if (pulseWidth > _maxUs) - pulseWidth = _maxUs; + if (pulseWidth < servo[servoIndex].minPulseUs) + pulseWidth = servo[servoIndex].minPulseUs; + else if (pulseWidth > servo[servoIndex].maxPulseUs) + pulseWidth = servo[servoIndex].maxPulseUs; writeMicroseconds(servoIndex, pulseWidth); @@ -353,7 +353,7 @@ bool RP2040_ISR_Servo::enable(const uint8_t& servoIndex) return false; } - if ( servo[servoIndex].position >= _minUs ) + if ( servo[servoIndex].position >= servo[servoIndex].minPulseUs ) servo[servoIndex].enabled = true; return true; @@ -379,7 +379,7 @@ void RP2040_ISR_Servo::enableAll() // Enable all servos with a enabled and count != 0 (has PWM) and good pin for (int8_t servoIndex = 0; servoIndex < MAX_SERVOS; servoIndex++) { - if ( (servo[servoIndex].position >= _minUs) && !servo[servoIndex].enabled + if ( (servo[servoIndex].position >= servo[servoIndex].minPulseUs) && !servo[servoIndex].enabled && (servo[servoIndex].pin <= RP2040_MAX_PIN) ) { servo[servoIndex].enabled = true;