Revision 0:dd5d4837292c, committed 2021-06-09

Comitter:

panzhan

Date:

Wed Jun 09 01:41:45 2021 +0000

Commit message:

continuous motion

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,112 @@
+#include "CAN.h"
+#include "used_leg_message.h"
+
+
+
+//定义设备
+CAN knee_can(PB_8, PB_9);                                                         // CAN Rx pin name, CAN Tx pin name
+CAN ankle_can(PB_12, PB_13);                                                       // CAN Rx pin name, CAN Tx pin name
+// 定义CAN消息
+CANMessage knee_rxMsg, ankle_rxMsg;                                                  // 主控收到的CAN消息
+CANMessage knee_txMsg, ankle_txMsg;                                           // 主控发送的CAN消息
+
+//=================================函数=======================================//
+
+/// 将控制参数打包入CAN消息中
+/// CAN Command Packet Structure
+/// 16 bit position command, between -4*pi and 4*pi
+/// 12 bit velocity command, between -30 and + 30 rad/s
+/// 12 bit kp, between 0 and 500 N-m/rad
+/// 12 bit kd, between 0 and 100 N-m*s/rad
+/// 12 bit feed forward torque, between -18 and 18 N-m
+/// CAN Packet is 8 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], kp[11-8]]
+/// 4: [kp[7-0]]
+/// 5: [kd[11-4]]
+/// 6: [kd[3-0], torque[11-8]]
+/// 7: [torque[7-0]]
+void pack_cmd(CANMessage * msg, joint_control joint){
+     
+     /// limit data to be within bounds ///
+     float p_des = fminf(fmaxf(P_MIN, joint.p_des), P_MAX);                    
+     float v_des = fminf(fmaxf(V_MIN, joint.v_des), V_MAX);
+     float kp = fminf(fmaxf(KP_MIN, joint.kp), KP_MAX);
+     float kd = fminf(fmaxf(KD_MIN, joint.kd), KD_MAX);
+     float t_ff = fminf(fmaxf(T_MIN, joint.t_ff), T_MAX);
+     /// convert floats to unsigned ints ///
+     uint16_t p_int = float_to_uint(p_des, P_MIN, P_MAX, 16);            
+     uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 12);
+     uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 12);
+     uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 12);
+     uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 12);
+     /// pack ints into the can buffer ///
+     msg->data[0] = p_int>>8;                                       
+     msg->data[1] = p_int&0xFF;
+     msg->data[2] = v_int>>4;
+     msg->data[3] = ((v_int&0xF)<<4)|(kp_int>>8);
+     msg->data[4] = kp_int&0xFF;
+     msg->data[5] = kd_int>>4;
+     msg->data[6] = ((kd_int&0xF)<<4)|(t_int>>8);
+     msg->data[7] = t_int&0xff;
+}
+
+
+// 多个控制器联合打包准备发送
+void PackAll(){
+    pack_cmd(&knee_txMsg, a_control.knee); 
+    pack_cmd(&ankle_txMsg, a_control.ankle); 
+}
+
+
+// 写联合打包的数据
+void WriteAll(){
+    knee_can.write(knee_txMsg);
+    wait(.00002);
+    ankle_can.write(ankle_txMsg);
+    wait(.00002);
+}
+
+
+/// 提取信息并存入状态结构体
+/// CAN Reply Packet Structure
+/// 16 bit position, between -4*pi and 4*pi
+/// 12 bit velocity, between -30 and + 30 rad/s
+/// 12 bit current, between -40 and 40;
+/// CAN Packet is 5 8-bit words
+/// Formatted as follows.  For each quantity, bit 0 is LSB
+/// 0: [position[15-8]]
+/// 1: [position[7-0]] 
+/// 2: [velocity[11-4]]
+/// 3: [velocity[3-0], current[11-8]]
+/// 4: [current[7-0]]
+void unpack_reply(CANMessage msg, leg_state * state){
+    /// unpack ints from can buffer ///
+    uint16_t id = msg.data[0];
+    uint16_t p_int = (msg.data[1]<<8)|msg.data[2];
+    uint16_t v_int = (msg.data[3]<<4)|(msg.data[4]>>4);
+    uint16_t i_int = ((msg.data[4]&0xF)<<8)|msg.data[5];
+    /// convert uints to floats ///
+    float p = uint_to_float(p_int, P_MIN, P_MAX, 16);
+    float v = uint_to_float(v_int, V_MIN, V_MAX, 12);
+    float t = uint_to_float(i_int, -T_MAX, T_MAX, 12);
+    
+    if(id==0x02){
+        state->knee_state.p = p;
+        state->knee_state.v = v;
+        state->knee_state.t = t;
+        }
+    else if(id==0x01){
+        state->ankle_state.p = p;
+        state->ankle_state.v = v;
+        state->ankle_state.t = t;
+        }
+} 
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,39 @@
+#ifndef _CAN_H
+#define _CAN_H
+
+// 头文件
+#include "mbed.h"
+#include "math_ops.h"
+#include "leg_message.h"
+#include "used_leg_message.h"
+
+
+// 宏定义
+#define CAN_ID  0x01                                                            // Master CAN ID
+
+#define P_MIN   -12.5f                                                          // Value Limits
+#define P_MAX   12.5f
+#define V_MIN   -45.0f
+#define V_MAX   45.0f
+#define KP_MIN  0.0f
+#define KP_MAX  500.0f
+#define KD_MIN  0.0f
+#define KD_MAX  5.0f
+#define T_MIN   -18.0f
+#define T_MAX   18.0f                                                           // Value Limits
+
+
+// 对象外部申明
+extern CAN knee_can,ankle_can;                 // 设备的外部申明
+extern CANMessage knee_rxMsg, ankle_rxMsg;     // 主控收到的CAN消息
+extern CANMessage knee_txMsg, ankle_txMsg;     // 主控发送的CAN消息
+
+// 函数外部申明
+void pack_cmd(CANMessage * msg, joint_control joint);
+void PackAll();
+void WriteAll();
+void unpack_reply(CANMessage msg, leg_state * state);
+
+
+
+#endif
\ No newline at end of file

