Diff: main.cpp

Revision:

54:4cda9af56bed

Parent:

53:bf0d97487e84

Child:

55:726fdab812a9

--- a/main.cpp	Tue Oct 27 10:04:46 2015 +0000
+++ b/main.cpp	Tue Oct 27 10:11:29 2015 +0000
@@ -2,6 +2,7 @@
 #include "HIDScope.h"
 #include "MODSERIAL.h"
 #include "biquadFilter.h"
+#include "Servo.h"
 #include "QEI.h"
 #include "math.h"
 #include <string> 
@@ -177,7 +178,8 @@
 //Forward Kinematics
 const double l1 = 0.25; const double l2 = 0.25;     //Arm lengths
 double current_x; double current_y;                 //Current position
-double theta1; double theta2;                       //Current angles
+double theta1; double theta2; double theta3;        //Current angles
+double servopos;                                    //servo position in usec
 double rpc;                                         //Encoder resolution: radians per count
 
 //Reference position
@@ -1052,7 +1054,8 @@
     
     //Servo alignment
     theta3 = 180 - theta1 - theta2;
-    servoPwm.SetPosition = (2100/180)*theta3 + 600;
+    servopos = (2100/180)*theta3 + 600;
+    servoPwm.SetPosition(servopos);
     
     /*if(u1 > 0)
     {