Skip to content

Commit 987fb8f

Browse files
fwacerfwacer
committed
Resolves arduino-libraries#67 by changing Servo constructors and adding destructors. Constructor looks for an unused Servo to re-use first, before adding onto the end of the array.
1 parent 9fe7546 commit 987fb8f

File tree

8 files changed

+121
-51
lines changed

8 files changed

+121
-51
lines changed

Diff for: ‎src/Servo.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -105,6 +105,7 @@ class Servo
105105
{
106106
public:
107107
Servo();
108+
~Servo();
108109
uint8_t attach(int pin); // attach the given pin to the next free channel, sets pinMode, returns channel number or INVALID_SERVO if failure
109110
uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
110111
void detach();
@@ -114,7 +115,7 @@ class Servo
114115
int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release)
115116
bool attached(); // return true if this servo is attached, otherwise false
116117
private:
117-
uint8_t servoIndex; // index into the channel data for this servo
118+
uint8_t servoIndex = INVALID_SERVO;// index into the channel data for this servo
118119
int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH
119120
int8_t max; // maximum is this value times 4 added to MAX_PULSE_WIDTH
120121
};

Diff for: ‎src/avr/Servo.cpp

+20-10
Original file line numberDiff line numberDiff line change
@@ -35,9 +35,6 @@
3535
static servo_t servos[MAX_SERVOS]; // static array of servo structures
3636
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
3737

38-
uint8_t ServoCount = 0; // the total number of attached servos
39-
40-
4138
// convenience macros
4239
#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
4340
#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
5451
if( Channel[timer] < 0 )
5552
*TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
5653
else{
57-
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true )
54+
if( SERVO_INDEX(timer,Channel[timer]) < MAX_SERVOS && SERVO(timer,Channel[timer]).Pin.isActive == true )
5855
digitalWrite( SERVO(timer,Channel[timer]).Pin.nbr,LOW); // pulse this channel low if activated
5956
}
6057

6158
Channel[timer]++; // increment to the next channel
62-
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
59+
if( SERVO_INDEX(timer,Channel[timer]) < MAX_SERVOS && Channel[timer] < SERVOS_PER_TIMER) {
6360
*OCRnA = *TCNTn + SERVO(timer,Channel[timer]).ticks;
6461
if(SERVO(timer,Channel[timer]).Pin.isActive == true) // check if activated
6562
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)
222219

223220
Servo::Servo()
224221
{
225-
if( ServoCount < MAX_SERVOS) {
226-
this->servoIndex = ServoCount++; // assign a servo index to this instance
227-
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
222+
// Iterate over array to find an uninitialized servo
223+
this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
224+
for (int8_t i = 0; i < MAX_SERVOS; i++) {
225+
if (servos[i].servoIndex == INVALID_SERVO) {
226+
this->servoIndex = i;
227+
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
228+
break;
229+
}
230+
}
231+
// servoIndex will be INVALID_SERVO if no free timers found
232+
}
233+
234+
Servo::~Servo()
235+
{
236+
if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
237+
// Disable this servo if it was attached
238+
this->detach();
239+
this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
228240
}
229-
else
230-
this->servoIndex = INVALID_SERVO ; // too many servos
231241
}
232242

233243
uint8_t Servo::attach(int pin)

Diff for: ‎src/mbed/Servo.cpp

+18-6
Original file line numberDiff line numberDiff line change
@@ -43,8 +43,7 @@ class ServoImpl {
4343
int32_t duration = -1;
4444
};
4545

46-
static ServoImpl* servos[MAX_SERVOS]; // static array of servo structures
47-
uint8_t ServoCount = 0; // the total number of attached servos
46+
static ServoImpl* servos[MAX_SERVOS] = {NULL}; // static array of servo structures
4847

4948
#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min) // minimum value in us for this servo
5049
#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
5352

5453
Servo::Servo()
5554
{
56-
if (ServoCount < MAX_SERVOS) {
57-
this->servoIndex = ServoCount++;
58-
} else {
59-
this->servoIndex = INVALID_SERVO; // too many servos
55+
// Iterate over array to find an uninitialized servo
56+
this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
57+
for (int8_t i = 0; i < MAX_SERVOS; i++) {
58+
if (servos[i] == NULL) {
59+
this->servoIndex = i;
60+
break;
61+
}
62+
}
63+
// servoIndex will be INVALID_SERVO if no free timers found
64+
}
65+
66+
Servo::~Servo()
67+
{
68+
if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
69+
// Disable this servo if it was attached
70+
this->detach();
71+
this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
6072
}
6173
}
6274

