yezhong yezhong
11
Diff: CONTROL/CALCULATE/calculate.cpp
- Revision:
- 0:dd5d4837292c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/CALCULATE/calculate.cpp Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,265 @@
+#include "calculate.h"
+
+
+///////////计时函数////////////////
+float timeRecord()
+{
+ float s =tim.read();
+ if(mode_flag == 8) {
+ if(s >= 2.0f) {
+ tim.reset();
+ }
+ } else if(mode_flag==1){
+ if(s >= 4.0f) {
+ tim.reset();
+ wait_ms(20);
+ mode_flag = 2;
+ send_enable = 0;
+ }
+ }else if(mode_flag==2){
+ if(s >= 4.0f) {
+ tim.reset();
+ wait_ms(20);
+ mode_flag = 1;
+ send_enable = 0;
+ }
+ }else{
+ if(s >= 3.0f) {
+ tim.reset();
+ }
+ }
+ return s;
+}
+
+
+
+unsigned int send_enable = 0;
+float pos_ankle = 0.0f;
+float pos_knee = 0.0f;
+
+
+float position_knee(float res)
+{
+ float x = res*100/3.0f; //可能不太合适
+ float a = 0.0f;
+
+ if(mode_flag==1) {
+ //坐
+ x = res*100/4.0f;
+ a = 0.8308f - 0.7549f*cos(0.04647f*x) - 0.555f*sin(0.04647f*x) \
+ - 0.01085f*cos(2*0.04647f*x) - 0.06257f*sin(2*0.04647f*x) \
+ - 0.0555f*cos(3*0.04647f*x) + 0.1742f*sin(3*0.04647f*x) \
+ - 0.0227f*cos(4*0.04647f*x) + 0.01162f*sin(4*0.04647f*x) \
+ + 0.0292f*cos(5*0.04647f*x) - 0.0004315f*sin(5*0.04647f*x);
+ } else if(mode_flag==2) {
+ //起
+ x = res*100/4.0f;
+ a = 0.8303f + 0.5947f*cos(0.04688f*x) + 0.7276f*sin(0.04688f*x) \
+ + 0.007792f*cos(2*0.04688f*x) - 0.0688f*sin(2*0.04688f*x) \
+ + 0.1629f*cos(3*0.04688f*x) - 0.0822f*sin(3*0.04688f*x) \
+ - 0.02656f*cos(4*0.04688f*x) - 0.008309f*sin(4*0.04688f*x) \
+ - 0.007829f*cos(5*0.04688f*x) - 0.02764f*sin(5*0.04688f*x);
+ } else if(mode_flag==3) {
+ //平地
+ a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
+ - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f) \
+ - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
+ } else if(mode_flag==4) {
+ //上坡
+ a = 3.099f + 1.654f*cos(0.03788f*x) - 4.737f*sin(0.03788f*x) \
+ - 2.759f*cos(2*0.03788f*x) - 2.703f*sin(2*0.03788f*x) \
+ - 2.541f*cos(3*0.03788f*x) + 1.4f*sin(3*0.03788f*x) \
+ + 0.1508f*cos(4*0.03788f*x) + 1.524f*sin(4*0.03788f*x)\
+ + 0.6595f*cos(5*0.03788f*x) + 0.2162f*sin(5*0.03788f*x)\
+ + 0.1359f*cos(6*0.03788f*x) - 0.1806f*sin(6*0.03788f*x)\
+ - 0.02243f*cos(7*0.03788f*x) - 0.04809f*sin(7*0.03788f*x);
+ } else if(mode_flag==5) {
+ //下坡
+ a = 0.6109f + -0.2748f*cos(0.06062f*x) - 0.2946f*sin(0.06062f*x) \
+ - 0.2302f*cos(2*0.06062f*x) + 0.1319f*sin(2*0.06062f*x) \
+ + 0.02774f*cos(3*0.06062f*x) + 0.04863f*sin(3*0.06062f*x) \
+ + 0.01865f*cos(4*0.06062f*x) - 0.01827f*sin(4*0.06062f*x) \
+ + 0.009155f*cos(5*0.06062f*x) - 0.008681f*sin(5*0.06062f*x) \
+ + 0.008983f*cos(6*0.06062f*x) - 0.01089f*sin(6*0.06062f*x) \
+ - 0.00243f*cos(7*0.06062f*x) - 0.004621f*sin(7*0.06062f*x);
+
+ } else if(mode_flag==6) {
+ //上楼
+ a = 0.6949f + 0.2931f*cos(0.064f*x) - 0.4634f*sin(0.064f*x) \
+ - 0.1563f*cos(2*0.064f*x) + 0.01809f*sin(2*0.064f*x) \
+ + 0.01518f*cos(3*0.064f*x) + 0.02992f*sin(3*0.064f*x);
+
+ } else if(mode_flag==7) {
+ //下楼
+ a = 0.5695f - 0.2778f*cos(x*0.0614f) - 0.3586f*sin(x*0.0614f) \
+ - 0.2202f*cos(2*x*0.0614f) + 0.2438f*sin(2*x*0.0614f) \
+ + 0.05218f*cos(3*x*0.0614f) + 0.03495f*sin(3*x*0.0614f) \
+ - 0.00463f*cos(4*x*0.0614f) - 0.05175f*sin(4*x*0.0614f);
+ } else if(mode_flag==8) {
+ x = res*100/2.0f;
+ a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
+ - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f) \
+ - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
+ }
+
+ return a;
+}
+
+float position_ankle(float res)
+{
+ float x = res*100/3.0f; //可能不太合适
+ float a = 0.0f;
+ if(mode_flag == 1) {
+ //坐
+ a = 0.0f;
+ } else if(mode_flag == 2) {
+ //起
+ a = 0.0f;
+ } else if(mode_flag == 3) {
+ //平地
+ if( x <= 72.54f){
+ a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
+ + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
+ + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
+ + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
+ - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
+ - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
+ - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
+ }else{
+ a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
+ + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
+ + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
+ }
+
+
+// a = 0.07088f - 0.02461f*cos(x*0.06048f) + 0.1543f *sin(x*0.06048f)\
+// + 0.05244f*cos(2*x*0.06048f) - 0.09977f*sin(2*x*0.06048f) \
+// - 0.07853f*cos(3*x*0.06048f) - 0.01301f*sin(3*x*0.06048f) \
+// + 0.02483f*cos(4*x*0.06048f) + 0.01296f*sin(4*x*0.06048f) \
+// + 0.003777f*cos(5*x*0.06048f) - 0.0268f*sin(5*x*0.06048f);
+ } else if(mode_flag == 4) {
+ //上坡
+ a = 0.2081f + 0.1834f*cos(x*0.05377f) + 0.0957f*sin(x*0.05377f) \
+ - 0.125f*cos(2*x*0.05377f) - 0.1071f*sin(2*x*0.05377f) \
+ + 0.0755f*cos(3*x*0.05377f) + 0.01942f*sin(3*x*0.05377f) \
+ - 0.04593f*cos(4*x*0.05377f) - 0.04182f*sin(4*x*0.05377f) \
+ + 0.02662f*cos(5*x*0.05377f) - 0.005419f*sin(5*x*0.05377f) \
+ - 0.01759f*cos(6*x*0.05377f) - 0.006702f*sin(6*x*0.05377f) \
+ + 0.003964f*cos(7*x*0.05377f) - 0.004773f*sin(7*x*0.05377f) \
+ - 0.007487f*cos(8*x*0.05377f) + 0.0009378f*sin(8*x*0.05377f);
+
+ } else if(mode_flag == 5) {
+ //下坡
+ a = 0.07331f - 0.1611f*cos(0.06333f*x) - 0.01543f*sin(0.06333f*x) \
+ + 0.05741f*cos(2*0.06333f*x) - 0.0004983f*sin(2*0.06333f*x) \
+ + 0.002168f*cos(3*0.06333f*x) - 0.07856f*sin(3*0.06333f*x) \
+ - 0.01604f*cos(4*0.06333f*x) + 0.0007893f*sin(4*0.06333f*x) \
+ + 0.02003f*cos(5*0.06333f*x) + 0.005234f*sin(5*0.06333f*x) \
+ + 0.00913f*cos(6*0.06333f*x) + 0.0001917f*sin(6*0.06333f*x) \
+ + 0.0007703f*cos(7*0.06333f*x) + 0.002357f*sin(7*0.06333f*x);
+
+ } else if(mode_flag == 6) {
+ //上楼
+ a = 0.3354f + 0.1043f*cos(0.04022f*x) - 0.2226f*sin(0.04022f*x) \
+ - 0.01181f*cos(2*0.04022f*x) - 0.1101f*sin(2*0.04022f*x) \
+ - 0.2017f*cos(3*0.04022f*x) + 0.003176f*sin(3*0.04022f*x) \
+ - 0.03876f*cos(4*0.04022f*x) + 0.09576f*sin(4*0.04022f*x) \
+ + 0.04748f*cos(5*0.04022f*x) + 0.09573f*sin(5*0.04022f*x) \
+ + 0.0475f*cos(6*0.04022f*x) - 0.009034f*sin(6*0.04022f*x) \
+ + 0.01196f*cos(7*0.04022f*x) - 0.02833f*sin(7*0.04022f*x) \
+ - 0.007736f*cos(8*0.04022f*x) + 0.001967f*sin(8*0.04022f*x);
+ } else if(mode_flag == 7) {
+ //下楼
+ a = 0.1254f - 0.3401f*cos(x*0.0573f) + 0.1949f*sin(x*0.0573f) \
+ - 0.01378f*cos(2*x*0.0573f) + 0.0842f*sin(2*x*0.0573f) \
+ - 0.08164f*cos(3*x*0.0573f) + 0.05474f*sin(3*x*0.0573f) \
+ + 0.01607f*cos(4*x*0.0573f) - 0.01914f*sin(4*x*0.0573f) \
+ - 0.04177f*cos(5*x*0.0573f) - 0.01208f*sin(5*x*0.0573f);
+ } else if(mode_flag == 8) {
+ //快速
+ x = res*100/2.0f;
+ if( x <= 72.54f){
+ a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
+ + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
+ + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
+ + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
+ - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
+ - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
+ - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
+ }else{
+ a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
+ + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
+ + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
+ }
+ }
+ return -a;
+}
+
+unsigned int init_joint_angles_flag = 0;
+
+void calculate_fuzzy(float ankp, float knp)
+{
+ send_enable = 1; // can可以发送数据
+
+ //初始化关节角度
+ if(init_joint_angles_flag == 0){
+ init_joint_angles_flag = 1;
+ init_joint_angles();
+ }
+
+ calculate_ankle_fuzzy(ankp,knp);
+}
+
+////////////////////////踝关节模糊控制//////////////////////////////
+// Gait_per_now 当前步态百分比
+
+float ank_e = 0.0f, ank_before_e = 0.0f, ank_ec = 0.0f;
+unsigned int fuzzy50 = 1;
+
+void calculate_ankle_fuzzy(float ankp, float knp)
+{
+
+
+ float s = timeRecord();
+ pos_knee = position_knee(s); //用于测试模糊PID参数
+ pos_ankle = position_ankle(s);
+
+// 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);
+
+
+ cal_command.q_des_ankle = pos_ankle;
+ cal_command.qd_des_ankle = 0;
+ cal_command.kp_ankle = 280;
+ cal_command.kd_ankle = 2;
+ cal_command.tau_ankle_ff = 0;
+
+ cal_command.q_des_knee = pos_knee;
+ cal_command.qd_des_knee = 0;
+ cal_command.kp_knee = 180;
+ cal_command.kd_knee = 0;
+ cal_command.tau_knee_ff = 0;
+}
+
+
+void init_joint_angles(){
+
+ a_control.ankle.p_des = position_ankle(0);
+ a_control.ankle.v_des = 0;
+ a_control.ankle.kp = 60;
+ a_control.ankle.kd = 0;
+ a_control.ankle.t_ff = 0;
+
+ a_control.knee.p_des = position_knee(0.01);
+ a_control.knee.v_des = 0;
+ a_control.knee.kp = 60;
+ a_control.knee.kd = 0;
+ a_control.knee.t_ff = 0;
+
+ PackAll();
+ WriteAll();
+
+ wait(2);
+}
+
+
+