Diff: Servo.cpp

Revision:

9:6bfea9af4dcb

Parent:

8:131785ed96fb

Child:

10:f0df63e99a96

--- a/Servo.cpp	Wed Oct 03 07:28:03 2012 +0000
+++ b/Servo.cpp	Sun Oct 21 00:03:06 2012 +0000
@@ -17,9 +17,12 @@
 // the initial pulse width can be omitted, giving a default of 
 // 1500 us. 
 // This should be at about half the range of most servos.
-Servo::Servo(PinName pin, unsigned int width) : signalPin(pin)
+Servo::Servo(PinName pin) : signalPin(pin)
 {
-    pulseWidth = width;
+    // Set default calibration
+    calibrate(2000, 1000, 60.f, -60.f);
+    
+    pulseWidth = center;
     
     if (numServos == 0)
     {
@@ -34,7 +37,36 @@
 
 
 
-void Servo::write(int width)
+bool Servo::calibrate(unsigned int plus60, unsigned int minus60, float upperLimit, float lowerLimit)
+{
+    // Check if given parameters are valid
+    if (plus60 > minus60 && upperLimit > lowerLimit)
+    {
+        center = (plus60 + minus60) / 2;
+        usPerDegree = (plus60 - center) / 60.f;
+        this->upperLimit = upperLimit;
+        this->lowerLimit = lowerLimit;   
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+
+
+void Servo::write(float degrees)
+{
+    // Limit to the valid angle range
+    degrees = (degrees > upperLimit ? upperLimit : (degrees < lowerLimit ? lowerLimit : degrees));
+    
+    pulseWidth = center + (int)(degrees * usPerDegree);
+}
+
+
+
+void Servo::writeWidth(unsigned int width)
 {
     // Make sure that the pulse width is less than the refresh period
     pulseWidth = width < period ? width : period;
@@ -42,23 +74,37 @@
 
 
 
-int Servo::read()
+float Servo::read()
+{
+    return ((int)pulseWidth - (int)center) / usPerDegree;
+}
+
+
+
+int Servo::readWidth()
 {
     return pulseWidth;
 }
 
 
 
-void Servo::operator=(int width)
+void Servo::operator=(float degrees)
 {
-    write(width);
+    write(degrees);
+}
+
+
+
+Servo::operator float()
+{
+    return read();
 }
 
 
 
 Servo::operator int()
 {
-    return read();
+    return readWidth();
 }