Diff for: ‎src/megaavr/Servo.cpp

+20-9
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,6 @@
1010

1111
static servo_t servos[MAX_SERVOS]; // static array of servo structures
1212

13-
uint8_t ServoCount = 0; // the total number of attached servos
14-
1513
static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval)
1614

1715
// convenience macros
@@ -32,15 +30,15 @@ void ServoHandler(int timer)
3230
// Write compare register
3331
_timer->CCMP = 0;
3432
} else {
35-
if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
33+
if (SERVO_INDEX(timer, currentServoIndex[timer]) < MAX_SERVOS && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
3634
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
3735
}
3836
}
3937

4038
// Select the next servo controlled by this timer
4139
currentServoIndex[timer]++;
4240

43-
if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
41+
if (SERVO_INDEX(timer, currentServoIndex[timer]) < MAX_SERVOS && currentServoIndex[timer] < SERVOS_PER_TIMER) {
4442
if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated
4543
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
4644
}
@@ -114,11 +112,24 @@ static boolean isTimerActive(timer16_Sequence_t timer)
114112

115113
Servo::Servo()
116114
{
117-
if (ServoCount < MAX_SERVOS) {
118-
this->servoIndex = ServoCount++; // assign a servo index to this instance
119-
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
120-
} else {
121-
this->servoIndex = INVALID_SERVO; // too many servos
115+
// Iterate over array to find an uninitialized servo
116+
this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
117+
for (int8_t i = 0; i < MAX_SERVOS; i++) {
118+
if (servos[i].servoIndex == INVALID_SERVO) {
119+
this->servoIndex = i;
120+
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
121+
break;
122+
}
123+
}
124+
// servoIndex will be INVALID_SERVO if no free timers found
125+
}
126+
127+
Servo::~Servo()
128+
{
129+
if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
130+
// Disable this servo if it was attached
131+
this->detach();
132+
this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
122133
}
123134
}
124135

Diff for: ‎src/nrf52/Servo.cpp

+16-7
Original file line numberDiff line numberDiff line change
@@ -24,21 +24,30 @@
2424

2525
static servo_t servos[MAX_SERVOS]; // static array of servo structures
2626

27-
uint8_t ServoCount = 0; // the total number of attached servos
28-
29-
3027

3128
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}};
3229
static uint16_t seq_values[3][NRF_PWM_CHANNEL_COUNT]={{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}};
3330

3431
Servo::Servo()
3532
{
36-
if (ServoCount < MAX_SERVOS) {
37-
this->servoIndex = ServoCount++; // assign a servo index to this instance
38-
} else {
39-
this->servoIndex = INVALID_SERVO; // too many servos
33+
// Iterate over array to find an uninitialized servo
34+
this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
35+
for (int8_t i = 0; i < MAX_SERVOS; i++) {
36+
if (servos[i].servoIndex == INVALID_SERVO) {
37+
this->servoIndex = i;
38+
break;
39+
}
4040
}
41+
// servoIndex will be INVALID_SERVO if no free timers found
42+
}
4143

44+
Servo::~Servo()
45+
{
46+
if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
47+
// Disable this servo if it was attached
48+
this->detach();
49+
this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
50+
}
4251
}
4352

4453
uint8_t Servo::attach(int pin)

Diff for: ‎src/sam/Servo.cpp

+20-9
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,6 @@
2828

2929
static servo_t servos[MAX_SERVOS]; // static array of servo structures
3030

31-
uint8_t ServoCount = 0; // the total number of attached servos
32-
3331
static volatile int8_t Channel[_Nbr_16timers ]; // counter for the servo being pulsed for each timer (or -1 if refresh interval)
3432

