diff --git a/src/Servo.h b/src/Servo.h
index 53ecb8e..9485c9b 100644
--- a/src/Servo.h
+++ b/src/Servo.h
@@ -105,6 +105,7 @@ class Servo
{
public:
Servo();
+ ~Servo();
uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or INVALID_SERVO if failure
uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
void detach();
@@ -114,7 +115,7 @@ class Servo
int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release)
bool attached(); // return true if this servo is attached, otherwise false
private:
- uint8_t servoIndex; // index into the channel data for this servo
+ uint8_t servoIndex = INVALID_SERVO;// index into the channel data for this servo
int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH
int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH
};
diff --git a/src/avr/Servo.cpp b/src/avr/Servo.cpp
index 11fecc5..d472c51 100644
--- a/src/avr/Servo.cpp
+++ b/src/avr/Servo.cpp
@@ -35,9 +35,6 @@
static servo_t servos[MAX_SERVOS]; // static array of servo structures
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
-uint8_t ServoCount = 0; // the total number of attached servos
-
-
// convenience macros
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER) // returns the index of the servo on this timer
@@ -54,12 +51,12 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t
if( Channel[timer] < 0 )
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
else{
- if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
+ if( SERVO_INDEX(timer,Channel[timer]) < MAX_SERVOS && SERVO(timer,Channel[timer]).Pin.isActive == true )
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
}
Channel[timer]++; // increment to the next channel
- if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
+ if( SERVO_INDEX(timer,Channel[timer]) < MAX_SERVOS && Channel[timer] < SERVOS_PER_TIMER) {
*OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
@@ -222,12 +219,25 @@ static boolean isTimerActive(timer16_Sequence_t timer)
Servo::Servo()
{
- if( ServoCount < MAX_SERVOS) {
- this->servoIndex = ServoCount++; // assign a servo index to this instance
- servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
+ // Iterate over array to find an uninitialized servo
+ this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
+ for (int8_t i = 0; i < MAX_SERVOS; i++) {
+ if (servos[i].servoIndex == INVALID_SERVO) {
+ this->servoIndex = i;
+ servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
+ break;
+ }
+ }
+ // servoIndex will be INVALID_SERVO if no free timers found
+}
+
+Servo::~Servo()
+{
+ if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
+ // Disable this servo if it was attached
+ this->detach();
+ this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
}
- else
- this->servoIndex = INVALID_SERVO ; // too many servos
}
uint8_t Servo::attach(int pin)
diff --git a/src/mbed/Servo.cpp b/src/mbed/Servo.cpp
index efb67f9..06d3339 100644
--- a/src/mbed/Servo.cpp
+++ b/src/mbed/Servo.cpp
@@ -43,8 +43,7 @@ class ServoImpl {
int32_t duration = -1;
};
-static ServoImpl* servos[MAX_SERVOS]; // static array of servo structures
-uint8_t ServoCount = 0; // the total number of attached servos
+static ServoImpl* servos[MAX_SERVOS] = {NULL}; // static array of servo structures
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min) // minimum value in us for this servo
#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max) // maximum value in us for this servo
@@ -53,10 +52,23 @@ uint8_t ServoCount = 0; // the total number o
Servo::Servo()
{
- if (ServoCount < MAX_SERVOS) {
- this->servoIndex = ServoCount++;
- } else {
- this->servoIndex = INVALID_SERVO; // too many servos
+ // Iterate over array to find an uninitialized servo
+ this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
+ for (int8_t i = 0; i < MAX_SERVOS; i++) {
+ if (servos[i] == NULL) {
+ this->servoIndex = i;
+ break;
+ }
+ }
+ // servoIndex will be INVALID_SERVO if no free timers found
+}
+
+Servo::~Servo()
+{
+ if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
+ // Disable this servo if it was attached
+ this->detach();
+ this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
}
}
diff --git a/src/megaavr/Servo.cpp b/src/megaavr/Servo.cpp
index 59b3e44..e7b8436 100644
--- a/src/megaavr/Servo.cpp
+++ b/src/megaavr/Servo.cpp
@@ -10,8 +10,6 @@
static servo_t servos[MAX_SERVOS]; // static array of servo structures
-uint8_t ServoCount = 0; // the total number of attached servos
-
static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval)
// convenience macros
@@ -32,7 +30,7 @@ void ServoHandler(int timer)
// Write compare register
_timer->CCMP = 0;
} else {
- if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
+ if (SERVO_INDEX(timer, currentServoIndex[timer]) < MAX_SERVOS && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
@@ -40,7 +38,7 @@ void ServoHandler(int timer)
// Select the next servo controlled by this timer
currentServoIndex[timer]++;
- if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
+ if (SERVO_INDEX(timer, currentServoIndex[timer]) < MAX_SERVOS && currentServoIndex[timer] < SERVOS_PER_TIMER) {
if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
}
@@ -114,11 +112,24 @@ static boolean isTimerActive(timer16_Sequence_t timer)
Servo::Servo()
{
- if (ServoCount < MAX_SERVOS) {
- this->servoIndex = ServoCount++; // assign a servo index to this instance
- servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
- } else {
- this->servoIndex = INVALID_SERVO; // too many servos
+ // Iterate over array to find an uninitialized servo
+ this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
+ for (int8_t i = 0; i < MAX_SERVOS; i++) {
+ if (servos[i].servoIndex == INVALID_SERVO) {
+ this->servoIndex = i;
+ servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
+ break;
+ }
+ }
+ // servoIndex will be INVALID_SERVO if no free timers found
+}
+
+Servo::~Servo()
+{
+ if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
+ // Disable this servo if it was attached
+ this->detach();
+ this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
}
}
diff --git a/src/nrf52/Servo.cpp b/src/nrf52/Servo.cpp
index 72bd504..59c11df 100644
--- a/src/nrf52/Servo.cpp
+++ b/src/nrf52/Servo.cpp
@@ -24,21 +24,30 @@
static servo_t servos[MAX_SERVOS]; // static array of servo structures
-uint8_t ServoCount = 0; // the total number of attached servos
-
-
uint32_t group_pins[3][NRF_PWM_CHANNEL_COUNT]={{NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}, {NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}, {NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED, NRF_PWM_PIN_NOT_CONNECTED}};
static uint16_t seq_values[3][NRF_PWM_CHANNEL_COUNT]={{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
Servo::Servo()
{
- if (ServoCount < MAX_SERVOS) {
- this->servoIndex = ServoCount++; // assign a servo index to this instance
- } else {
- this->servoIndex = INVALID_SERVO; // too many servos
+ // Iterate over array to find an uninitialized servo
+ this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
+ for (int8_t i = 0; i < MAX_SERVOS; i++) {
+ if (servos[i].servoIndex == INVALID_SERVO) {
+ this->servoIndex = i;
+ break;
+ }
}
+ // servoIndex will be INVALID_SERVO if no free timers found
+}
+Servo::~Servo()
+{
+ if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
+ // Disable this servo if it was attached
+ this->detach();
+ this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
+ }
}
uint8_t Servo::attach(int pin)
diff --git a/src/sam/Servo.cpp b/src/sam/Servo.cpp
index df5058f..b82a5c6 100644
--- a/src/sam/Servo.cpp
+++ b/src/sam/Servo.cpp
@@ -28,8 +28,6 @@
static servo_t servos[MAX_SERVOS]; // static array of servo structures
-uint8_t ServoCount = 0; // the total number of attached servos
-
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
// convenience macros
@@ -80,13 +78,13 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel)
if (Channel[timer] < 0) {
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
} else {
- if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
+ if (SERVO_INDEX(timer,Channel[timer]) < MAX_SERVOS && SERVO(timer,Channel[timer]).Pin.isActive == true) {
digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
Channel[timer]++; // increment to the next channel
- if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
+ if( SERVO_INDEX(timer,Channel[timer]) < MAX_SERVOS && Channel[timer] < SERVOS_PER_TIMER) {
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,HIGH); // its an active channel so pulse it high
@@ -182,11 +180,24 @@ static boolean isTimerActive(timer16_Sequence_t timer)
Servo::Servo()
{
- if (ServoCount < MAX_SERVOS) {
- this->servoIndex = ServoCount++; // assign a servo index to this instance
- servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
- } else {
- this->servoIndex = INVALID_SERVO; // too many servos
+ // Iterate over array to find an uninitialized servo
+ this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
+ for (int8_t i = 0; i < MAX_SERVOS; i++) {
+ if (servos[i].servoIndex == INVALID_SERVO) {
+ this->servoIndex = i;
+ servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
+ break;
+ }
+ }
+ // servoIndex will be INVALID_SERVO if no free timers found
+}
+
+Servo::~Servo()
+{
+ if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
+ // Disable this servo if it was attached
+ this->detach();
+ this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
}
}
diff --git a/src/samd/Servo.cpp b/src/samd/Servo.cpp
index d8e2ec4..a812385 100644
--- a/src/samd/Servo.cpp
+++ b/src/samd/Servo.cpp
@@ -28,8 +28,6 @@
static servo_t servos[MAX_SERVOS]; // static array of servo structures
-uint8_t ServoCount = 0; // the total number of attached servos
-
static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval)
// convenience macros
@@ -63,7 +61,7 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel, uint8_t in
tc->COUNT16.COUNT.reg = (uint16_t) 0;
WAIT_TC16_REGS_SYNC(tc)
} else {
- if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
+ if (SERVO_INDEX(timer, currentServoIndex[timer]) < MAX_SERVOS && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
}
}
@@ -71,7 +69,7 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel, uint8_t in
// Select the next servo controlled by this timer
currentServoIndex[timer]++;
- if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
+ if (SERVO_INDEX(timer, currentServoIndex[timer]) < MAX_SERVOS && currentServoIndex[timer] < SERVOS_PER_TIMER) {
if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
}
@@ -197,11 +195,24 @@ static boolean isTimerActive(timer16_Sequence_t timer)
Servo::Servo()
{
- if (ServoCount < MAX_SERVOS) {
- this->servoIndex = ServoCount++; // assign a servo index to this instance
- servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
- } else {
- this->servoIndex = INVALID_SERVO; // too many servos
+ // Iterate over array to find an uninitialized servo
+ this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
+ for (int8_t i = 0; i < MAX_SERVOS; i++) {
+ if (servos[i].servoIndex == INVALID_SERVO) {
+ this->servoIndex = i;
+ servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
+ break;
+ }
+ }
+ // servoIndex will be INVALID_SERVO if no free timers found
+}
+
+Servo::~Servo()
+{
+ if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
+ // Disable this servo if it was attached
+ this->detach();
+ this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
}
}
diff --git a/src/stm32f4/Servo.cpp b/src/stm32f4/Servo.cpp
index 01d05d9..bee1176 100644
--- a/src/stm32f4/Servo.cpp
+++ b/src/stm32f4/Servo.cpp
@@ -59,6 +59,11 @@ Servo::Servo() {
this->resetFields();
}
+Servo::~Servo() {
+ this->detach();
+ this->resetFields();
+}
+
bool Servo::attach(uint8 pin, uint16 minPW, uint16 maxPW, int16 minAngle, int16 maxAngle)
{
// SerialUSB.begin(115200);