BE@R lab / InverseLeg

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?

UserRevisionLine numberNew 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 }