InverseLeg
Dependents: SwitchMode Stabilizer SwitchMode
Kinematic.cpp
- Committer:
- soulx
- Date:
- 2016-01-23
- Revision:
- 6:444a33819962
- Parent:
- 5:a2a5ebd65f46
File content as of revision 6:444a33819962:
#include "Kinematic.h" #include "math.h" #define Kinematic_Debug //MACRO for test system kinematic class #ifndef PI #define PI 3.14159265358979f #endif Kinematic::Kinematic(float Link_Hip, float Link_Knee) :test(SERIAL_TX,SERIAL_RX) { this->Link_Hip = Link_Hip; this->Link_Knee = Link_Knee; } Kinematic::Kinematic(char text, float Link_Hip, float Link_Knee, float Hip, float Knee) :test(SERIAL_TX,SERIAL_RX) { if(text == 'Z' || text == 'z') { this->Link_Hip = Link_Hip; this->Link_Knee = Link_Knee; Zeta_Hip = Hip; Zeta_Knee = Knee; ForwardKinematicCalculation(); #ifdef Kinematic_Debug test.printf("Forward Kinematic\n"); #endif } if(text == 'P' || text == 'p') { this->Link_Hip = Link_Hip; this->Link_Knee = Link_Knee; Position_Y = Hip; Position_Z = Knee; InverseKinematicCalculation(); #ifdef Kinematic_Debug test.printf("Inverse Kinematic\n"); #endif } } void Kinematic::set_Link_Hip(float Link_Hip) { this->Link_Hip = Link_Hip; } void Kinematic::set_Link_Knee(float Link_Knee) { this->Link_Knee = Link_Knee; } void Kinematic::set_Zeta_Hip(float Zeta_Hip) { this->Zeta_Hip = Zeta_Hip; } void Kinematic::set_Zeta_Knee(float Zeta_Knee) { this->Zeta_Knee = Zeta_Knee; } void Kinematic::set_Position_Y(float Position_Y) { this->Position_Y = Position_Y; } void Kinematic::set_Position_Z(float Position_Z) { this->Position_Z = Position_Z; } void Kinematic::set_zeta_knee_range(float max,float min) { zeta_knee_range_max = max; zeta_knee_range_min = min; } void Kinematic::set_zeta_hip_range(float max,float min) { zeta_hip_range_max = max; zeta_hip_range_min = min; } /* void Kinematic::set_offset_YZ(float offset_y, float offset_z) { this->offset_y = offset_y; this->offset_z = offset_z; } */ /* // function offset position do out class befor in class 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 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 = (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 //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 Kinematic_Debug test.printf("Y = %f, Z = %.2f\n",Position_Y,Position_Z); #endif //test.test.test.test.test.printf("Height in Stabilizer : %f\n",height); } void Kinematic::show_z() { 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); }