Diff: main.cpp

Revision:

18:9a30764847d2

Parent:

17:0b89c374f2f7

Child:

19:eef0e3ec32a0

--- a/main.cpp	Fri Aug 28 07:23:00 2015 +0000
+++ b/main.cpp	Fri Aug 28 07:49:06 2015 +0000
@@ -203,6 +203,35 @@
     
     led4= !led4;
 }
+
+void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){
+    /*angleDeg:  desired angle
+      minDeg:    minimum angle in degrees
+      minNorVal: normalized value[0,1] when servo is at its minimum angle
+      maxDeg:    maximum angle in degrees
+      maxNorVal: normalized value[0,1] when servo is at its maximum angle
+    */
+    
+    float pos; //normalized angle, [0,1]
+    float scale; //scale factor for servo calibration
+    
+    //extreme conditions
+    if (angleDeg<minDeg){
+        pos=minNorVal;
+    }   
+    if (angleDeg>maxDeg){
+        pos=maxNorVal;
+    }
+    
+    //Calculate scale factor for servo calibration
+    scale = (angleDeg-minDeg)/(maxDeg-minDeg);
+    //Calculate servo normalized value
+    pos = minNorVal + scale*(maxNorVal-minNorVal);
+    
+    //send signal to servo
+    targetServo=pos;
+}
+
  
 int main() {
     IMU.baud(57600);