Diff: src/dynamics.cpp

Revision:

43:68faf056ed5c

Parent:

39:b765b6dd01c3

Child:

46:4497e945de6b

--- a/src/dynamics.cpp	Tue Nov 26 19:38:54 2013 +0000
+++ b/src/dynamics.cpp	Tue Nov 26 21:26:44 2013 +0000
@@ -5,13 +5,10 @@
 Joints invKinBody(Point &in){
     float x=in.x;
     float y=in.y;
-    float theta1 = (float) 2 * atan(sqrt( ( (l2+l3+l4)*(l2+l3+l4) - x*x - y*y) / (x*x + y*y - (l2+l4-l3)*(l2+l4-l3)) ));
-    float theta2 = (float) atan2(y,x) - atan2(l3*sin(theta1), l2 + l4 + l3*cos(theta1));
+    //float theta1 = (float) 2 * atan(sqrt( ( (l2+l3+l4)*(l2+l3+l4) - x*x - y*y) / (x*x + y*y - (l2+l4-l3)*(l2+l4-l3)) ));
+    //float theta2 = (float) atan2(y,x) - atan2(l3*sin(theta1), l2 + l4 + l3*cos(theta1));
+    float theta2 = -3.14159+((float) acos((x*x+y*y-(l2+l4)*(l2+l4)-l3*l3)/(2*(l2+l4)*l3)))+3.1415/8;
+    float theta1 = -atan2(y,x)-acos((l3*l3-x*x-y*y-(l2+l4)*(l2+l4))/(2*(l2+l4)*sqrt(x*x+y*y)));
     return Joints(theta1, theta2);
 }
 
-float kinGantry (theta3){
-    float y_gplus = lg*sin(theta3);
-    float y_plus = y_gplus-l1;
-    return h+y_plus;  
-    }
\ No newline at end of file