Skip to content

Commit 1e0726d

Browse files
dadul96dadul96
committed
updated some data types
updated the constructor method updated the attach method updated the detach method updated the write method
1 parent 2960d3a commit 1e0726d

File tree

2 files changed

+70
-55
lines changed

2 files changed

+70
-55
lines changed

‎src/Servo_Hardware_PWM.cpp‎

Lines changed: 64 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -7,21 +7,31 @@ For information about the library, license and author, see Servo_Hardware_PWM.h
77
#include <Arduino.h>
88
#include <Servo_Hardware_PWM.h>
99

10-
static int servoCount = 0;
11-
static bool pinActive[MAX_SERVOS] = {false};
10+
#define BOOL_FALSE 0
11+
#define BOOL_TRUE 255
12+
13+
static uint8_t servoCount = 0;
14+
static uint8_t pinActive[MAX_SERVOS] = {BOOL_FALSE};
1215

1316
Servo::Servo()
1417
{
18+
this->servoIndex = 0;
19+
this->servoPin = 0;
20+
1521
if (servoCount < MAX_SERVOS) {
1622
this->servoIndex = servoCount++; //assign a servoIndex to this instance
1723
}
24+
else
25+
{
26+
this->servoIndex = INVALID_SERVO_NUMBER; //too many servos
27+
}
1828
}
1929

