Diff: Kinematic.cpp

Revision:

3:fd037c89ba5f

Parent:

2:c9c38629be85

Child:

4:d3bb9ead2a55

--- a/Kinematic.cpp	Tue Dec 22 18:01:37 2015 +0000
+++ b/Kinematic.cpp	Wed Dec 23 00:32:09 2015 +0000
@@ -1,6 +1,8 @@
 #include "Kinematic.h"
 #include "math.h"
 
+#define Kinematic_Debug
+
 #define PI 3.1415926
 
 Kinematic::Kinematic(float Link_Hip, float Link_Knee)
@@ -9,22 +11,28 @@
     this->Link_Knee = Link_Knee;
 }
 
-Kinematic::Kinematic(char text, float Link_Hip, float Link_Knee, float Zeta_Hip, float Zeta_Knee)
+Kinematic::Kinematic(char text, float Link_Hip, float Link_Knee, float Hip, float Knee)
 {
-    if(text == 'Z' && text == 'z') {
+    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;
+        Zeta_Hip = Hip;
+        Zeta_Knee = Knee;
         ForwardKinematicCalculation();
+#ifdef Kinematic_Debug
+        test.printf("Forward Kinematic\n");
+#endif
     }
-    
-    if(text == 'P' && text == 'p') {
+
+    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;
+        Position_Y = Hip;
+        Position_Z = Knee;
         InverseKinematicCalculation();
+#ifdef Kinematic_Debug
+        test.printf("Inverse Kinematic\n");
+#endif
     }
 }
 
@@ -58,6 +66,31 @@
     this->Position_Z = Position_Z;
 }
 
+void Kinematic::set_zeta_knee_range(float zeta1,float zeta2)
+{
+    zeta_knee_range_max = zeta2;
+    zeta_knee_range_min = zeta1;
+}
+
+void Kinematic::set_zeta_hip_range(float zeta1,float zeta2)
+{
+    zeta_hip_range_max = zeta2;
+    zeta_hip_range_min = zeta1;
+}
+
+void Kinematic::set_offset_YZ(float offset_y, float offset_z)
+{
+    this->offset_y = offset_y;
+    this->offset_z = offset_z;
+}
+
+void Kinematic::SumPositionWithOffset()
+{
+    Position_Y += offset_y;
+    Position_Z += offset_z;
+}
+
+
 void Kinematic::ForwardKinematicCalculation()
 {
     Position_Y = Link_Hip*sin(Link_Knee)+Link_Knee*sin(Link_Knee+Zeta_Knee); //zeta1-zeta2
@@ -70,31 +103,57 @@
     float L;
     L = (Position_Y*Position_Y) + (Position_Z*Position_Z);
     L = sqrt(L);
+#ifdef Kinematic_Debug
+    test.printf("L : %f\n",L);
+#endif
 
 // Work out the Knee angle
-    float Knee = (Link_Hip*Link_Hip)+(Link_Knee*Link_Knee)-(L*L);
+    //test.printf("Link Hip : %f, Link Knee : %f\n",Link_Hip,Link_Knee);
+    float Knee;
+    Knee = pow(Link_Hip,2)+pow(Link_Knee,2)-pow(L,2);
     Knee = Knee / (2*Link_Hip*Link_Knee);
     Knee = acos(Knee);
     Knee = Knee * 180 / PI;
     Zeta_Knee = Knee;
+#ifdef Kinematic_Debug
+    test.printf("Zeta Knee : %f\n",Zeta_Knee);
+#endif
 
 // Work out Alpha
     float Alpha = (Link_Hip*Link_Hip)+(L*L)-(Link_Knee*Link_Knee);
     Alpha = Alpha / (2*Link_Hip*L);
     Alpha = acos(Alpha);
+#ifdef Kinematic_Debug
+    test.printf("Alpha : %f\n",Alpha);
+#endif
 
 
 // Work out Beta
     float Beta = (float) Position_Y/(float) Position_Z;
     Beta = atan(Beta);
+#ifdef Kinematic_Debug
+    test.printf("Beta : %f\n",Beta);
+#endif
 
 // FinallZ work out the Hip angle
     float Hip = PI - Alpha - Beta;
     Hip = Hip * 180 / PI;
     Zeta_Hip = Hip;
+#ifdef Kinematic_Debug
+    test.printf("Zeta Hip : %f\n",Zeta_Hip);
+#endif
 
-#ifdef Stabilizer_Debug
-    printf("Y = %f, Hip = %.2f, Knee = %.2f\n",Position_Y,hip_angle,knee_angle);
+#ifdef Kinematic_Debug
+    test.printf("Y = %f, Z = %.2f\n",Position_Y,Position_Z);
 #endif
-    //printf("Height in Stabilizer : %f\n",height);
+    //test.test.test.test.test.printf("Height in Stabilizer : %f\n",height);
+}
+
+void Kinematic::print()
+{
+    if(!(Zeta_Hip > zeta_hip_range_max || Zeta_Hip < zeta_hip_range_min)) test.printf("Z-Hip : Out of Range, ");
+    else test.printf("Z-Hip : %f, ",Zeta_Hip);
+    if(!(Zeta_Knee > zeta_knee_range_max || Zeta_Knee < zeta_knee_range_min)) test.printf("Z-Knee : Out of Range, ");
+    else test.printf("Z-Knee : %f, ",Zeta_Knee);
+    test.printf("Y : %f, Z : %f\n",Position_Y,Position_Z);
 }
\ No newline at end of file