InverseLeg
Dependents: SwitchMode Stabilizer SwitchMode
Diff: Kinematic.cpp
- Revision:
- 3:fd037c89ba5f
- Parent:
- 2:c9c38629be85
- Child:
- 4:d3bb9ead2a55
diff -r c9c38629be85 -r fd037c89ba5f Kinematic.cpp --- 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