20-
void Servo::attach(int pin)
30+
uint8_t Servo::attach(int pin)
2131
{
2232
if (this->servoIndex < MAX_SERVOS) {
2333
if (pin == 2) {
24-
if(pinActive[1] == false){
34+
if(pinActive[1] == BOOL_FALSE){
2535
//resetting the control register A and B:
2636
TCCR3A = 0x0;
2737
TCCR3B = 0x0;
@@ -45,11 +55,11 @@ void Servo::attach(int pin)
4555
//OC3B, Port E, Bit 4; setting pin 2 as output:
4656
DDRE |= (1 << PE4); //bit 4 (pin 2) as output
4757

48-
pinActive[0] = true;
58+
pinActive[0] = BOOL_TRUE;
4959
this->servoPin = 2;
5060
}
5161
else if (pin == 3) {
52-
if(pinActive[0] == false){
62+
if(pinActive[0] == BOOL_FALSE){
5363
//resetting the control register A and B:
5464
TCCR3A = 0x0;
5565
TCCR3B = 0x0;
@@ -73,11 +83,11 @@ void Servo::attach(int pin)
7383
//OC3C, Port E, Bit 5; setting pin 3 as output:
7484
DDRE |= (1 << PE5); //bit 5 (pin 3) as output
7585

76-
pinActive[1] = true;
86+
pinActive[1] = BOOL_TRUE;
7787
this->servoPin = 3;
7888
}
7989
else if (pin == 7) {
80-
if(pinActive[3] == false){
90+
if(pinActive[3] == BOOL_FALSE){
8191
//resetting the control register A and B:
8292
TCCR4A = 0x0;
8393
TCCR4B = 0x0;
@@ -101,11 +111,11 @@ void Servo::attach(int pin)
101111
//OC4B, Port H, Bit 4; setting pin 7 as output:
102112
DDRH |= (1 << PH4); //bit 4 (pin 7) as output
103113

104-
pinActive[2] = true;
114+
pinActive[2] = BOOL_TRUE;
105115
this->servoPin = 7;
106116
}
107117
else if (pin == 8) {
108-
if(pinActive[2] == false){
118+
if(pinActive[2] == BOOL_FALSE){
109119
//resetting the control register A and B:
110120
TCCR4A = 0x0;
111121
TCCR4B = 0x0;
@@ -129,11 +139,11 @@ void Servo::attach(int pin)
129139
//OC4C, Port H, Bit 5; setting pin 8 as output:
130140
DDRH |= (1 << PH5); //bit 5 (pin 8) as output
131141

132-
pinActive[3] = true;
142+
pinActive[3] = BOOL_TRUE;
133143
this->servoPin = 8;
134144
}
135145
else if (pin == 44) {
136-
if(pinActive[5] == false){
146+
if(pinActive[5] == BOOL_FALSE){
137147
//resetting the control register A and B:
138148
TCCR5A = 0x0;
139149
TCCR5B = 0x0;
@@ -157,11 +167,11 @@ void Servo::attach(int pin)
157167
//OC5C, Port L, Bit 5; setting pin 44 as output:
158168
DDRL |= (1 << PL5); //bit 5 (pin 44) as output
159169

160-
pinActive[4] = true;
170+
pinActive[4] = BOOL_TRUE;
161171
this->servoPin = 44;
162172
}
163173
else if (pin == 45) {
164-
if(pinActive[4] == false){
174+
if(pinActive[4] == BOOL_FALSE){
165175
//resetting the control register A and B:
166176
TCCR5A = 0x0;
167177
TCCR5B = 0x0;
@@ -185,16 +195,18 @@ void Servo::attach(int pin)
185195
//OC5B, Port L, Bit 4; setting pin 45 as output:
186196
DDRL |= (1 << PL4); //bit 4 (pin 45) as output
187197

188-
pinActive[5] = true;
198+
pinActive[5] = BOOL_TRUE;
189199
this->servoPin = 45;
190200
}
191201
}
202+
203+
return this->servoIndex;
192204
}
193205

194206
void Servo::detach()
195207
{
196-
if (servoPin == 2) {
197-
if(pinActive[1] == false){
208+
if (servoPin == 2 && pinActive[0] == BOOL_TRUE) {
209+
if(pinActive[1] == BOOL_FALSE){
198210
//resetting the control register A and B:
199211
TCCR3A = 0x0;
200212
TCCR3B = 0x0;
@@ -203,11 +215,11 @@ void Servo::detach()
203215
}
204216
OCR3B = 0x0; //resetting the pulse width
205217
DDRE ^= (1 << PE4); //bit 4 (pin 2) stop output
206-
pinActive[0] = false;
218+
pinActive[0] = BOOL_FALSE;
207219
this->servoPin = 0;
208220
}
209-
else if (servoPin == 3) {
210-
if(pinActive[0] == false){
221+
else if (servoPin == 3 && pinActive[1] == BOOL_TRUE) {
222+
if(pinActive[0] == BOOL_FALSE){
211223
//resetting the control register A and B:
212224
TCCR3A = 0x0;
213225
TCCR3B = 0x0;
@@ -216,11 +228,11 @@ void Servo::detach()
216228
}
217229
OCR3C = 0x0; //resetting the pulse width
218230
DDRE ^= (1 << PE5); //bit 5 (pin 3) stop output
219-
pinActive[1] = false;
231+
pinActive[1] = BOOL_FALSE;
220232
this->servoPin = 0;
221233
}
222-
else if (servoPin == 7) {
223-
if(pinActive[3] == false){
234+
else if (servoPin == 7 && pinActive[2] == BOOL_TRUE) {
235+
if(pinActive[3] == BOOL_FALSE){
224236
//resetting the control register A and B:
225237
TCCR4A = 0x0;
226238
TCCR4B = 0x0;
@@ -229,11 +241,11 @@ void Servo::detach()
229241
}
230242
OCR4B = 0x0; //resetting the pulse width
231243
DDRH ^= (1 << PH4); //bit 4 (pin 7) stop output
232-
pinActive[2] = false;
244+
pinActive[2] = BOOL_FALSE;
233245
this->servoPin = 0;
234246
}
235-
else if (servoPin == 8) {
236-
if(pinActive[2] == false){
247+
else if (servoPin == 8 && pinActive[3] == BOOL_TRUE) {
248+
if(pinActive[2] == BOOL_FALSE){
237249
//resetting the control register A and B:
238250
TCCR4A = 0x0;
239251
TCCR4B = 0x0;
@@ -242,11 +254,11 @@ void Servo::detach()
242254
}
243255
OCR4C = 0x0; //resetting the pulse width
244256
DDRH ^= (1 << PH5); //bit 5 (pin 8) stop output
245-
pinActive[3] = false;
257+
pinActive[3] = BOOL_FALSE;
246258
this->servoPin = 0;
247259
}
248-
else if (servoPin == 44) {
249-
if(pinActive[5] == false){
260+
else if (servoPin == 44 && pinActive[4] == BOOL_TRUE) {
261+
if(pinActive[5] == BOOL_FALSE){
250262
//resetting the control register A and B:
251263
TCCR5A = 0x0;
252264
TCCR5B = 0x0;
@@ -255,11 +267,11 @@ void Servo::detach()
255267
}
256268
OCR5C = 0x0; //resetting the pulse width
257269
DDRL ^= (1 << PL5); //bit 5 (pin 44) stop output
258-
pinActive[4] = false;
270+
pinActive[4] = BOOL_FALSE;
259271
this->servoPin = 0;
260272
}
261-
else if (servoPin == 45) {
262-
if(pinActive[4] == false){
273+
else if (servoPin == 45 && pinActive[5] == BOOL_TRUE) {
274+
if(pinActive[4] == BOOL_FALSE){
263275
//resetting the control register A and B:
264276
TCCR5A = 0x0;
265277
TCCR5B = 0x0;
@@ -268,13 +280,13 @@ void Servo::detach()
268280
}
269281
OCR5B = 0x0; //resetting the pulse width
270282
DDRL ^= (1 << PL4); //bit 4 (pin 45) stop output
271-
pinActive[5] = false;
283+
pinActive[5] = BOOL_FALSE;
272284
this->servoPin = 0;
273285
}
274286
}
275287

276288
void Servo::detachAll() {
277-
if (pinActive[0] == true)
289+
if (pinActive[0] == BOOL_TRUE)
278290
{
279291
//resetting the control register A and B:
280292
TCCR3A = 0x0;
@@ -284,9 +296,9 @@ void Servo::detachAll() {
284296

285297
OCR3B = 0x0; //resetting the pulse width
286298
DDRE ^= (1 << PE4); //bit 4 (pin 2) stop output
287-
pinActive[0] = false;
299+
pinActive[0] = BOOL_FALSE;
288300
}
289-
if (pinActive[1] == true)
301+
if (pinActive[1] == BOOL_TRUE)
290302
{
291303
//resetting the control register A and B:
292304
TCCR3A = 0x0;
@@ -296,9 +308,9 @@ void Servo::detachAll() {
296308

297309
OCR3C = 0x0; //resetting the pulse width
298310
DDRE ^= (1 << PE5); //bit 5 (pin 3) stop output
299-
pinActive[1] = false;
311+
pinActive[1] = BOOL_FALSE;
300312
}
301-
if (pinActive[2] == true)
313+
if (pinActive[2] == BOOL_TRUE)
302314
{
303315
//resetting the control register A and B:
304316
TCCR4A = 0x0;
@@ -308,9 +320,9 @@ void Servo::detachAll() {
308320

309321
OCR4B = 0x0; //resetting the pulse width
310322
DDRH ^= (1 << PH4); //bit 4 (pin 7) stop output
311-
pinActive[2] = false;
323+
pinActive[2] = BOOL_FALSE;
312324
}
313-
if (pinActive[3] == true)
325+
if (pinActive[3] == BOOL_TRUE)
314326
{
315327
//resetting the control register A and B:
316328
TCCR4A = 0x0;
@@ -320,9 +332,9 @@ void Servo::detachAll() {
320332

321333
OCR4C = 0x0; //resetting the pulse width
322334
DDRH ^= (1 << PH5); //bit 5 (pin 8) stop output
323-
pinActive[3] = false;
335+
pinActive[3] = BOOL_FALSE;
324336
}
325-
if (pinActive[4] == true)
337+
if (pinActive[4] == BOOL_TRUE)
326338
{
327339
//resetting the control register A and B:
328340
TCCR5A = 0x0;
@@ -332,9 +344,9 @@ void Servo::detachAll() {
332344

333345
OCR5C = 0x0; //resetting the pulse width
334346
DDRL ^= (1 << PL5); //bit 5 (pin 44) stop output
335-
pinActive[4] = false;
347+
pinActive[4] = BOOL_FALSE;
336348
}
337-
if (pinActive[5] == true)
349+
if (pinActive[5] == BOOL_TRUE)
338350
{
339351
//resetting the control register A and B:
340352
TCCR5A = 0x0;
@@ -344,7 +356,7 @@ void Servo::detachAll() {
344356

345357
OCR5B = 0x0; //resetting the pulse width
346358
DDRL ^= (1 << PL4); //bit 4 (pin 45) stop output
347-
pinActive[5] = false;
359+
pinActive[5] = BOOL_FALSE;
348360
}
349361
}
350362

@@ -358,7 +370,7 @@ void Servo::write(int value)
358370
{
359371
value = 180;
360372
}
361-
value = map(value, 0, 180, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
373+
value = (((MAX_PULSE_WIDTH - MIN_PULSE_WIDTH) * value) / 180) + MIN_PULSE_WIDTH;
362374

363375
this->writeMicroseconds(value);
364376
}
@@ -372,27 +384,27 @@ void Servo::writeMicroseconds(int value)
372384
else if (value > MAX_PULSE_WIDTH) {
373385
value = MAX_PULSE_WIDTH;
374386
}
375-
if (this->servoPin == 2 && pinActive[0] == true) {
387+
if (this->servoPin == 2 && pinActive[0] == BOOL_TRUE) {
376388
OCR3B = 0x0;
377389
OCR3B = value * 2;
378390
}
379-
else if (this->servoPin == 3 && pinActive[1] == true) {
391+
else if (this->servoPin == 3 && pinActive[1] == BOOL_TRUE) {
380392
OCR3C = 0x0;
381393
OCR3C = value * 2;
382394
}
383-
else if (this->servoPin == 7 && pinActive[2] == true) {
395+
else if (this->servoPin == 7 && pinActive[2] == BOOL_TRUE) {
384396
OCR4B = 0x0;
385397
OCR4B = value * 2;
386398
}
387-
else if (this->servoPin == 8 && pinActive[3] == true) {
399+
else if (this->servoPin == 8 && pinActive[3] == BOOL_TRUE) {
388400
OCR4C = 0x0;
389401
OCR4C = value * 2;
390402
}
391-
else if (this->servoPin == 44 && pinActive[4] == true) {
403+
else if (this->servoPin == 44 && pinActive[4] == BOOL_TRUE) {
392404
OCR5C = 0x0;
393405
OCR5C = value * 2;
394406
}
395-
else if (this->servoPin == 45 && pinActive[5] == true) {
407+
else if (this->servoPin == 45 && pinActive[5] == BOOL_TRUE) {
396408
OCR5B = 0x0;
397409
OCR5B = value * 2;
398410
}

‎src/Servo_Hardware_PWM.h‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -49,6 +49,8 @@
4949
#ifndef Servo_Hardware_PWM_h
5050
#define Servo_Hardware_PWM_h
5151

52+
#include <inttypes.h>
53+
5254
#if !defined(ARDUINO_ARCH_AVR)
5355
#error "This library only supports boards with an AVR processor."
5456
#endif
@@ -61,19 +63,20 @@
6163
#define MAX_TIMER_COUNT 40000 //the timer TOP value (for creating 50Hz)
6264

6365
#define MAX_SERVOS 6 //6 Servos can be attached
66+
#define INVALID_SERVO_NUMBER 255 //flag indicating an invalid servo index
6467

6568
class Servo
6669
{
6770
public:
6871
Servo();
69-
void attach(int pin); //attach the given pin
72+
uint8_t attach(int pin); //attach the given pin; returns servoIndex number or 255 if too many servos
7073
void detach(); //detach the used pin
7174
void detachAll(); //automatically detaches all used pins
7275
void write(int value); //write angle in degrees
7376
void writeMicroseconds(int value); //write pulse width in microseconds
7477
private:
75-
int servoIndex = 0; //number of attached Servos
76-
int servoPin = 0; //pin number of the attached Servo
78+
uint8_t servoIndex; //number of attached Servos
79+
uint8_t servoPin; //pin number of the attached Servo
7780
};
7881

7982
#endif

0 commit comments

Comments
 (0)