Skip to content

Commit 9dad172

Browse files
committed
Merge branch 'feature/adding_read_methods' into develop
2 parents 00c760b + db1aba7 commit 9dad172

File tree

3 files changed

+67
-27
lines changed

3 files changed

+67
-27
lines changed

keywords.txt

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -11,11 +11,13 @@ Servo KEYWORD1 Servo
1111
#######################################
1212
# Methods and Functions (KEYWORD2)
1313
#######################################
14-
attach KEYWORD2
15-
detach KEYWORD2
16-
detachAll KEYWORD2
17-
write KEYWORD2
18-
writeMicroseconds KEYWORD2
14+
attach KEYWORD2
15+
detach KEYWORD2
16+
detachAll KEYWORD2
17+
write KEYWORD2
18+
writeMicroseconds KEYWORD2
19+
read KEYWORD2
20+
readMicroseconds KEYWORD2
1921

2022
#######################################
2123
# Constants (LITERAL1)

src/Servo_Hardware_PWM.cpp

Lines changed: 45 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -376,16 +376,24 @@ void Servo::detachAll() {
376376

377377
void Servo::write(int value)
378378
{
379-
if (value < 0)
379+
float tempValue;
380+
381+
if (value <= 0)
380382
{
381-
value = 0;
383+
tempValue = 0.0;
382384
}
383-
else if (value > 180)
385+
else if (value >= 180)
384386
{
385-
value = 180;
387+
tempValue = 180.0;
386388
}
387-
value = (((this->max - this->min) * value) / 180) + this->min;
389+
else
390+
{
391+
tempValue = (float)value;
392+
}
393+
394+
tempValue = (((this->max - this->min) * tempValue) / 180.0) + this->min;
388395

396+
value = (int)tempValue;
389397
this->writeMicroseconds(value);
390398
}
391399

@@ -398,30 +406,56 @@ void Servo::writeMicroseconds(int value)
398406
else if (value > this->max) {
399407
value = this->max;
400408
}
409+
this->pulseWidth = value;
410+
401411
if (this->servoPin == 2 && pinActive[0] == BOOL_TRUE) {
402412
OCR3B = 0x0;
403-
OCR3B = value * 2;
413+
OCR3B = this->pulseWidth * 2;
404414
}
405415
else if (this->servoPin == 3 && pinActive[1] == BOOL_TRUE) {
406416
OCR3C = 0x0;
407-
OCR3C = value * 2;
417+
OCR3C = this->pulseWidth * 2;
408418
}
409419
else if (this->servoPin == 7 && pinActive[2] == BOOL_TRUE) {
410420
OCR4B = 0x0;
411-
OCR4B = value * 2;
421+
OCR4B = this->pulseWidth * 2;
412422
}
413423
else if (this->servoPin == 8 && pinActive[3] == BOOL_TRUE) {
414424
OCR4C = 0x0;
415-
OCR4C = value * 2;
425+
OCR4C = this->pulseWidth * 2;
416426
}
417427
else if (this->servoPin == 44 && pinActive[4] == BOOL_TRUE) {
418428
OCR5C = 0x0;
419-
OCR5C = value * 2;
429+
OCR5C = this->pulseWidth * 2;
420430
}
421431
else if (this->servoPin == 45 && pinActive[5] == BOOL_TRUE) {
422432
OCR5B = 0x0;
423-
OCR5B = value * 2;
433+
OCR5B = this->pulseWidth * 2;
424434
}
425435
}
426436
}
437+
438+
int Servo::read() {
439+
float angle;
440+
441+
if ((this->readMicroseconds() - this->min) <= 0)
442+
{
443+
angle = 0.0;
444+
}
445+
else
446+
{
447+
angle = (180.0 / (this->max - this->min)) * (this->readMicroseconds() - this->min);
448+
}
449+
450+
return (int)angle;
451+
}
452+
453+
int Servo::readMicroseconds() {
454+
if (this->servoIndex == INVALID_SERVO_NUMBER)
455+
{
456+
this->pulseWidth = 0;
457+
}
458+
459+
return this->pulseWidth;
460+
}
427461
#endif

src/Servo_Hardware_PWM.h

Lines changed: 15 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -69,19 +69,23 @@ class Servo
6969
{
7070
public:
7171
Servo();
72-
uint8_t attach(int pin); //attach the given pin; returns servoIndex number or 255 if too many servos
73-
uint8_t attach(int pin, int min, int max); //attach the given pin and set the upper and lower pulse width limits; returns servoIndex number or 255 if too many servos
72+
uint8_t attach(int pin); //attach the given pin; returns servoIndex number or 255 if too many servos
73+
uint8_t attach(int pin, int min, int max); //attach the given pin and set the upper and lower pulse width limits; returns servoIndex number or 255 if too many servos
7474
uint8_t attach(int pin, int min, int max, int defaultPos); //attach the given pin, set the upper and lower pulse width limits and set the pulse width when servo is attached; returns servoIndex number or 255 if too many servos
75-
void detach(); //detach the used pin
76-
void detachAll(); //automatically detaches all used pins
77-
void write(int value); //write angle in degrees
78-
void writeMicroseconds(int value); //write pulse width in microseconds
75+
void detach(); //detach the used pin
76+
void detachAll(); //automatically detaches all used pins
77+
void write(int value); //write angle in degrees
78+
void writeMicroseconds(int value); //write pulse width in microseconds
79+
int read(); //returns the current write angle in degrees
80+
int readMicroseconds(); //returns the current write angle in microseconds
81+
7982
private:
80-
uint8_t servoIndex; //number of attached Servos
81-
uint8_t servoPin; //pin number of the attached Servo
82-
uint16_t min; //lower pulse width limit
83-
uint16_t max; //upper pulse width limit
84-
uint16_t defaultPos; //pulse width when servo is attached
83+
uint8_t servoIndex; //number of attached Servos
84+
uint8_t servoPin; //pin number of the attached Servo
85+
uint16_t min; //lower pulse width limit
86+
uint16_t max; //upper pulse width limit
87+
uint16_t defaultPos; //pulse width when servo is attached
88+
uint16_t pulseWidth; //set pulse width
8589
};
8690

8791
#endif

0 commit comments

Comments
 (0)