Diff: Kinematic.cpp

Revision:

2:c9c38629be85

Parent:

1:d5cf0907f83d

Child:

3:fd037c89ba5f

--- a/Kinematic.cpp	Tue Dec 22 17:28:20 2015 +0000
+++ b/Kinematic.cpp	Tue Dec 22 18:01:37 2015 +0000
@@ -1,4 +1,7 @@
 #include "Kinematic.h"
+#include "math.h"
+
+#define PI 3.1415926
 
 Kinematic::Kinematic(float Link_Hip, float Link_Knee)
 {
@@ -6,22 +9,23 @@
     this->Link_Knee = Link_Knee;
 }
 
-Kinematic::Kinematic(float Link_Hip, float Link_Knee, float Zeta_Hip, float Zeta_Knee)
+Kinematic::Kinematic(char text, float Link_Hip, float Link_Knee, float Zeta_Hip, float Zeta_Knee)
 {
-    this->Link_Hip = Link_Hip;
-    this->Link_Knee = Link_Knee;
-    this->Zeta_Hip = Zeta_Hip;
-    this->Zeta_Knee = Zeta_Knee;
-    ForwardKinematicCalculation();
-}
-
-Kinematic::Kinematic(float Link_Hip, float Link_Knee, float Position_Y, float Position_Z)
-{
-    this->Link_Hip = Link_Hip;
-    this->Link_Knee = Link_Knee;
-    this->Position_Y = Position_Y;
-    this->Position_Z = Position_Z;
-    InverseKinematicCalculation();
+    if(text == 'Z' && text == 'z') {
+        this->Link_Hip = Link_Hip;
+        this->Link_Knee = Link_Knee;
+        this->Zeta_Hip = Zeta_Hip;
+        this->Zeta_Knee = Zeta_Knee;
+        ForwardKinematicCalculation();
+    }
+    
+    if(text == 'P' && text == 'p') {
+        this->Link_Hip = Link_Hip;
+        this->Link_Knee = Link_Knee;
+        this->Position_Y = Position_Y;
+        this->Position_Z = Position_Z;
+        InverseKinematicCalculation();
+    }
 }
 
 void Kinematic::set_Link_Hip(float Link_Hip)
@@ -56,15 +60,15 @@
 
 void Kinematic::ForwardKinematicCalculation()
 {
-    Y = Link_Hip*sin(Link_Knee)+Link_Knee*sin(Link_Knee+Zeta_Knee); //zeta1-zeta2
-    Z = Link_Hip*cos(Link_Knee)+Link_Knee*cos(Link_Knee+Zeta_Knee);
+    Position_Y = Link_Hip*sin(Link_Knee)+Link_Knee*sin(Link_Knee+Zeta_Knee); //zeta1-zeta2
+    Position_Z = Link_Hip*cos(Link_Knee)+Link_Knee*cos(Link_Knee+Zeta_Knee);
 }
 
 void Kinematic::InverseKinematicCalculation()
 {
     // Begin by working out L
     float L;
-    L = (Y*Y) + (Z*Z);
+    L = (Position_Y*Position_Y) + (Position_Z*Position_Z);
     L = sqrt(L);
 
 // Work out the Knee angle
@@ -81,16 +85,16 @@
 
 
 // Work out Beta
-    float Beta = (float) Y/(float) Z;
+    float Beta = (float) Position_Y/(float) Position_Z;
     Beta = atan(Beta);
 
 // FinallZ work out the Hip angle
     float Hip = PI - Alpha - Beta;
     Hip = Hip * 180 / PI;
     Zeta_Hip = Hip;
-    
-    #ifdef Stabilizer_Debug
-    printf("Y = %f, Hip = %.2f, Knee = %.2f\n",Y,hip_angle,knee_angle);
-    #endif
+
+#ifdef Stabilizer_Debug
+    printf("Y = %f, Hip = %.2f, Knee = %.2f\n",Position_Y,hip_angle,knee_angle);
+#endif
     //printf("Height in Stabilizer : %f\n",height);
 }
\ No newline at end of file