yezhong yezhong / panzhan_main_controller_continuous_LX

11

Dependencies:   mbed-dev-f303

CONTROL/CALCULATE/calculate.cpp@2:9418258519ea, 2021-08-06 (annotated)

Committer:
yezhong
Date:
Fri Aug 06 09:03:02 2021 +0000
Revision:
2:9418258519ea
Parent:
0:dd5d4837292c 
111

Who changed what in which revision?

UserRevisionLine numberNew contents of line
panzhan 0:dd5d4837292c 1 #include "calculate.h"
panzhan 0:dd5d4837292c 2
panzhan 0:dd5d4837292c 3
panzhan 0:dd5d4837292c 4 ///////////计时函数////////////////
panzhan 0:dd5d4837292c 5 float timeRecord()
panzhan 0:dd5d4837292c 6 {
panzhan 0:dd5d4837292c 7 float s =tim.read();
panzhan 0:dd5d4837292c 8 if(mode_flag == 8) {
panzhan 0:dd5d4837292c 9 if(s >= 2.0f) {
panzhan 0:dd5d4837292c 10 tim.reset();
panzhan 0:dd5d4837292c 11 }
panzhan 0:dd5d4837292c 12 } else if(mode_flag==1){
panzhan 0:dd5d4837292c 13 if(s >= 4.0f) {
panzhan 0:dd5d4837292c 14 tim.reset();
panzhan 0:dd5d4837292c 15 wait_ms(20);
panzhan 0:dd5d4837292c 16 mode_flag = 2;
panzhan 0:dd5d4837292c 17 send_enable = 0;
panzhan 0:dd5d4837292c 18 }
panzhan 0:dd5d4837292c 19 }else if(mode_flag==2){
panzhan 0:dd5d4837292c 20 if(s >= 4.0f) {
panzhan 0:dd5d4837292c 21 tim.reset();
panzhan 0:dd5d4837292c 22 wait_ms(20);
panzhan 0:dd5d4837292c 23 mode_flag = 1;
panzhan 0:dd5d4837292c 24 send_enable = 0;
panzhan 0:dd5d4837292c 25 }
panzhan 0:dd5d4837292c 26 }else{
panzhan 0:dd5d4837292c 27 if(s >= 3.0f) {
panzhan 0:dd5d4837292c 28 tim.reset();
panzhan 0:dd5d4837292c 29 }
panzhan 0:dd5d4837292c 30 }
panzhan 0:dd5d4837292c 31 return s;
panzhan 0:dd5d4837292c 32 }
panzhan 0:dd5d4837292c 33
panzhan 0:dd5d4837292c 34
panzhan 0:dd5d4837292c 35
panzhan 0:dd5d4837292c 36 unsigned int send_enable = 0;
panzhan 0:dd5d4837292c 37 float pos_ankle = 0.0f;
panzhan 0:dd5d4837292c 38 float pos_knee = 0.0f;
panzhan 0:dd5d4837292c 39
panzhan 0:dd5d4837292c 40
panzhan 0:dd5d4837292c 41 float position_knee(float res)
panzhan 0:dd5d4837292c 42 {
panzhan 0:dd5d4837292c 43 float x = res*100/3.0f; //可能不太合适
panzhan 0:dd5d4837292c 44 float a = 0.0f;
panzhan 0:dd5d4837292c 45
panzhan 0:dd5d4837292c 46 if(mode_flag==1) {
panzhan 0:dd5d4837292c 47 //坐
panzhan 0:dd5d4837292c 48 x = res*100/4.0f;
panzhan 0:dd5d4837292c 49 a = 0.8308f - 0.7549f*cos(0.04647f*x) - 0.555f*sin(0.04647f*x) \
panzhan 0:dd5d4837292c 50 - 0.01085f*cos(2*0.04647f*x) - 0.06257f*sin(2*0.04647f*x) \
panzhan 0:dd5d4837292c 51 - 0.0555f*cos(3*0.04647f*x) + 0.1742f*sin(3*0.04647f*x) \
panzhan 0:dd5d4837292c 52 - 0.0227f*cos(4*0.04647f*x) + 0.01162f*sin(4*0.04647f*x) \
panzhan 0:dd5d4837292c 53 + 0.0292f*cos(5*0.04647f*x) - 0.0004315f*sin(5*0.04647f*x);
panzhan 0:dd5d4837292c 54 } else if(mode_flag==2) {
panzhan 0:dd5d4837292c 55 //起
panzhan 0:dd5d4837292c 56 x = res*100/4.0f;
panzhan 0:dd5d4837292c 57 a = 0.8303f + 0.5947f*cos(0.04688f*x) + 0.7276f*sin(0.04688f*x) \
panzhan 0:dd5d4837292c 58 + 0.007792f*cos(2*0.04688f*x) - 0.0688f*sin(2*0.04688f*x) \
panzhan 0:dd5d4837292c 59 + 0.1629f*cos(3*0.04688f*x) - 0.0822f*sin(3*0.04688f*x) \
panzhan 0:dd5d4837292c 60 - 0.02656f*cos(4*0.04688f*x) - 0.008309f*sin(4*0.04688f*x) \
panzhan 0:dd5d4837292c 61 - 0.007829f*cos(5*0.04688f*x) - 0.02764f*sin(5*0.04688f*x);
panzhan 0:dd5d4837292c 62 } else if(mode_flag==3) {
panzhan 0:dd5d4837292c 63 //平地
panzhan 0:dd5d4837292c 64 a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
panzhan 0:dd5d4837292c 65 - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f) \
panzhan 0:dd5d4837292c 66 - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
panzhan 0:dd5d4837292c 67 } else if(mode_flag==4) {
panzhan 0:dd5d4837292c 68 //上坡
panzhan 0:dd5d4837292c 69 a = 3.099f + 1.654f*cos(0.03788f*x) - 4.737f*sin(0.03788f*x) \
panzhan 0:dd5d4837292c 70 - 2.759f*cos(2*0.03788f*x) - 2.703f*sin(2*0.03788f*x) \
panzhan 0:dd5d4837292c 71 - 2.541f*cos(3*0.03788f*x) + 1.4f*sin(3*0.03788f*x) \
panzhan 0:dd5d4837292c 72 + 0.1508f*cos(4*0.03788f*x) + 1.524f*sin(4*0.03788f*x)\
panzhan 0:dd5d4837292c 73 + 0.6595f*cos(5*0.03788f*x) + 0.2162f*sin(5*0.03788f*x)\
panzhan 0:dd5d4837292c 74 + 0.1359f*cos(6*0.03788f*x) - 0.1806f*sin(6*0.03788f*x)\
panzhan 0:dd5d4837292c 75 - 0.02243f*cos(7*0.03788f*x) - 0.04809f*sin(7*0.03788f*x);
panzhan 0:dd5d4837292c 76 } else if(mode_flag==5) {
panzhan 0:dd5d4837292c 77 //下坡
panzhan 0:dd5d4837292c 78 a = 0.6109f + -0.2748f*cos(0.06062f*x) - 0.2946f*sin(0.06062f*x) \
panzhan 0:dd5d4837292c 79 - 0.2302f*cos(2*0.06062f*x) + 0.1319f*sin(2*0.06062f*x) \
panzhan 0:dd5d4837292c 80 + 0.02774f*cos(3*0.06062f*x) + 0.04863f*sin(3*0.06062f*x) \
panzhan 0:dd5d4837292c 81 + 0.01865f*cos(4*0.06062f*x) - 0.01827f*sin(4*0.06062f*x) \
panzhan 0:dd5d4837292c 82 + 0.009155f*cos(5*0.06062f*x) - 0.008681f*sin(5*0.06062f*x) \
panzhan 0:dd5d4837292c 83 + 0.008983f*cos(6*0.06062f*x) - 0.01089f*sin(6*0.06062f*x) \
panzhan 0:dd5d4837292c 84 - 0.00243f*cos(7*0.06062f*x) - 0.004621f*sin(7*0.06062f*x);
panzhan 0:dd5d4837292c 85
panzhan 0:dd5d4837292c 86 } else if(mode_flag==6) {
panzhan 0:dd5d4837292c 87 //上楼
panzhan 0:dd5d4837292c 88 a = 0.6949f + 0.2931f*cos(0.064f*x) - 0.4634f*sin(0.064f*x) \
panzhan 0:dd5d4837292c 89 - 0.1563f*cos(2*0.064f*x) + 0.01809f*sin(2*0.064f*x) \
panzhan 0:dd5d4837292c 90 + 0.01518f*cos(3*0.064f*x) + 0.02992f*sin(3*0.064f*x);
panzhan 0:dd5d4837292c 91
panzhan 0:dd5d4837292c 92 } else if(mode_flag==7) {
panzhan 0:dd5d4837292c 93 //下楼
panzhan 0:dd5d4837292c 94 a = 0.5695f - 0.2778f*cos(x*0.0614f) - 0.3586f*sin(x*0.0614f) \
panzhan 0:dd5d4837292c 95 - 0.2202f*cos(2*x*0.0614f) + 0.2438f*sin(2*x*0.0614f) \
panzhan 0:dd5d4837292c 96 + 0.05218f*cos(3*x*0.0614f) + 0.03495f*sin(3*x*0.0614f) \
panzhan 0:dd5d4837292c 97 - 0.00463f*cos(4*x*0.0614f) - 0.05175f*sin(4*x*0.0614f);
panzhan 0:dd5d4837292c 98 } else if(mode_flag==8) {
panzhan 0:dd5d4837292c 99 x = res*100/2.0f;
panzhan 0:dd5d4837292c 100 a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
panzhan 0:dd5d4837292c 101 - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f) \
panzhan 0:dd5d4837292c 102 - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
panzhan 0:dd5d4837292c 103 }
panzhan 0:dd5d4837292c 104
panzhan 0:dd5d4837292c 105 return a;
panzhan 0:dd5d4837292c 106 }
panzhan 0:dd5d4837292c 107
panzhan 0:dd5d4837292c 108 float position_ankle(float res)
panzhan 0:dd5d4837292c 109 {
panzhan 0:dd5d4837292c 110 float x = res*100/3.0f; //可能不太合适
panzhan 0:dd5d4837292c 111 float a = 0.0f;
panzhan 0:dd5d4837292c 112 if(mode_flag == 1) {
panzhan 0:dd5d4837292c 113 //坐
panzhan 0:dd5d4837292c 114 a = 0.0f;
panzhan 0:dd5d4837292c 115 } else if(mode_flag == 2) {
panzhan 0:dd5d4837292c 116 //起
panzhan 0:dd5d4837292c 117 a = 0.0f;
panzhan 0:dd5d4837292c 118 } else if(mode_flag == 3) {
panzhan 0:dd5d4837292c 119 //平地
panzhan 0:dd5d4837292c 120 if( x <= 72.54f){
panzhan 0:dd5d4837292c 121 a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
panzhan 0:dd5d4837292c 122 + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
panzhan 0:dd5d4837292c 123 + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
panzhan 0:dd5d4837292c 124 + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
panzhan 0:dd5d4837292c 125 - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
panzhan 0:dd5d4837292c 126 - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
panzhan 0:dd5d4837292c 127 - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
panzhan 0:dd5d4837292c 128 }else{
panzhan 0:dd5d4837292c 129 a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
panzhan 0:dd5d4837292c 130 + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
panzhan 0:dd5d4837292c 131 + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
panzhan 0:dd5d4837292c 132 }
panzhan 0:dd5d4837292c 133
panzhan 0:dd5d4837292c 134
panzhan 0:dd5d4837292c 135 // a = 0.07088f - 0.02461f*cos(x*0.06048f) + 0.1543f *sin(x*0.06048f)\
panzhan 0:dd5d4837292c 136 // + 0.05244f*cos(2*x*0.06048f) - 0.09977f*sin(2*x*0.06048f) \
panzhan 0:dd5d4837292c 137 // - 0.07853f*cos(3*x*0.06048f) - 0.01301f*sin(3*x*0.06048f) \
panzhan 0:dd5d4837292c 138 // + 0.02483f*cos(4*x*0.06048f) + 0.01296f*sin(4*x*0.06048f) \
panzhan 0:dd5d4837292c 139 // + 0.003777f*cos(5*x*0.06048f) - 0.0268f*sin(5*x*0.06048f);
panzhan 0:dd5d4837292c 140 } else if(mode_flag == 4) {
panzhan 0:dd5d4837292c 141 //上坡
panzhan 0:dd5d4837292c 142 a = 0.2081f + 0.1834f*cos(x*0.05377f) + 0.0957f*sin(x*0.05377f) \
panzhan 0:dd5d4837292c 143 - 0.125f*cos(2*x*0.05377f) - 0.1071f*sin(2*x*0.05377f) \
panzhan 0:dd5d4837292c 144 + 0.0755f*cos(3*x*0.05377f) + 0.01942f*sin(3*x*0.05377f) \
panzhan 0:dd5d4837292c 145 - 0.04593f*cos(4*x*0.05377f) - 0.04182f*sin(4*x*0.05377f) \
panzhan 0:dd5d4837292c 146 + 0.02662f*cos(5*x*0.05377f) - 0.005419f*sin(5*x*0.05377f) \
panzhan 0:dd5d4837292c 147 - 0.01759f*cos(6*x*0.05377f) - 0.006702f*sin(6*x*0.05377f) \
panzhan 0:dd5d4837292c 148 + 0.003964f*cos(7*x*0.05377f) - 0.004773f*sin(7*x*0.05377f) \
panzhan 0:dd5d4837292c 149 - 0.007487f*cos(8*x*0.05377f) + 0.0009378f*sin(8*x*0.05377f);
panzhan 0:dd5d4837292c 150
panzhan 0:dd5d4837292c 151 } else if(mode_flag == 5) {
panzhan 0:dd5d4837292c 152 //下坡
panzhan 0:dd5d4837292c 153 a = 0.07331f - 0.1611f*cos(0.06333f*x) - 0.01543f*sin(0.06333f*x) \
panzhan 0:dd5d4837292c 154 + 0.05741f*cos(2*0.06333f*x) - 0.0004983f*sin(2*0.06333f*x) \
panzhan 0:dd5d4837292c 155 + 0.002168f*cos(3*0.06333f*x) - 0.07856f*sin(3*0.06333f*x) \
panzhan 0:dd5d4837292c 156 - 0.01604f*cos(4*0.06333f*x) + 0.0007893f*sin(4*0.06333f*x) \
panzhan 0:dd5d4837292c 157 + 0.02003f*cos(5*0.06333f*x) + 0.005234f*sin(5*0.06333f*x) \
panzhan 0:dd5d4837292c 158 + 0.00913f*cos(6*0.06333f*x) + 0.0001917f*sin(6*0.06333f*x) \
panzhan 0:dd5d4837292c 159 + 0.0007703f*cos(7*0.06333f*x) + 0.002357f*sin(7*0.06333f*x);
panzhan 0:dd5d4837292c 160
panzhan 0:dd5d4837292c 161 } else if(mode_flag == 6) {
panzhan 0:dd5d4837292c 162 //上楼
panzhan 0:dd5d4837292c 163 a = 0.3354f + 0.1043f*cos(0.04022f*x) - 0.2226f*sin(0.04022f*x) \
panzhan 0:dd5d4837292c 164 - 0.01181f*cos(2*0.04022f*x) - 0.1101f*sin(2*0.04022f*x) \
panzhan 0:dd5d4837292c 165 - 0.2017f*cos(3*0.04022f*x) + 0.003176f*sin(3*0.04022f*x) \
panzhan 0:dd5d4837292c 166 - 0.03876f*cos(4*0.04022f*x) + 0.09576f*sin(4*0.04022f*x) \
panzhan 0:dd5d4837292c 167 + 0.04748f*cos(5*0.04022f*x) + 0.09573f*sin(5*0.04022f*x) \
panzhan 0:dd5d4837292c 168 + 0.0475f*cos(6*0.04022f*x) - 0.009034f*sin(6*0.04022f*x) \
panzhan 0:dd5d4837292c 169 + 0.01196f*cos(7*0.04022f*x) - 0.02833f*sin(7*0.04022f*x) \
panzhan 0:dd5d4837292c 170 - 0.007736f*cos(8*0.04022f*x) + 0.001967f*sin(8*0.04022f*x);
panzhan 0:dd5d4837292c 171 } else if(mode_flag == 7) {
panzhan 0:dd5d4837292c 172 //下楼
panzhan 0:dd5d4837292c 173 a = 0.1254f - 0.3401f*cos(x*0.0573f) + 0.1949f*sin(x*0.0573f) \
panzhan 0:dd5d4837292c 174 - 0.01378f*cos(2*x*0.0573f) + 0.0842f*sin(2*x*0.0573f) \
panzhan 0:dd5d4837292c 175 - 0.08164f*cos(3*x*0.0573f) + 0.05474f*sin(3*x*0.0573f) \
panzhan 0:dd5d4837292c 176 + 0.01607f*cos(4*x*0.0573f) - 0.01914f*sin(4*x*0.0573f) \
panzhan 0:dd5d4837292c 177 - 0.04177f*cos(5*x*0.0573f) - 0.01208f*sin(5*x*0.0573f);
panzhan 0:dd5d4837292c 178 } else if(mode_flag == 8) {
panzhan 0:dd5d4837292c 179 //快速
panzhan 0:dd5d4837292c 180 x = res*100/2.0f;
panzhan 0:dd5d4837292c 181 if( x <= 72.54f){
panzhan 0:dd5d4837292c 182 a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
panzhan 0:dd5d4837292c 183 + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
panzhan 0:dd5d4837292c 184 + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
panzhan 0:dd5d4837292c 185 + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
panzhan 0:dd5d4837292c 186 - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
panzhan 0:dd5d4837292c 187 - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
panzhan 0:dd5d4837292c 188 - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
panzhan 0:dd5d4837292c 189 }else{
panzhan 0:dd5d4837292c 190 a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
panzhan 0:dd5d4837292c 191 + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
panzhan 0:dd5d4837292c 192 + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
panzhan 0:dd5d4837292c 193 }
panzhan 0:dd5d4837292c 194 }
panzhan 0:dd5d4837292c 195 return -a;
panzhan 0:dd5d4837292c 196 }
panzhan 0:dd5d4837292c 197
panzhan 0:dd5d4837292c 198 unsigned int init_joint_angles_flag = 0;
panzhan 0:dd5d4837292c 199
panzhan 0:dd5d4837292c 200 void calculate_fuzzy(float ankp, float knp)
panzhan 0:dd5d4837292c 201 {
panzhan 0:dd5d4837292c 202 send_enable = 1; // can可以发送数据
panzhan 0:dd5d4837292c 203
panzhan 0:dd5d4837292c 204 //初始化关节角度
panzhan 0:dd5d4837292c 205 if(init_joint_angles_flag == 0){
panzhan 0:dd5d4837292c 206 init_joint_angles_flag = 1;
panzhan 0:dd5d4837292c 207 init_joint_angles();
panzhan 0:dd5d4837292c 208 }
panzhan 0:dd5d4837292c 209
panzhan 0:dd5d4837292c 210 calculate_ankle_fuzzy(ankp,knp);
panzhan 0:dd5d4837292c 211 }
panzhan 0:dd5d4837292c 212
panzhan 0:dd5d4837292c 213 ////////////////////////踝关节模糊控制//////////////////////////////
panzhan 0:dd5d4837292c 214 // Gait_per_now 当前步态百分比
panzhan 0:dd5d4837292c 215
panzhan 0:dd5d4837292c 216 float ank_e = 0.0f, ank_before_e = 0.0f, ank_ec = 0.0f;
panzhan 0:dd5d4837292c 217 unsigned int fuzzy50 = 1;
panzhan 0:dd5d4837292c 218
panzhan 0:dd5d4837292c 219 void calculate_ankle_fuzzy(float ankp, float knp)
panzhan 0:dd5d4837292c 220 {
panzhan 0:dd5d4837292c 221
panzhan 0:dd5d4837292c 222
panzhan 0:dd5d4837292c 223 float s = timeRecord();
panzhan 0:dd5d4837292c 224 pos_knee = position_knee(s); //用于测试模糊PID参数
panzhan 0:dd5d4837292c 225 pos_ankle = position_ankle(s);
panzhan 0:dd5d4837292c 226
panzhan 0:dd5d4837292c 227 // pc.printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", a_state.knee_state.p, pos_knee, a_state.ankle_state.p, pos_ankle, s);
panzhan 0:dd5d4837292c 228
panzhan 0:dd5d4837292c 229
panzhan 0:dd5d4837292c 230 cal_command.q_des_ankle = pos_ankle;
panzhan 0:dd5d4837292c 231 cal_command.qd_des_ankle = 0;
panzhan 0:dd5d4837292c 232 cal_command.kp_ankle = 280;
panzhan 0:dd5d4837292c 233 cal_command.kd_ankle = 2;
panzhan 0:dd5d4837292c 234 cal_command.tau_ankle_ff = 0;
panzhan 0:dd5d4837292c 235
panzhan 0:dd5d4837292c 236 cal_command.q_des_knee = pos_knee;
panzhan 0:dd5d4837292c 237 cal_command.qd_des_knee = 0;
panzhan 0:dd5d4837292c 238 cal_command.kp_knee = 180;
panzhan 0:dd5d4837292c 239 cal_command.kd_knee = 0;
panzhan 0:dd5d4837292c 240 cal_command.tau_knee_ff = 0;
panzhan 0:dd5d4837292c 241 }
panzhan 0:dd5d4837292c 242
panzhan 0:dd5d4837292c 243
panzhan 0:dd5d4837292c 244 void init_joint_angles(){
panzhan 0:dd5d4837292c 245
panzhan 0:dd5d4837292c 246 a_control.ankle.p_des = position_ankle(0);
panzhan 0:dd5d4837292c 247 a_control.ankle.v_des = 0;
panzhan 0:dd5d4837292c 248 a_control.ankle.kp = 60;
panzhan 0:dd5d4837292c 249 a_control.ankle.kd = 0;
panzhan 0:dd5d4837292c 250 a_control.ankle.t_ff = 0;
panzhan 0:dd5d4837292c 251
panzhan 0:dd5d4837292c 252 a_control.knee.p_des = position_knee(0.01);
panzhan 0:dd5d4837292c 253 a_control.knee.v_des = 0;
panzhan 0:dd5d4837292c 254 a_control.knee.kp = 60;
panzhan 0:dd5d4837292c 255 a_control.knee.kd = 0;
panzhan 0:dd5d4837292c 256 a_control.knee.t_ff = 0;
panzhan 0:dd5d4837292c 257
panzhan 0:dd5d4837292c 258 PackAll();
panzhan 0:dd5d4837292c 259 WriteAll();
panzhan 0:dd5d4837292c 260
panzhan 0:dd5d4837292c 261 wait(2);
panzhan 0:dd5d4837292c 262 }
panzhan 0:dd5d4837292c 263
panzhan 0:dd5d4837292c 264
panzhan 0:dd5d4837292c 265