Merge branch 'feature/adding_read_methods' into develop

Author: dadul96

keywords.txt

@@ -11,11 +11,13 @@ Servo	KEYWORD1	Servo
 #######################################
 # Methods and Functions (KEYWORD2)
 #######################################
-attach             KEYWORD2
-detach             KEYWORD2
-detachAll          KEYWORD2
-write              KEYWORD2
-writeMicroseconds  KEYWORD2
+attach	KEYWORD2
+detach	KEYWORD2
+detachAll	KEYWORD2
+write	KEYWORD2
+writeMicroseconds	KEYWORD2
+read	KEYWORD2
+readMicroseconds	KEYWORD2
 
 #######################################
 # Constants (LITERAL1)

src/Servo_Hardware_PWM.cpp

@@ -376,16 +376,24 @@ void Servo::detachAll() {
 
 void Servo::write(int value) 
 {
-	if (value < 0)
+	float tempValue;
+
+	if (value <= 0)
 	{
-		value = 0;
+		tempValue = 0.0;
 	}
-	else if (value > 180)
+	else if (value >= 180)
 	{
-		value = 180;
+		tempValue = 180.0;
 	}
-	value = (((this->max - this->min) * value) / 180) + this->min;
+	else
+	{
+		tempValue = (float)value;
+	}
+
+	tempValue = (((this->max - this->min) * tempValue) / 180.0) + this->min;
 
+	value = (int)tempValue;
 	this->writeMicroseconds(value);
 }
 
@@ -398,30 +406,56 @@ 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 Servo::read() {
+	float angle;
+
+	if ((this->readMicroseconds() - this->min) <= 0)
+	{
+		angle = 0.0;
+	}
+	else
+	{
+		angle = (180.0 / (this->max - this->min)) * (this->readMicroseconds() - this->min);
+	}
+
+	return (int)angle;
+}
+
+int Servo::readMicroseconds() {
+	if (this->servoIndex == INVALID_SERVO_NUMBER)
+	{
+		this->pulseWidth = 0;
+	}
+
+	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