--- /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);
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/CALCULATE/calculate.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,29 @@
+#ifndef _CALCULATE_H
+#define _CALCULATE_H
+
+#include "mbed.h"
+#include "data_pc.h"
+#include "data_board.h"
+#include "fuzzy.h"
+#include "timer.h"
+#include "used_leg_message.h"
+
+
+
+#define CT (10)
+
+extern unsigned int send_enable;
+extern float pos_ankle;
+extern float pos_knee;
+
+
+
+float position_knee(float res);
+float position_ankle(float res);
+void calculate_ankle_fuzzy(float ankp, float knp);
+void calculate_fuzzy(float ankp, float knp);
+void init_joint_angles();
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,43 @@
+#include "control.h"
+
+
+void control(float ankp, float knp)
+{
+    if(command_control_flag == 1)
+        command_control();
+            
+    
+    if(c_lock == 0)
+    {
+        calculate_fuzzy(ankp,knp);
+    }
+    
+    // 将计算得到的数值赋给控制器
+    
+    a_control.ankle.p_des = cal_command.q_des_ankle;
+    a_control.ankle.v_des = cal_command.qd_des_ankle;
+    a_control.ankle.kp    = cal_command.kp_ankle;
+    a_control.ankle.kd    = cal_command.kd_ankle;
+    a_control.ankle.t_ff  = cal_command.tau_ankle_ff;
+        
+    a_control.knee.p_des = cal_command.q_des_knee;
+    a_control.knee.v_des = cal_command.qd_des_knee;
+    a_control.knee.kp    = cal_command.kp_knee;
+    a_control.knee.kd    = cal_command.kd_knee;
+    a_control.knee.t_ff  = cal_command.tau_knee_ff;
+    
+}
+  
+    
+
+
+
+
+
+
+      
+        
+        
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,20 @@
+#ifndef _CONTROL_H
+#define _CONTROL_H
+
+
+#include "mbed.h"
+#include "CAN.h"
+#include "mode.h"
+#include "data_command.h"
+#include "data_board.h"
+#include "used_leg_message.h"
+#include "calculate.h"
+
+
+
+void control(float ankp, float knp);
+
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_BOARD/data_board.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,109 @@
+#include "data_board.h"
+
+
+
+
+Serial foot(PA_9, PA_10);              // tx, rx U1
+
+unsigned int flag_foot_A = 0, flag_foot_B = 0;
+unsigned int Sfootnum = 0;
+
+uint16_t Sfootget[10] = {0};                                                    
+uint16_t Sfootuse[10] = {0};
+
+////////////////////////////////////////////////////////////////////////////////
+//   Gait_now           当前相位          Gait_per_now  当前步态时刻      Gait_change 步态改变标志位    
+//   Gait_num_valid     步数统计          time_portion_now    当前相位时间段
+////////////////////////////////////////////////////////////////////////////////
+
+unsigned int Gait_num_valid = 0, Gait_now = 0, Gait_per_now = 0, Gait_cycle_now = 0, time_portion_now=0;
+uint16_t Gait_per_now_int = 0, COP_Y_int = 0, COP_X_int = 0, Gait_cycle_now_int = 0, time_portion_now_int=0;
+
+
+unsigned int Gait_num_valid_0before = 0, Gait_now_0before = 0;
+float Gait_per_now_0before = 0.0f, COP_Y_0before = 0.0f, COP_X_0before = 0.0f, Gait_cycle_now_0before = 0.0f;
+
+unsigned int Gait_num_valid_real = 0, Gait_now_real = 0, Gait_per_now_real = 0, Gait_cycle_now_real = 0.0f, time_portion_now_real=0.0f;
+
+
+void gait_decode()
+{
+    Gait_now = Sfootuse[0];
+    Gait_per_now = Sfootuse[1]*100 + Sfootuse[2]*10 + Sfootuse[3];
+    time_portion_now = Sfootuse[4]*100000 + Sfootuse[5]*10000 + Sfootuse[6]*1000 + Sfootuse[7]*100 + Sfootuse[8]*10 + Sfootuse[9];
+              
+        
+    Gait_num_valid_real = Gait_num_valid;
+    Gait_now_real = Gait_now;
+    Gait_per_now_real = Gait_per_now;
+    Gait_cycle_now_real = Gait_cycle_now;
+    time_portion_now_real=time_portion_now;
+    
+    command.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
+}
+
+void gait_clear()
+{
+    for(int i = 0; i < 10; i++){
+        Sfootuse[i] = 0;
+    }
+}
+
+
+void serial_board_isr(void)
+{
+//    pc.printf("begin\n");
+    while(foot.readable())
+    {       
+        uint16_t c = foot.getc();
+        if(c == 'A')
+        {
+            flag_foot_A = 1;
+            
+            flag_foot_B = 0;
+            Sfootnum = 0;
+            for(unsigned int i = 0; i < 10; i++)
+            {
+                Sfootget[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 'B')
+        {
+            flag_foot_B = 1;
+        }
+        
+        if(flag_foot_A == 1)
+        {
+            if((flag_foot_B != 1) && (Sfootnum < 10))
+            {
+                Sfootget[Sfootnum] = c;    
+            }
+            
+            Sfootnum++;
+            
+            if((flag_foot_B == 1) && (Sfootnum != 11))
+            {    
+                flag_foot_A = 0;
+                flag_foot_B = 0;
+                Sfootnum    = 0;     
+            }
+            
+            if((flag_foot_B == 1) && (Sfootnum == 11))
+            {
+                flag_foot_A = 0;
+                flag_foot_B = 0;
+                Sfootnum    = 0;
+                
+                for(unsigned int i = 0; i < 10; i++)
+                {
+                    Sfootuse[i] = Sfootget[i] - '0';
+                }
+                
+                gait_decode();
+                gait_clear(); 
+            } 
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_BOARD/data_board.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,53 @@
+#ifndef _DATA_BOARD_H
+#define _DATA_BOARD_H
+
+
+#include "mbed.h"
+#include "math_ops.h"
+#include "data_pc.h"
+
+extern Serial      foot;
+
+
+
+//extern unsigned int flag_foot_A, flag_foot_B;
+//extern unsigned int Sfootnum;
+//extern uint16_t Sfootget[10];                                                    
+//extern uint16_t Sfootuse[10];
+
+////////////////////////////////////////////////////////////////////////////////
+
+extern unsigned int Gait_num_valid, Gait_now, Gait_per_now, Gait_cycle_now;
+extern uint16_t Gait_per_now_int, COP_Y_int, COP_X_int, Gait_cycle_now_int;
+
+extern unsigned int Gait_num_valid_0before, Gait_now_0before;
+extern float Gait_per_now_0before, COP_Y_0before, COP_X_0before, Gait_cycle_now_0before;
+
+extern unsigned int Gait_num_valid_real, Gait_now_real, Gait_per_now_real, Gait_cycle_now_real, time_portion_now_real;
+
+
+
+//void gait_decode();
+//void gait_clear();
+
+////////////////////////////////////////////////////////////////////////////////
+
+void serial_board_isr();
+
+////////////////////////////////////////////////////////////////////////////////
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,162 @@
+#include "data_command.h"
+#include "data_pc.h"
+#include "CAN.h"
+
+Serial      command(PC_10, PC_11);                                              // U3
+
+
+int enabled = 0;                                                                // 进入电机模式标志位
+int counter = 0;                                                                // 输出计数器
+int command_control_flag = 0;                                                   // 命令帧标志位
+
+float  v_pid = 0;
+float  p_pid = 0;
+float  kp_pid = 0;
+float  kd_pid = 0;
+float  tff_pid = 0;
+
+int c_lock = 0;                                                                 // 位置锁定标志位 
+
+
+unsigned int flag_c_A = 0, flag_c_B = 0;
+unsigned int Snum = 0;
+uint16_t Sget[14] = {0};                                                    
+uint16_t Suse[14] = {0};
+
+
+void serial_command_isr()
+{
+    
+    while(command.readable())
+    {
+        uint8_t c = command.getc();
+        if(c == 'A')
+        {
+            flag_c_A = 1;           
+            flag_c_B = 0;
+            Snum   = 0;
+            for(unsigned int i = 0; i < 14; i++)
+            {
+                Sget[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 'B')
+        {
+            flag_c_B = 1;
+        }
+    
+        if(flag_c_A == 1)
+        {
+            if((flag_c_B != 1) && (Snum < 14))
+            {
+                Sget[Snum] = c;    
+            }
+            
+            Snum++;
+            
+            if((flag_c_B == 1) && (Snum != 15))
+            {                        
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum   = 0;     
+            }
+            
+            if((flag_c_B == 1) && (Snum == 15))
+            {
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum   = 0;
+                
+                for(unsigned int i = 0; i < 14; i++)
+                {
+                    Suse[i] = Sget[i];
+                }  
+                
+                command_control_flag = 1;                                                  // 命令帧接收成功
+                
+//                command.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n\r", Suse[0], Suse[1], Suse[2], Suse[3], Suse[4], Suse[5], Suse[6], Suse[7], Suse[8], Suse[9], Suse[10], Suse[11], Suse[12], Suse[13]);
+            }
+        }
+    }       
+}
+
+
+void command_control()
+{
+    v_pid = 100*(Suse[1] - '0') + 10*(Suse[2] - '0') + (Suse[3] - '0');
+    p_pid = (Suse[4] - '0') + 0.1*(Suse[5] - '0') + 0.01*(Suse[6] - '0');
+    kp_pid = 100*(Suse[7] - '0') + 10*(Suse[8] - '0') + (Suse[9] - '0');
+    kd_pid = (Suse[10] - '0') + 0.1*(Suse[11] - '0');
+    tff_pid = (Suse[12] - '0') + 0.1*(Suse[13] - '0');
+
+    
+    command.printf("%.0f\t%.2f\t%.0f\t%.2f\t%.2f\n", v_pid, p_pid, kp_pid, kd_pid, tff_pid);   //Am0001000100000B
+    
+    wait_us(200); 
+    
+      
+    switch(Suse[0])
+    {
+        case('m'):
+            command.printf("\n\rCommand motor mode\r");                         //Am+010+010B  旋转1rad
+            c_lock = 1;  
+            send_enable = 1;  
+//            EnterMotorMode(&knee_txMsg);
+//            EnterMotorMode(&ankle_txMsg);                                                  // main发送CAN位置命令
+            break; 
+    }   
+    
+    // 电机模式命令在此发送，锁定控制模式的位置量在main中发送   运行到此处 进入/退出/设置零点位置 命令发送完成                 
+    // 解除串口控制，缓冲区清0
+    wait_ms(2);
+    
+    command_control_flag = 0;                                                   //避免下次继续进入command_control函数
+
+    for(unsigned int i = 0; i < 9; i++)
+    {
+        Suse[i] = 0;      
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,58 @@
+#ifndef _DATA_COMMAND_H
+#define _DATA_COMMAND_H
+
+
+#include "mbed.h"
+#include "CAN.h"
+#include "used_leg_message.h"
+#include "mode.h"
+#include "control.h"
+
+
+extern Serial      command;
+
+extern int command_control_flag;
+
+extern float  v_pid, p_pid, kp_pid, kd_pid, tff_pid;
+
+extern int c_lock; 
+
+
+void serial_command_isr();
+void command_control();
+
+
+
+
+
+
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_PC/data_pc.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,66 @@
+#include "data_pc.h"
+#include "data_command.h"
+
+Serial pc(PA_2, PA_3);                                                          // tx, rx U2 min
+
+
+int pc_control = 0;                                                              // 命令帧标志位
+unsigned int flag_A = 0, flag_B = 0;
+unsigned int pc_num = 0;
+
+uint16_t mode_flag_get[1] = {0};
+uint16_t mode_flag_use[1] = {0};
+unsigned int mode_flag = 3;
+
+void pc_decode(){
+    mode_flag = mode_flag_use[0];
+}
+
+void pc_clear(){
+    mode_flag_use[0] = 0;
+}
+
+void serial_pc_isr(){
+    while(pc.readable()){
+        uint16_t c=pc.getc();
+        if(c == 'A'){
+            flag_A = 1;
+            flag_B = 0;
+            pc_num = 0;
+            mode_flag_get[0] = 0;
+            break; 
+        }
+        if(c == 'B'){
+            flag_B = 1;
+        }
+        if(flag_A == 1){
+            if((flag_B != 1) && (pc_num < 1))
+            {
+                mode_flag_get[pc_num] = c;    
+            }
+            pc_num++;
+            
+            if((flag_B == 1) && (pc_num != 2))
+            {                        
+                flag_A = 0;
+                flag_B = 0; 
+                pc_num   = 0;     
+            }
+            if((flag_B == 1) && (pc_num == 2))
+            {                        
+                flag_A = 0;
+                flag_B = 0; 
+                pc_num   = 0; 
+                
+                mode_flag_use[0] = mode_flag_get[0] - '0'; 
+                pc_decode();
+                
+                command.printf("%d", mode_flag);                                                    // 命令帧接收成功   
+                
+                pc_clear();
+                
+            }
+            pc_control = 1;                                                      
+        } 
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_PC/data_pc.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,21 @@
+#ifndef _DATA_PC_H
+#define _DATA_PC_H
+
+#include "mbed.h"
+#include "data_command.h"
+
+extern Serial pc;              // tx, rx U2 min
+
+extern int pc_control;                                                              // 命令帧标志位
+//extern unsigned int flag_A, flag_B;
+//extern unsigned int pc_num;
+
+//extern uint16_t mode_flag_get[1];
+//extern uint16_t mode_flag_use[1];
+extern unsigned int mode_flag;
+
+void pc_decode();
+void pc_clear();
+void serial_pc_isr();
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/leg_message.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,61 @@
+#ifndef _leg_message
+#define _leg_message
+
+// 头文件
+#include <stdint.h>
+
+// 定义结构体
+struct joint_control{                                                           // 关节控制结构体
+    float p_des, v_des, kp, kd, t_ff;                                           // 控制量为: p_des, v_des, kp, kd, t_ff
+    };
+
+struct leg_control{                                                             // 系统控制结构体
+    joint_control ankle, knee;                                                  // 控制成员: knee, ankle关节
+    };
+
+
+struct joint_state{                                                             // 关节状态结构体
+    float p, v, t;                                                              // 状态成员:  p\v\t
+    };
+    
+struct leg_state{                                                               // 系统状态结构体
+    joint_state ankle_state, knee_state;                                        // 状态成员: ankle_state, knee_state状态
+    };
+
+
+//struct cal_data_t
+//{
+//    float q_pf;
+//    float q_df;
+//
+//    float qd_pf;
+//    float qd_df;
+//
+//    int32_t flag_pf;
+//    int32_t flag_df;
+//    //int32_t checksum;
+//};
+//
+struct cal_command_t
+{
+    float q_des_ankle;
+    float q_des_knee;
+
+    float qd_des_ankle;
+    float qd_des_knee;
+
+    float kp_ankle;
+    float kp_knee;
+
+    float kd_ankle;
+    float kd_knee;
+
+    float tau_ankle_ff;
+    float tau_knee_ff;
+
+    int32_t flag;                                                               // 进入电机模式标志位
+    //int32_t checksum;
+};
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/used_leg_message.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,24 @@
+#include "used_leg_message.h"
+
+
+
+
+leg_control a_control;
+leg_state   a_state;
+
+
+cal_command_t   cal_command; 
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/used_leg_message.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,28 @@
+#ifndef _USED_LEG_MESSAGE_H
+#define _USED_LEG_MESSAGE_H
+
+
+
+#include "mbed.h"
+#include "leg_message.h"
+
+
+
+
+
+
+
+
+
+extern leg_control a_control;
+extern leg_state   a_state;
+
+extern cal_command_t   cal_command; 
+
+
+
+
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODE/mode.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,45 @@
+#include "mode.h"
+#include "CAN.h"
+
+
+
+// 
+void Zero(CANMessage * msg){
+    msg->data[0] = 0xFF;
+    msg->data[1] = 0xFF;
+    msg->data[2] = 0xFF;
+    msg->data[3] = 0xFF;
+    msg->data[4] = 0xFF;
+    msg->data[5] = 0xFF;
+    msg->data[6] = 0xFF;
+    msg->data[7] = 0xFE;
+    WriteAll();
+}
+
+
+// 进入电机模式
+void EnterMotorMode(CANMessage * msg){
+    msg->data[0] = 0xFF;
+    msg->data[1] = 0xFF;
+    msg->data[2] = 0xFF;
+    msg->data[3] = 0xFF;
+    msg->data[4] = 0xFF;
+    msg->data[5] = 0xFF;
+    msg->data[6] = 0xFF;
+    msg->data[7] = 0xFC;
+    WriteAll();
+}
+
+
+// 退出电机模式
+void ExitMotorMode(CANMessage * msg){
+    msg->data[0] = 0xFF;
+    msg->data[1] = 0xFF;
+    msg->data[2] = 0xFF;
+    msg->data[3] = 0xFF;
+    msg->data[4] = 0xFF;
+    msg->data[5] = 0xFF;
+    msg->data[6] = 0xFF;
+    msg->data[7] = 0xFD;
+    WriteAll();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODE/mode.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,22 @@
+#ifndef _MODE_H
+#define _MODE_H
+
+
+#include "mbed.h"
+
+
+void Zero(CANMessage * msg);
+void EnterMotorMode(CANMessage * msg);
+void ExitMotorMode(CANMessage * msg);
+
+
+
+
+
+
+
+
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TIMER/timer.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,8 @@
+#include "timer.h"
+
+
+
+Timer fuzzytimer;
+uint64_t Tfuzzy = 0;
+
+Timer tim;
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TIMER/timer.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,15 @@
+#ifndef _TIMER_H
+#define _TIMER_H
+
+
+#include "mbed.h"
+
+
+extern Timer fuzzytimer;
+extern uint64_t Tfuzzy;
+
+
+extern Timer tim;
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fuzzy/fuzzy.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,99 @@
+/************************************************************************************
+* @author：
+* @date : 2019/3/25
+* @fuction name：FUZZY_CONTROL
+* @fuction description： 模糊自适应控制算法
+*************************************************************************************/
+#include "fuzzy.h"
+
+fuzzy_control_t ankleFuzzy, kneeFuzzy;
+
+
+//模糊集合
+/*
+#define NL   -3
+#define NM   -2
+#define NS   -1
+#define ZE   0
+#define PS   1
+#define PM   2
+#define PL   3
+*/
+const float FuzzyRuleKp[7][7]={
+    /*                  EC                     */
+    /*  -3,       -2,     -1,     0,     1,      2,     3 */
+/*-3*/ 473.86, 473.86, 416.63, 416.63, 333.37, 250.00, 250.00,
+/*-2*/ 473.86, 473.86, 416.63, 333.37, 333.37, 250.00, 250.00,
+/*-1*/ 416.63, 416.63, 416.63, 416.63, 250.00, 166.64, 166.64,
+/* 0*/ 416.63, 416.63, 416.63, 250.00, 333.37, 83.38,  83.38,
+/* 1*/ 333.37, 333.37, 250.00, 166.64, 166.64, 83.38,  83.38,
+/* 2*/ 333.37, 250.00, 166.64, 83.38,  83.38,  83.38,  26.15,
+/* 3*/ 250.00, 250.00, 83.38,  83.38,  83.38,  26.15,  26.15,
+};
+
+const float FuzzyRuleKd[7][7]={
+    /*                  EC                     */
+    /*  -3,   -2,   -1,   0,     1,   2,    3 */
+/*-3*/ 3.33, 1.67, 0.26, 0.26, 0.26, 0.83, 3.33,
+/*-2*/ 3.33, 2.50, 0.26, 0.26, 0.26, 0.83, 3.33,
+/*-1*/ 2.50, 1.67, 1.67, 0.83, 1.67, 1.67, 2.50,
+/* 0*/ 2.50, 1.67, 4.17, 1.67, 1.67, 1.67, 2.50,
+/* 1*/ 2.50, 2.50, 2.50, 2.50, 2.50, 2.50, 2.50,
+/* 2*/ 4.74, 1.67, 3.33, 3.33, 3.33, 3.33, 4.74,
+/* 3*/ 4.74, 4.17, 4.17, 4.17, 3.33, 3.33, 0.26,
+};
+
+
+void fuzzy_init(fuzzy_control_t *fuzzy ,float max_e,float max_ec,float gain)
+{
+    fuzzy->quantied_e = 3.0f/max_e;
+    fuzzy->quantied_ec = 3.0f/max_ec;
+    fuzzy->quantied_resault = gain;
+    fuzzy->outKp = 0.0f;
+    fuzzy->outKd = 0.0f;
+}
+void fuzzy_control(float e,float ec,fuzzy_control_t *fuzzy,const float fuzzyRuleKp[7][7], const float fuzzyRuleKd[7][7])
+{
+
+     float etemp,ectemp;
+     float eLefttemp,ecLefttemp;
+     float eRighttemp ,ecRighttemp;
+
+     int eLeftIndex,ecLeftIndex;
+     int eRightIndex,ecRightIndex;
+     e = e * fuzzy->quantied_e;
+     ec = ec * fuzzy->quantied_ec;
+     e = range(e,-3.0f,3.0f);
+     ec = range(ec,-3.0f,3.0f);
+     etemp = e > 3.0f ? 0.0f : (e < - 3.0f ? 0.0f : (e >= 0.0f ? (e >= 2.0f ? 2.5f: (e >= 1.0f ? 1.5f : 0.5f)) : (e >= -1.0f ? -0.5f : (e >= -2.0f ? -1.5f : (e >= -3.0f ? -2.5f : 0.0f)))));
+     eLeftIndex = (int)((etemp-0.5f) + 3.0f);        //[-3,3] -> [0,6]
+     eRightIndex = (int)((etemp+0.5f) + 3.0f);
+     eLefttemp =etemp == 0.0f ? 0.0f:((etemp+0.5f)-e);             
+     eRighttemp=etemp == 0.0f ? 0.0f:( e-(etemp-0.5f));
+     
+     ectemp = ec > 3.0f ? 0.0f : (ec < - 3.0f ? 0.0f : (ec >= 0.0f ? (ec >= 2.0f ? 2.5f: (ec >= 1.0f ? 1.5f : 0.5f)) : (ec >= -1.0f ? -0.5f : (ec >= -2.0f ? -1.5f : (ec >= -3.0f ? -2.5f : 0.0f) ))));
+     ecLeftIndex = (int)((ectemp-0.5f) + 3.0f);        //[-6,6] -> [0,12]
+     ecRightIndex = (int)((ectemp+0.5f) + 3.0f);
+     ecLefttemp =ectemp == 0.0f ? 0.0f:((ectemp+0.5f)-ec);
+     ecRighttemp=ectemp == 0.0f ? 0.0f:( ec-(ectemp-0.5f));
+
+
+/*************************************反模糊*************************************/
+
+
+
+
+    fuzzy->outKp = (eLefttemp * ecLefttemp * fuzzyRuleKp[eLeftIndex][ecLeftIndex]
+                    + eLefttemp * ecRighttemp * fuzzyRuleKp[eLeftIndex][ecRightIndex]
+                    + eRighttemp * ecLefttemp * fuzzyRuleKp[eRightIndex][ecLeftIndex]
+                    + eRighttemp * ecRighttemp * fuzzyRuleKp[eRightIndex][ecRightIndex]);
+                    
+    fuzzy->outKd = (eLefttemp * ecLefttemp * fuzzyRuleKd[eLeftIndex][ecLeftIndex]
+                    + eLefttemp * ecRighttemp * fuzzyRuleKd[eLeftIndex][ecRightIndex]
+                    + eRighttemp * ecLefttemp * fuzzyRuleKd[eRightIndex][ecLeftIndex]
+                    + eRighttemp * ecRighttemp * fuzzyRuleKd[eRightIndex][ecRightIndex]);
+
+    //对解算出的模糊结果进行量化
+    fuzzy->outKp = fuzzy->outKp * fuzzy->quantied_resault;
+    fuzzy->outKd = fuzzy->outKd * fuzzy->quantied_resault;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fuzzy/fuzzy.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,37 @@
+#ifndef _fuzzy_h_
+#define _fuzzy_h_
+
+#include "mbed.h"
+
+#define max(a, b)           (a>b? a:b)
+#define min(a, b)           (a<b? a:b)
+#define range(x, a, b)      (min(max(x, a), b))
+
+
+#define ANKLEE   (60.0f)
+#define ANKLEEC  (15.0f)
+
+#define KNEEE   (35.0f)
+#define KNEEEC  (15.0f)
+
+
+
+
+extern const float FuzzyRuleKp[7][7];
+extern const float FuzzyRuleKd[7][7];
+
+
+typedef struct{
+float quantied_e;
+float quantied_ec;
+float quantied_resault;
+float outKp;
+float outKd;
+}fuzzy_control_t;
+void fuzzy_init(fuzzy_control_t *fuzzy ,float max_e,float max_ec,float gain);
+void fuzzy_control(float e,float ec,fuzzy_control_t *fuzzy,const float fuzzyRuleKp[7][7],const float fuzzyRuleKd[7][7]);
+
+extern fuzzy_control_t ankleFuzzy, kneeFuzzy;
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,126 @@
+#include "mbed.h"
+#include <cstring>
+#include "math_ops.h"
+#include "leg_message.h"
+#include "CAN.h"
+#include "used_leg_message.h"
+#include "data_pc.h"
+#include "data_board.h"
+#include "mode.h"
+#include "data_command.h"
+#include "math_ops.h"
+#include "fuzzy.h"
+#include "calculate.h"
+#include "timer.h"
+
+
+float ankp = 0.0f;
+float ankv = 0.0f;
+float ankt = 0.0f;
+
+float knp = 0.0f;
+float knv = 0.0f;
+float knt = 0.0f;
+
+float count=0.0f;
+float result_knee=0.0f;
+float result_ankle=0.0f;
+
+////////////////////////////////////////////////////////////////////////////////
+//                           PID调试                          //
+////////////////////////////////////////////////////////////////////////////////
+
+
+int main()
+{
+
+////////////////////////初始化//////////////////////////////////////
+    pc.baud(115200);                                                            // U2
+    pc.attach(&serial_pc_isr);
+
+    foot.baud(115200);                                                          // U1
+    foot.attach(&serial_board_isr);
+
+    command.baud(115200);                                                       // U3
+    command.attach(&serial_command_isr);
+
+    ankle_can.frequency(1000000);
+    ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    ankle_rxMsg.len = 6;
+    ankle_txMsg.len   = 8;
+    ankle_txMsg.id    = 0x01;
+
+    knee_can.frequency(1000000);
+    knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    knee_rxMsg.len = 6;
+    knee_txMsg.len   = 8;
+    knee_txMsg.id    = 0x02;
+////////////////////////////////////////////////////////////////////////////////
+
+    wait(4);
+    EnterMotorMode(&knee_txMsg);
+    EnterMotorMode(&ankle_txMsg);
+    Zero(&knee_txMsg);
+    Zero(&ankle_txMsg);
+      
+    tim.start();
+    
+    while(1) {    
+        ankle_can.read(ankle_rxMsg);
+        unpack_reply(ankle_rxMsg, &a_state);
+        wait_us(100);
+        knee_can.read(knee_rxMsg);
+        unpack_reply(knee_rxMsg, &a_state);
+    
+        ankp = a_state.ankle_state.p;                                           // 从CAN获得的当前位置
+        ankv = a_state.ankle_state.v;
+        ankt = a_state.ankle_state.t;
+  
+        knp = a_state.knee_state.p;                                            // 从CAN获得的当前位置
+        knv = a_state.knee_state.v;
+        knt = a_state.knee_state.t;
+       
+        control(ankp, knp);
+        
+        if(send_enable == 1){ 
+            PackAll();
+            WriteAll(); 
+        }
+//      pc.printf("p:%.3f\tv:%.3f\tKp:%.3f\tKd:%.3f\ttff:%.3f\n",a_control.ankle.p_des, a_control.ankle.v_des, a_control.ankle.kp, a_control.ankle.kd, a_control.ankle.t_ff);     
+//      pc.printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", a_state.knee_state.p, a_control.knee.p_des, a_state.ankle_state.p, a_control.ankle.p_des,tim.read());  
+//      pc.printf("AB%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", a_state.knee_state.p, a_control.knee.p_des, a_state.knee_state.t, a_state.ankle_state.p, a_control.ankle.p_des, a_state.ankle_state.t);  
+        
+        pc.printf("AB");
+        if(a_state.knee_state.p >= 0)
+            pc.printf("+%.3f\t",a_state.knee_state.p);
+        else
+            pc.printf("%.3f\t",a_state.knee_state.p);
+        
+        if(a_control.knee.p_des >= 0)
+            pc.printf("+%.3f\t",a_control.knee.p_des);
+        else
+            pc.printf("%.3f\t",a_control.knee.p_des);
+        
+        if(a_state.knee_state.t >= 0)
+            pc.printf("+%.3f\t",a_state.knee_state.t);
+        else
+            pc.printf("%.3f\t",a_state.knee_state.t);
+        
+        if(a_state.ankle_state.p >= 0)
+            pc.printf("+%.3f\t",a_state.ankle_state.p);
+        else
+            pc.printf("%.3f\t",a_state.ankle_state.p);
+        
+        if(a_control.ankle.p_des >= 0)
+            pc.printf("+%.3f\t",a_control.ankle.p_des);
+        else
+            pc.printf("%.3f\t",a_control.ankle.p_des);
+             
+        if(a_state.ankle_state.t >= 0)
+            pc.printf("+%.3f\n",a_state.ankle_state.t);
+        else
+            pc.printf("%.3f\n",a_state.ankle_state.t);       
+        
+        
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops/math_ops.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,48 @@
+
+#include "math_ops.h"
+
+
+float fmaxf(float x, float y){
+    /// Returns maximum of x, y ///
+    return (((x)>(y))?(x):(y));
+    }
+
+float fminf(float x, float y){
+    /// Returns minimum of x, y ///
+    return (((x)<(y))?(x):(y));
+    }
+
+float fmaxf3(float x, float y, float z){
+    /// Returns maximum of x, y, z ///
+    return (x > y ? (x > z ? x : z) : (y > z ? y : z));
+    }
+
+float fminf3(float x, float y, float z){
+    /// Returns minimum of x, y, z ///
+    return (x < y ? (x < z ? x : z) : (y < z ? y : z));
+    }
+    
+void limit_norm(float *x, float *y, float limit){
+    /// Scales the lenght of vector (x, y) to be <= limit ///
+    float norm = sqrt(*x * *x + *y * *y);
+    if(norm > limit){
+        *x = *x * limit/norm;
+        *y = *y * limit/norm;
+        }
+    }
+
+
+int float_to_uint(float x, float x_min, float x_max, int bits){
+    /// Converts a float to an unsigned int, given range and number of bits ///
+    float span = x_max - x_min;
+    float offset = x_min;
+    return (int) ((x-offset)*((float)((1<<bits)-1))/span);
+    }
+    
+    
+float uint_to_float(int x_int, float x_min, float x_max, int bits){
+    /// converts unsigned int to float, given range and number of bits ///
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops/math_ops.h	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,16 @@
+#ifndef MATH_OPS_H
+#define MATH_OPS_H
+
+#define PI 3.14159265359f
+
+#include "math.h"
+
+float fmaxf(float x, float y);
+float fminf(float x, float y);
+float fmaxf3(float x, float y, float z);
+float fminf3(float x, float y, float z);
+void limit_norm(float *x, float *y, float limit);
+int float_to_uint(float x, float x_min, float x_max, int bits);
+float uint_to_float(int x_int, float x_min, float x_max, int bits);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dev.lib	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/benkatz/code/mbed-dev_spine/#97f825502e2a