3533
// convenience macros
@@ -80,13 +78,13 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel)
8078
if (Channel[timer] < 0) {
8179
tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer
8280
} else {
83-
if (SERVO_INDEX(timer,Channel[timer]) < ServoCount && SERVO(timer,Channel[timer]).Pin.isActive == true) {
81+
if (SERVO_INDEX(timer,Channel[timer]) < MAX_SERVOS && SERVO(timer,Channel[timer]).Pin.isActive == true) {
8482
digitalWrite(SERVO(timer,Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated
8583
}
8684
}
8785

8886
Channel[timer]++; // increment to the next channel
89-
if( SERVO_INDEX(timer,Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) {
87+
if( SERVO_INDEX(timer,Channel[timer]) < MAX_SERVOS && Channel[timer] < SERVOS_PER_TIMER) {
9088
tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks;
9189
if(SERVO(timer,Channel[timer]).Pin.isActive == true) { // check if activated
9290
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)
182180

183181
Servo::Servo()
184182
{
185-
if (ServoCount < MAX_SERVOS) {
186-
this->servoIndex = ServoCount++; // assign a servo index to this instance
187-
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
188-
} else {
189-
this->servoIndex = INVALID_SERVO; // too many servos
183+
// Iterate over array to find an uninitialized servo
184+
this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
185+
for (int8_t i = 0; i < MAX_SERVOS; i++) {
186+
if (servos[i].servoIndex == INVALID_SERVO) {
187+
this->servoIndex = i;
188+
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values - 12 Aug 2009
189+
break;
190+
}
191+
}
192+
// servoIndex will be INVALID_SERVO if no free timers found
193+
}
194+
195+
Servo::~Servo()
196+
{
197+
if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
198+
// Disable this servo if it was attached
199+
this->detach();
200+
this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
190201
}
191202
}
192203

Diff for: ‎src/samd/Servo.cpp

+20-9
Original file line numberDiff line numberDiff line change
@@ -28,8 +28,6 @@
2828

2929
static servo_t servos[MAX_SERVOS]; // static array of servo structures
3030

31-
uint8_t ServoCount = 0; // the total number of attached servos
32-
3331
static volatile int8_t currentServoIndex[_Nbr_16timers]; // index for the servo being pulsed for each timer (or -1 if refresh interval)
3432

3533
// convenience macros
@@ -63,15 +61,15 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel, uint8_t in
6361
tc->COUNT16.COUNT.reg = (uint16_t) 0;
6462
WAIT_TC16_REGS_SYNC(tc)
6563
} else {
66-
if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
64+
if (SERVO_INDEX(timer, currentServoIndex[timer]) < MAX_SERVOS && SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) {
6765
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated
6866
}
6967
}
7068

7169
// Select the next servo controlled by this timer
7270
currentServoIndex[timer]++;
7371

74-
if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) {
72+
if (SERVO_INDEX(timer, currentServoIndex[timer]) < MAX_SERVOS && currentServoIndex[timer] < SERVOS_PER_TIMER) {
7573
if (SERVO(timer, currentServoIndex[timer]).Pin.isActive == true) { // check if activated
7674
digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high
7775
}
@@ -197,11 +195,24 @@ static boolean isTimerActive(timer16_Sequence_t timer)
197195

198196
Servo::Servo()
199197
{
200-
if (ServoCount < MAX_SERVOS) {
201-
this->servoIndex = ServoCount++; // assign a servo index to this instance
202-
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
203-
} else {
204-
this->servoIndex = INVALID_SERVO; // too many servos
198+
// Iterate over array to find an uninitialized servo
199+
this->servoIndex = INVALID_SERVO; // index of this servo or INVALID_SERVO if not found
200+
for (int8_t i = 0; i < MAX_SERVOS; i++) {
201+
if (servos[i].servoIndex == INVALID_SERVO) {
202+
this->servoIndex = i;
203+
servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH); // store default values
204+
break;
205+
}
206+
}
207+
// servoIndex will be INVALID_SERVO if no free timers found
208+
}
209+
210+
Servo::~Servo()
211+
{
212+
if( this->servoIndex != INVALID_SERVO ) { // if this instance is attached to a pin, make it available to be reused
213+
// Disable this servo if it was attached
214+
this->detach();
215+
this->servoIndex = INVALID_SERVO; // This instance of servo can now be reused
205216
}
206217
}
207218

Diff for: ‎src/stm32f4/Servo.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,11 @@ Servo::Servo() {
5959
this->resetFields();
6060
}
6161

62+
Servo::~Servo() {
63+
this->detach();
64+
this->resetFields();
65+
}
66+
6267
bool Servo::attach(uint8 pin, uint16 minPW, uint16 maxPW, int16 minAngle, int16 maxAngle)
6368
{
6469
// SerialUSB.begin(115200);

0 commit comments

Comments
 (0)