Diff: SteeringServo.cpp

Revision:

1:88fe796b4c2e

Parent:

0:5141c5e51f39

Child:

2:80a7bb0d71af

Child:

3:f81145ed8daa

--- a/SteeringServo.cpp	Fri Jun 23 23:36:13 2017 +0000
+++ b/SteeringServo.cpp	Sat Sep 02 00:52:36 2017 +0000
@@ -11,20 +11,30 @@
     }
 
 // -- Go to position
-void SteeringServo::goToAngle(int pulseWidth){
-    // -- Ensure that pulse width doesn't exceed limits 
-    if (pulseWidth < minimumPulseWidth_){
-        servo_.pulsewidth_us(minimumPulseWidth_);
-        }
-    else if (pulseWidth < minimumPulseWidth_){
-        servo_.pulsewidth_us(minimumPulseWidth_);
-        }
-    else{
-        servo_.pulsewidth_us(pulseWidth);
-        }
+void SteeringServo::goToAngle(int pulseWidth)
+{
+    // -- Ensure that pulse width doesn't exceed limits
+    if (pulseWidth < minimumPulseWidth_)
+    {
+        pulseWidth = mimimumPulseWidth_;
     }
+    else if (pulseWidth > maximumPulseWidth_)
+    {
+        pulseWidth = maximumPulseWidth_;
+    }
+    
+    servo_.pulsewidth_us(currentPulseWidth_ = pulseWidth); // -- set servo to pulse width and also store it in currentPulseWidth
+}
 
 // -- Go to centre (generally used for debugging)
-void SteeringServo::goStraight(void){
-    servo_.pulsewidth_us(centrePulseWidth_);
-    }
\ No newline at end of file
+void SteeringServo::goStraight(void)
+{
+    servo_.pulsewidth_us(currentPulseWidth_ = centrePulseWidth_);
+}
+    
+//-- return the current Pulse Width
+int SteeringServo::getPulseWidth(void)
+{
+    return(currentPulseWidth);
+}
+    
\ No newline at end of file