Diff: Controls/Kinmatics.cpp

Revision:

10:769cc457c3a4

Parent:

8:1a3a69fecedf

Child:

11:711d3c207e8c

--- a/Controls/Kinmatics.cpp	Sat Dec 05 00:40:42 2015 +0000
+++ b/Controls/Kinmatics.cpp	Sat Dec 05 09:04:23 2015 +0000
@@ -2,18 +2,31 @@
 #include <math.h>
 
 
-void getMassMatrix(float A[2][2]){
-    A[0][0] = 0;
-    A[0][1] = 0;
-    A[1][0] = 0;
-    A[1][1] = 0;
+void getMassMatrix(float A[2][2], volatile float z[4], float p[10]){
+    float I1 = p[6];
+    float I2 = p[7];
+    float c1 = p[2];
+    float c2 = p[3];
+    float l1 = p[0];
+    float m1 = p[4];
+    float m2 = p[5];
+    float th2 = z[1];
+    float t2 = pow(c2,2);
+    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][1] = t6;
+    A[1][0] = t6;
+    A[1][1] = I2+t3;
 }
 
 float getPositionOfFoot(){
     return 0;
 }
 
-float getEnergy(float z[4], float p[10]){
+float getEnergy(volatile float z[4], float p[10]){
     float I1 = p[6];
     float I2 = p[7];
     float c1 = p[2];
@@ -34,7 +47,7 @@
     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;
 }
 
-float getGravity(float z[4], float p[10]){
+float getGravity(volatile float z[4], float p[10]){
 //    c1 = p(3,:);
     float c2 = p[3];
     float g = p[8];
@@ -49,7 +62,7 @@
     return c2*g*m2*t4;
 }
 
-float getCoriolis(float z[4], float p[10]){
+float getCoriolis(volatile float z[4], float p[10]){
     float c2 = p[3];
     float dth1 = z[2];
 //    float dth2 = z[3];