Diff: Controls/Kinmatics.cpp

Revision:

16:5b19be27f08a

Parent:

11:711d3c207e8c

--- a/Controls/Kinmatics.cpp	Thu Dec 10 09:46:56 2015 +0000
+++ b/Controls/Kinmatics.cpp	Thu Dec 10 10:55:35 2015 +0000
@@ -1,5 +1,6 @@
 #include "Kinematics.h"
 #include <math.h>
+#define M_PI 3.14159265358979323846
 
 
 void getMassMatrix(float A[2][2], volatile float z[4], float p[10]){
@@ -46,6 +47,22 @@
     velocity[1] = dth1*(l2*t4+l1*sin(th1))+dth2*l2*t4;
 }
 
+void getGripperJacobianTranspose(float Jtrans[2][2], volatile float z[4], float p[10]){
+    float l1 = p[0];
+    float l2 = p[1];
+    float th1 = z[0];
+    float th2 = z[1];
+    float t2 = th1+th2;
+    float t3 = cos(t2);
+    float t4 = l2*t3;
+    float t5 = sin(t2);
+    float t6 = l2*t5;
+    Jtrans[0][0] = t4+l1*cos(th1);
+    Jtrans[0][1] = t4;
+    Jtrans[1][0] = t6+l1*sin(th1);
+    Jtrans[1][1] = t6;
+}
+
 float getEnergy(volatile float z[4], float p[10]){
     float I1 = p[6];
     float I2 = p[7];
@@ -93,4 +110,13 @@
     float t2 = sin(th2);
     output[0] = -c2*dth2*l1*m2*t2*(dth1*2.0+dth2);
     output[1] = c2*dth1*dth1*l1*m2*t2;
+}
+
+float boundTheta(float theta){
+    int numTurns = fix(theta/(2*M_PI));
+    return theta-numTurns*2*M_PI;
+}
+
+int fix(float val){//round toward zero
+    return val > 0 ? floor(val) : ceil(val);
 }
\ No newline at end of file