InverseLeg
Dependents: SwitchMode Stabilizer SwitchMode
Kinematic.cpp@6:444a33819962, 2016-01-23 (annotated)
- Committer:
- soulx
- Date:
- Sat Jan 23 03:42:13 2016 +0000
- Revision:
- 6:444a33819962
- Parent:
- 5:a2a5ebd65f46
-
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
icyzkungz | 1:d5cf0907f83d | 1 | #include "Kinematic.h" |
icyzkungz | 2:c9c38629be85 | 2 | #include "math.h" |
icyzkungz | 2:c9c38629be85 | 3 | |
soulx | 4:d3bb9ead2a55 | 4 | #define Kinematic_Debug //MACRO for test system kinematic class |
icyzkungz | 3:fd037c89ba5f | 5 | |
soulx | 4:d3bb9ead2a55 | 6 | #ifndef PI |
soulx | 4:d3bb9ead2a55 | 7 | #define PI 3.14159265358979f |
soulx | 4:d3bb9ead2a55 | 8 | #endif |
icyzkungz | 1:d5cf0907f83d | 9 | |
soulx | 4:d3bb9ead2a55 | 10 | |
soulx | 4:d3bb9ead2a55 | 11 | |
soulx | 4:d3bb9ead2a55 | 12 | Kinematic::Kinematic(float Link_Hip, float Link_Knee) :test(SERIAL_TX,SERIAL_RX) |
icyzkungz | 1:d5cf0907f83d | 13 | { |
icyzkungz | 1:d5cf0907f83d | 14 | this->Link_Hip = Link_Hip; |
icyzkungz | 1:d5cf0907f83d | 15 | this->Link_Knee = Link_Knee; |
icyzkungz | 1:d5cf0907f83d | 16 | } |
icyzkungz | 1:d5cf0907f83d | 17 | |
soulx | 4:d3bb9ead2a55 | 18 | Kinematic::Kinematic(char text, float Link_Hip, float Link_Knee, float Hip, float Knee) :test(SERIAL_TX,SERIAL_RX) |
icyzkungz | 1:d5cf0907f83d | 19 | { |
icyzkungz | 3:fd037c89ba5f | 20 | if(text == 'Z' || text == 'z') { |
icyzkungz | 2:c9c38629be85 | 21 | this->Link_Hip = Link_Hip; |
icyzkungz | 2:c9c38629be85 | 22 | this->Link_Knee = Link_Knee; |
icyzkungz | 3:fd037c89ba5f | 23 | Zeta_Hip = Hip; |
icyzkungz | 3:fd037c89ba5f | 24 | Zeta_Knee = Knee; |
icyzkungz | 2:c9c38629be85 | 25 | ForwardKinematicCalculation(); |
icyzkungz | 3:fd037c89ba5f | 26 | #ifdef Kinematic_Debug |
icyzkungz | 3:fd037c89ba5f | 27 | test.printf("Forward Kinematic\n"); |
icyzkungz | 3:fd037c89ba5f | 28 | #endif |
icyzkungz | 2:c9c38629be85 | 29 | } |
icyzkungz | 3:fd037c89ba5f | 30 | |
icyzkungz | 3:fd037c89ba5f | 31 | if(text == 'P' || text == 'p') { |
icyzkungz | 2:c9c38629be85 | 32 | this->Link_Hip = Link_Hip; |
icyzkungz | 2:c9c38629be85 | 33 | this->Link_Knee = Link_Knee; |
icyzkungz | 3:fd037c89ba5f | 34 | Position_Y = Hip; |
icyzkungz | 3:fd037c89ba5f | 35 | Position_Z = Knee; |
icyzkungz | 2:c9c38629be85 | 36 | InverseKinematicCalculation(); |
icyzkungz | 3:fd037c89ba5f | 37 | #ifdef Kinematic_Debug |
icyzkungz | 3:fd037c89ba5f | 38 | test.printf("Inverse Kinematic\n"); |
icyzkungz | 3:fd037c89ba5f | 39 | #endif |
icyzkungz | 2:c9c38629be85 | 40 | } |
icyzkungz | 1:d5cf0907f83d | 41 | } |
icyzkungz | 1:d5cf0907f83d | 42 | |
icyzkungz | 1:d5cf0907f83d | 43 | void Kinematic::set_Link_Hip(float Link_Hip) |
icyzkungz | 1:d5cf0907f83d | 44 | { |
icyzkungz | 1:d5cf0907f83d | 45 | this->Link_Hip = Link_Hip; |
icyzkungz | 1:d5cf0907f83d | 46 | } |
icyzkungz | 1:d5cf0907f83d | 47 | |
icyzkungz | 1:d5cf0907f83d | 48 | void Kinematic::set_Link_Knee(float Link_Knee) |
icyzkungz | 1:d5cf0907f83d | 49 | { |
icyzkungz | 1:d5cf0907f83d | 50 | this->Link_Knee = Link_Knee; |
icyzkungz | 1:d5cf0907f83d | 51 | } |
icyzkungz | 1:d5cf0907f83d | 52 | |
icyzkungz | 1:d5cf0907f83d | 53 | void Kinematic::set_Zeta_Hip(float Zeta_Hip) |
icyzkungz | 1:d5cf0907f83d | 54 | { |
icyzkungz | 1:d5cf0907f83d | 55 | this->Zeta_Hip = Zeta_Hip; |
icyzkungz | 1:d5cf0907f83d | 56 | } |
icyzkungz | 1:d5cf0907f83d | 57 | |
icyzkungz | 1:d5cf0907f83d | 58 | void Kinematic::set_Zeta_Knee(float Zeta_Knee) |
icyzkungz | 1:d5cf0907f83d | 59 | { |
icyzkungz | 1:d5cf0907f83d | 60 | this->Zeta_Knee = Zeta_Knee; |
icyzkungz | 1:d5cf0907f83d | 61 | } |
icyzkungz | 1:d5cf0907f83d | 62 | |
icyzkungz | 1:d5cf0907f83d | 63 | void Kinematic::set_Position_Y(float Position_Y) |
icyzkungz | 1:d5cf0907f83d | 64 | { |
icyzkungz | 1:d5cf0907f83d | 65 | this->Position_Y = Position_Y; |
icyzkungz | 1:d5cf0907f83d | 66 | } |
icyzkungz | 1:d5cf0907f83d | 67 | |
icyzkungz | 1:d5cf0907f83d | 68 | void Kinematic::set_Position_Z(float Position_Z) |
icyzkungz | 1:d5cf0907f83d | 69 | { |
icyzkungz | 1:d5cf0907f83d | 70 | this->Position_Z = Position_Z; |
icyzkungz | 1:d5cf0907f83d | 71 | } |
icyzkungz | 1:d5cf0907f83d | 72 | |
soulx | 4:d3bb9ead2a55 | 73 | void Kinematic::set_zeta_knee_range(float max,float min) |
icyzkungz | 3:fd037c89ba5f | 74 | { |
soulx | 4:d3bb9ead2a55 | 75 | zeta_knee_range_max = max; |
soulx | 4:d3bb9ead2a55 | 76 | zeta_knee_range_min = min; |
icyzkungz | 3:fd037c89ba5f | 77 | } |
icyzkungz | 3:fd037c89ba5f | 78 | |
soulx | 4:d3bb9ead2a55 | 79 | void Kinematic::set_zeta_hip_range(float max,float min) |
icyzkungz | 3:fd037c89ba5f | 80 | { |
soulx | 4:d3bb9ead2a55 | 81 | zeta_hip_range_max = max; |
soulx | 4:d3bb9ead2a55 | 82 | zeta_hip_range_min = min; |
icyzkungz | 3:fd037c89ba5f | 83 | } |
icyzkungz | 3:fd037c89ba5f | 84 | |
soulx | 4:d3bb9ead2a55 | 85 | /* |
icyzkungz | 3:fd037c89ba5f | 86 | void Kinematic::set_offset_YZ(float offset_y, float offset_z) |
icyzkungz | 3:fd037c89ba5f | 87 | { |
icyzkungz | 3:fd037c89ba5f | 88 | this->offset_y = offset_y; |
icyzkungz | 3:fd037c89ba5f | 89 | this->offset_z = offset_z; |
icyzkungz | 3:fd037c89ba5f | 90 | } |
soulx | 4:d3bb9ead2a55 | 91 | */ |
icyzkungz | 3:fd037c89ba5f | 92 | |
soulx | 4:d3bb9ead2a55 | 93 | /* |
soulx | 4:d3bb9ead2a55 | 94 | // function offset position do out class befor in class |
icyzkungz | 3:fd037c89ba5f | 95 | void Kinematic::SumPositionWithOffset() |
icyzkungz | 3:fd037c89ba5f | 96 | { |
icyzkungz | 3:fd037c89ba5f | 97 | Position_Y += offset_y; |
icyzkungz | 3:fd037c89ba5f | 98 | Position_Z += offset_z; |
icyzkungz | 3:fd037c89ba5f | 99 | } |
soulx | 4:d3bb9ead2a55 | 100 | */ |
icyzkungz | 3:fd037c89ba5f | 101 | |
icyzkungz | 1:d5cf0907f83d | 102 | void Kinematic::ForwardKinematicCalculation() |
icyzkungz | 1:d5cf0907f83d | 103 | { |
icyzkungz | 2:c9c38629be85 | 104 | Position_Y = Link_Hip*sin(Link_Knee)+Link_Knee*sin(Link_Knee+Zeta_Knee); //zeta1-zeta2 |
icyzkungz | 2:c9c38629be85 | 105 | Position_Z = Link_Hip*cos(Link_Knee)+Link_Knee*cos(Link_Knee+Zeta_Knee); |
icyzkungz | 1:d5cf0907f83d | 106 | } |
icyzkungz | 1:d5cf0907f83d | 107 | |
icyzkungz | 1:d5cf0907f83d | 108 | void Kinematic::InverseKinematicCalculation() |
icyzkungz | 1:d5cf0907f83d | 109 | { |
icyzkungz | 1:d5cf0907f83d | 110 | // Begin by working out L |
icyzkungz | 1:d5cf0907f83d | 111 | float L; |
icyzkungz | 2:c9c38629be85 | 112 | L = (Position_Y*Position_Y) + (Position_Z*Position_Z); |
icyzkungz | 1:d5cf0907f83d | 113 | L = sqrt(L); |
icyzkungz | 3:fd037c89ba5f | 114 | #ifdef Kinematic_Debug |
icyzkungz | 3:fd037c89ba5f | 115 | test.printf("L : %f\n",L); |
icyzkungz | 3:fd037c89ba5f | 116 | #endif |
icyzkungz | 1:d5cf0907f83d | 117 | |
icyzkungz | 1:d5cf0907f83d | 118 | // Work out the Knee angle |
icyzkungz | 3:fd037c89ba5f | 119 | //test.printf("Link Hip : %f, Link Knee : %f\n",Link_Hip,Link_Knee); |
icyzkungz | 3:fd037c89ba5f | 120 | float Knee; |
icyzkungz | 3:fd037c89ba5f | 121 | Knee = pow(Link_Hip,2)+pow(Link_Knee,2)-pow(L,2); |
icyzkungz | 1:d5cf0907f83d | 122 | Knee = Knee / (2*Link_Hip*Link_Knee); |
icyzkungz | 1:d5cf0907f83d | 123 | Knee = acos(Knee); |
icyzkungz | 1:d5cf0907f83d | 124 | Knee = Knee * 180 / PI; |
icyzkungz | 1:d5cf0907f83d | 125 | Zeta_Knee = Knee; |
icyzkungz | 3:fd037c89ba5f | 126 | #ifdef Kinematic_Debug |
icyzkungz | 3:fd037c89ba5f | 127 | test.printf("Zeta Knee : %f\n",Zeta_Knee); |
icyzkungz | 3:fd037c89ba5f | 128 | #endif |
icyzkungz | 1:d5cf0907f83d | 129 | |
icyzkungz | 1:d5cf0907f83d | 130 | // Work out Alpha |
icyzkungz | 1:d5cf0907f83d | 131 | float Alpha = (Link_Hip*Link_Hip)+(L*L)-(Link_Knee*Link_Knee); |
icyzkungz | 1:d5cf0907f83d | 132 | Alpha = Alpha / (2*Link_Hip*L); |
icyzkungz | 1:d5cf0907f83d | 133 | Alpha = acos(Alpha); |
icyzkungz | 3:fd037c89ba5f | 134 | #ifdef Kinematic_Debug |
icyzkungz | 3:fd037c89ba5f | 135 | test.printf("Alpha : %f\n",Alpha); |
icyzkungz | 3:fd037c89ba5f | 136 | #endif |
icyzkungz | 1:d5cf0907f83d | 137 | |
icyzkungz | 1:d5cf0907f83d | 138 | |
icyzkungz | 1:d5cf0907f83d | 139 | // Work out Beta |
icyzkungz | 2:c9c38629be85 | 140 | float Beta = (float) Position_Y/(float) Position_Z; |
icyzkungz | 1:d5cf0907f83d | 141 | Beta = atan(Beta); |
icyzkungz | 3:fd037c89ba5f | 142 | #ifdef Kinematic_Debug |
icyzkungz | 3:fd037c89ba5f | 143 | test.printf("Beta : %f\n",Beta); |
icyzkungz | 3:fd037c89ba5f | 144 | #endif |
icyzkungz | 1:d5cf0907f83d | 145 | |
icyzkungz | 1:d5cf0907f83d | 146 | // FinallZ work out the Hip angle |
icyzkungz | 1:d5cf0907f83d | 147 | float Hip = PI - Alpha - Beta; |
icyzkungz | 1:d5cf0907f83d | 148 | Hip = Hip * 180 / PI; |
icyzkungz | 1:d5cf0907f83d | 149 | Zeta_Hip = Hip; |
icyzkungz | 3:fd037c89ba5f | 150 | #ifdef Kinematic_Debug |
icyzkungz | 3:fd037c89ba5f | 151 | test.printf("Zeta Hip : %f\n",Zeta_Hip); |
icyzkungz | 3:fd037c89ba5f | 152 | #endif |
icyzkungz | 2:c9c38629be85 | 153 | |
icyzkungz | 3:fd037c89ba5f | 154 | #ifdef Kinematic_Debug |
icyzkungz | 3:fd037c89ba5f | 155 | test.printf("Y = %f, Z = %.2f\n",Position_Y,Position_Z); |
icyzkungz | 2:c9c38629be85 | 156 | #endif |
icyzkungz | 3:fd037c89ba5f | 157 | //test.test.test.test.test.printf("Height in Stabilizer : %f\n",height); |
icyzkungz | 3:fd037c89ba5f | 158 | } |
icyzkungz | 3:fd037c89ba5f | 159 | |
soulx | 5:a2a5ebd65f46 | 160 | void Kinematic::show_z() |
icyzkungz | 3:fd037c89ba5f | 161 | { |
icyzkungz | 3:fd037c89ba5f | 162 | if(!(Zeta_Hip > zeta_hip_range_max || Zeta_Hip < zeta_hip_range_min)) test.printf("Z-Hip : Out of Range, "); |
icyzkungz | 3:fd037c89ba5f | 163 | else test.printf("Z-Hip : %f, ",Zeta_Hip); |
icyzkungz | 3:fd037c89ba5f | 164 | if(!(Zeta_Knee > zeta_knee_range_max || Zeta_Knee < zeta_knee_range_min)) test.printf("Z-Knee : Out of Range, "); |
icyzkungz | 3:fd037c89ba5f | 165 | else test.printf("Z-Knee : %f, ",Zeta_Knee); |
icyzkungz | 3:fd037c89ba5f | 166 | test.printf("Y : %f, Z : %f\n",Position_Y,Position_Z); |
icyzkungz | 1:d5cf0907f83d | 167 | } |