Diff: Controls/Kinmatics.cpp

Revision:

11:711d3c207e8c

Parent:

10:769cc457c3a4

Child:

16:5b19be27f08a

--- a/Controls/Kinmatics.cpp	Sat Dec 05 09:04:23 2015 +0000
+++ b/Controls/Kinmatics.cpp	Tue Dec 08 22:52:46 2015 +0000
@@ -11,19 +11,39 @@
     float m1 = p[4];
     float m2 = p[5];
     float th2 = z[1];
-    float t2 = pow(c2,2);
+    float t2 = c2*c2;
     float t3 = m2*t2;
     float t4 = cos(th2);
     float t5 = c2*l1*m2*t4;
     float t6 = t3+t5;
-    A[0][0] = I1+I2+t3+pow(c1,2)*m1+pow(l1,2)*m2+c2*l1*m2*t4*2.0;
+    A[0][0] = I1+I2+t3+c1*c1*m1+l1*l1*m2+c2*l1*m2*t4*2.0;
     A[0][1] = t6;
     A[1][0] = t6;
     A[1][1] = I2+t3;
 }
 
-float getPositionOfFoot(){
-    return 0;
+void getGripperPosition(float position[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;
+    position[0] = l2*sin(t2)+l1*sin(th1);
+    position[1] = -l2*cos(t2)-l1*cos(th1);
+}
+
+void getGripperVelocity(float velocity[2], volatile float z[4], float p[10]){
+    float dth1 = z[2];
+    float dth2 = z[3];
+    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 = sin(t2);
+    velocity[0] = dth1*(l2*t3+l1*cos(th1))+dth2*l2*t3;
+    velocity[1] = dth1*(l2*t4+l1*sin(th1))+dth2*l2*t4;
 }
 
 float getEnergy(volatile float z[4], float p[10]){
@@ -39,36 +59,38 @@
     float m2 = p[5];
     float th1 = z[0];
     float th2 = z[1];
-    float t2 = pow(dth1,2);
-    float t3 = pow(c2,2);
-    float t4 = pow(dth2,2);
+    float t2 = dth1*dth1;
+    float t3 = c2*c2;
+    float t4 = dth2*dth2;
     float t5 = cos(th1);
     float t6 = cos(th2);
-    return I1*t2*(1.0/2.0)+I2*t2*(1.0/2.0)+I2*t4*(1.0/2.0)+pow(c1,2)*m1*t2*(1.0/2.0)+pow(l1,2)*m2*t2*(1.0/2.0)-g*m2*(l1*t5+c2*cos(th1+th2))+m2*t2*t3*(1.0/2.0)+m2*t3*t4*(1.0/2.0)+dth1*dth2*m2*t3-c1*g*m1*t5+c2*l1*m2*t2*t6+c2*dth1*dth2*l1*m2*t6;
+    return I1*t2*(1.0/2.0)+I2*t2*(1.0/2.0)+I2*t4*(1.0/2.0)+c1*c1*m1*t2*(1.0/2.0)+l1*l1*m2*t2*(1.0/2.0)-g*m2*(l1*t5+c2*cos(th1+th2))+m2*t2*t3*(1.0/2.0)+m2*t3*t4*(1.0/2.0)+dth1*dth2*m2*t3-c1*g*m1*t5+c2*l1*m2*t2*t6+c2*dth1*dth2*l1*m2*t6;
 }
 
-float getGravity(volatile float z[4], float p[10]){
-//    c1 = p(3,:);
+void getGravity(float output[2], volatile float z[4], float p[10]){
+    float c1 = p[2];
     float c2 = p[3];
     float g = p[8];
-//    l1 = p(1,:);
-//    m1 = p(5,:);
+    float l1 = p[0];
+    float m1 = p[4];
     float m2 = p[5];
     float th1 = z[0];
     float th2 = z[1];
-//    t2 = sin(th1);
+    float t2 = sin(th1);
     float t3 = th1+th2;
     float t4 = sin(t3);
-    return c2*g*m2*t4;
+    output[0] = g*m2*(c2*t4+l1*t2)+c1*g*m1*t2;
+    output[1] = c2*g*m2*t4;
 }
 
-float getCoriolis(volatile float z[4], float p[10]){
+void getCoriolisCentrip(float output[2], volatile float z[4], float p[10]){
     float c2 = p[3];
     float dth1 = z[2];
-//    float dth2 = z[3];
+    float dth2 = z[3];
     float l1 = p[0];
     float m2 = p[5];
     float th2 = z[1];
     float t2 = sin(th2);
-    return c2*pow(dth1,2)*l1*m2*t2;
+    output[0] = -c2*dth2*l1*m2*t2*(dth1*2.0+dth2);
+    output[1] = c2*dth1*dth1*l1*m2*t2;
 }
\ No newline at end of file