added read methods

Author: dadul96

src/Servo_Hardware_PWM.cpp

@@ -398,30 +398,44 @@ void Servo::writeMicroseconds(int value)
 		else if (value > this->max) {
 			value = this->max;
 		}
+		this->pulseWidth = value;
+
 		if (this->servoPin == 2 && pinActive[0] == BOOL_TRUE) {
 			OCR3B = 0x0;
-			OCR3B = value * 2;
+			OCR3B = this->pulseWidth * 2;
 		}
 		else if (this->servoPin == 3 && pinActive[1] == BOOL_TRUE) {
 			OCR3C = 0x0;
-			OCR3C = value * 2;
+			OCR3C = this->pulseWidth * 2;
 		}
 		else if (this->servoPin == 7 && pinActive[2] == BOOL_TRUE) {
 			OCR4B = 0x0;
-			OCR4B = value * 2;
+			OCR4B = this->pulseWidth * 2;
 		}
 		else if (this->servoPin == 8 && pinActive[3] == BOOL_TRUE) {
 			OCR4C = 0x0;
-			OCR4C = value * 2;
+			OCR4C = this->pulseWidth * 2;
 		}
 		else if (this->servoPin == 44 && pinActive[4] == BOOL_TRUE) {
 			OCR5C = 0x0;
-			OCR5C = value * 2;
+			OCR5C = this->pulseWidth * 2;
 		}
 		else if (this->servoPin == 45 && pinActive[5] == BOOL_TRUE) {
 			OCR5B = 0x0;
-			OCR5B = value * 2;
+			OCR5B = this->pulseWidth * 2;
 		}
 	}
 }
+
+int read() 
+{
+	int angle;
+	angle = (((180 * (this->readMicroseconds() - this->min)) / (this->max - this->min));
+	return angle;
+}
+
+int readMicroseconds() 
+{
+	return this->pulseWidth;
+}
 #endif

src/Servo_Hardware_PWM.h

@@ -69,19 +69,23 @@ class Servo
 {
 public:
 	Servo();
-	uint8_t attach(int pin);								//attach the given pin; returns servoIndex number or 255 if too many servos
-	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
+	uint8_t attach(int pin);									//attach the given pin; returns servoIndex number or 255 if too many servos
+	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
 	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
-	void detach();											//detach the used pin
-	void detachAll();										//automatically detaches all used pins
-	void write(int value);									//write angle in degrees
-	void writeMicroseconds(int value);						//write pulse width in microseconds 
+	void detach();												//detach the used pin
+	void detachAll();											//automatically detaches all used pins
+	void write(int value);										//write angle in degrees
+	void writeMicroseconds(int value);							//write pulse width in microseconds
+	int read();													//returns the current write angle in degrees
+	int readMicroseconds();										//returns the current write angle in microseconds
+
 private:
-	uint8_t servoIndex;										//number of attached Servos
-	uint8_t servoPin;										//pin number of the attached Servo
-	uint16_t min;											//lower pulse width limit
-	uint16_t max;											//upper pulse width limit
-	uint16_t defaultPos;									//pulse width when servo is attached
+	uint8_t servoIndex;											//number of attached Servos
+	uint8_t servoPin;											//pin number of the attached Servo
+	uint16_t min;												//lower pulse width limit
+	uint16_t max;												//upper pulse width limit
+	uint16_t defaultPos;										//pulse width when servo is attached
+	uint16_t pulseWidth;										//set pulse width
 };
 
 #endif