Revision 0:6aa5928be32a, committed 2021-06-18

Comitter:

panzhan

Date:

Fri Jun 18 07:17:12 2021 +0000

Commit message:

1

Changed in this revision

diff -r 000000000000 -r 6aa5928be32a CAN/CAN.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.cpp	Fri Jun 18 07:17:12 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;
+        }
+} 
+
+
+
+
+

diff -r 000000000000 -r 6aa5928be32a CAN/CAN.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.h	Fri Jun 18 07:17:12 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

diff -r 000000000000 -r 6aa5928be32a CONTROL/CALCULATE/calculate.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/CALCULATE/calculate.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,385 @@
+#include "calculate.h"
+
+
+unsigned int send_enable = 0;
+unsigned int init_joint_angles_flag = 0;
+float pos_ankle = 0.0f;
+float pos_knee = 0.0f;
+target_pid tagertPID;
+
+
+float position_knee(float res)
+{   
+    float x = res*100/3.0f;      //计算步态周期百分比   每一个步态周期长度为3s
+    
+    if (mode_flag==8){      
+        x = res*100/2.0f;        //快速行走步态周期长度为2s
+    }
+    
+    
+    float a = 0.0f;
+
+
+    /*
+      判断运动模式
+    */
+    if(mode_flag==1) {
+        //坐
+        tagertPID.knee_target_Kp = 150; 
+        tagertPID.knee_target_Kd = 0;
+        tagertPID.knee_target_tff = 0;
+        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) {
+        //起
+        tagertPID.knee_target_Kp = 150; 
+        tagertPID.knee_target_Kd = 0;
+        tagertPID.knee_target_tff = 0;
+        a =  0.9133f + 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) {
+        //平地
+        tagertPID.knee_target_Kp = 180; 
+        tagertPID.knee_target_Kd = 0;
+        tagertPID.knee_target_tff = 0;
+        
+        a = 0.2718f - 0.03479f*cos(0.06021f*x) - 0.2809f*sin(0.06021f*x) \
+            - 0.1804f*cos(2*0.06021f*x) + 0.09041f*sin(2*0.06021f*x) \
+            + 0.001454f*cos(3*0.06021f*x) + 0.06005f*sin(3*0.06021f*x);
+            
+
+    } else if(mode_flag==4) {
+        //上坡
+        tagertPID.knee_target_Kp = 180; 
+        tagertPID.knee_target_Kd = 0;
+        tagertPID.knee_target_tff = 0;
+        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) {
+        //下坡
+        tagertPID.knee_target_Kp = 180; 
+        tagertPID.knee_target_Kd = 0;
+        tagertPID.knee_target_tff = 0;        
+        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) {
+        //上楼
+        tagertPID.knee_target_Kp = 180; 
+        tagertPID.knee_target_Kd = 0;
+        tagertPID.knee_target_tff = 0;         
+        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) {
+        //下楼
+        tagertPID.knee_target_Kp = 180; 
+        tagertPID.knee_target_Kd = 0;
+        tagertPID.knee_target_tff = 0;         
+        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) {
+        tagertPID.knee_target_Kp = 180; 
+        tagertPID.knee_target_Kd = 0;
+        tagertPID.knee_target_tff = 0; 
+        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 a = 0.0f;
+    float x = res*100/3.0f;      
+    
+    if (mode_flag==8){
+        x = res*100/2.0f;
+    }
+    
+    if(mode_flag == 1) {
+        //坐
+        tagertPID.ankle_target_Kp = 0; 
+        tagertPID.ankle_target_Kd = 0;
+        tagertPID.ankle_target_tff = 0;         
+        a = 0.0f;
+    } else if(mode_flag == 2) {
+        //起
+        tagertPID.ankle_target_Kp = 0; 
+        tagertPID.ankle_target_Kd = 0;
+        tagertPID.ankle_target_tff = 0;   
+        a = 0.0f;
+    } else if(mode_flag == 3) {
+        //平地
+        tagertPID.ankle_target_Kp = 220; 
+        tagertPID.ankle_target_Kd = 1;
+        tagertPID.ankle_target_tff = 0; 
+        a =  0.05373f - 0.005853f*cos(0.05922f*x) + 0.1164f*sin(0.05922f*x) \
+                + 0.02177f*cos(2*0.05922f*x) - 0.08126f*sin(2*0.05922f*x) \
+                - 0.05912f*cos(3*0.05922f*x) + 0.01009f*sin(3*0.05922f*x) \
+                + 0.02115f*cos(4*0.05922f*x) + 0.001152f*sin(4*0.05922f*x) \
+                - 0.00824f*cos(5*0.*x) - 0.01898f*sin(5*0.05922f*x);
+
+    } else if(mode_flag == 4) {
+        //上坡
+        tagertPID.ankle_target_Kp = 220; 
+        tagertPID.ankle_target_Kd = 0;
+        tagertPID.ankle_target_tff = 0; 
+        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) {
+        //下坡
+        tagertPID.ankle_target_Kp = 220; 
+        tagertPID.ankle_target_Kd = 0;
+        tagertPID.ankle_target_tff = 0; 
+        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) {
+        //上楼
+        tagertPID.ankle_target_Kp = 280; 
+        tagertPID.ankle_target_Kd = 1;
+        tagertPID.ankle_target_tff = 0; 
+        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) {
+        //下楼
+        tagertPID.ankle_target_Kp = 220; 
+        tagertPID.ankle_target_Kd = 1;
+        tagertPID.ankle_target_tff = 0; 
+        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) {
+        //快速
+        tagertPID.ankle_target_Kp = 300; 
+        tagertPID.ankle_target_Kd = 1;
+        tagertPID.ankle_target_tff = 0; 
+        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;
+}
+
+
+
+void calculate_fuzzy(float ankle_real_p, float knee_real_p)
+{
+    send_enable = 1;    // can可以发送数据
+    
+    //初始化关节角度
+    if(init_joint_angles_flag == 0){
+        init_joint_angles_flag = 1;
+        init_joint_angles();
+    }
+    
+    
+    tim_time =  tim.read();
+//    pc.printf("%.3f\n",tim_time);    
+    
+    
+    if(Gait_reset_flag == 1){
+        Gait_reset_flag = 0;       
+        tim.reset();
+    }
+    
+    if (mode_flag==8){
+       if(tim_time >= 2){
+            send_enable = 0;
+        }else{        
+            calculate_ankle_fuzzy(ankle_real_p, knee_real_p);        
+        } 
+    }else if (mode_flag!=8){
+       if(tim_time >= 3){
+            send_enable = 0;
+        }else{        
+            calculate_ankle_fuzzy(ankle_real_p, knee_real_p);        
+        } 
+    }
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                              初始化关节角度                                 //
+////////////////////////////////////////////////////////////////////////////////
+
+void init_joint_angles(){
+    
+    a_control.ankle.p_des = position_ankle(3.0f);
+    a_control.ankle.v_des = 0;
+    a_control.ankle.kp    = 80;
+    a_control.ankle.kd    = 0;
+    a_control.ankle.t_ff  = 0;
+        
+    a_control.knee.p_des = position_knee(3.0f);
+    a_control.knee.v_des = 0;
+    a_control.knee.kp    = 80;
+    a_control.knee.kd    = 0;
+    a_control.knee.t_ff  = 0;
+    
+    PackAll();
+    WriteAll(); 
+    
+    wait(3);
+}
+
+
+
+//    Gait_per_now_real     当前步态百分比
+
+float ank_e = 0.0f, ank_before_e = 0.0f, ank_ec = 0.0f;
+float kn_e = 0.0f, kn_before_e = 0.0f, kn_ec = 0.0f;
+
+unsigned int fuzzy50 = 1;
+void calculate_ankle_fuzzy(float ankle_real_p, float knee_real_p)
+{
+    pos_ankle = position_ankle(tim.read());
+    pos_knee = position_knee(tim.read());
+    //膝关节
+    if(Gait_now_real==1 || Gait_now_real==4) {
+        fuzzy_position_control_ankle(ankle_real_p, pos_ankle);
+        fuzzy_position_control_knee(knee_real_p, pos_knee);
+        //膝关节
+    } else if(Gait_now_real==2 || Gait_now_real==3) {
+        fuzzy_position_control_ankle(ankle_real_p, pos_ankle);
+        fuzzy_position_control_knee(knee_real_p, pos_knee);
+    }
+}
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////踝关节模糊控制/////////////////////////////////
+void fuzzy_position_control_ankle(float ankle_real_p, float pos_ankle)
+{
+
+    ank_e = ankle_real_p - pos_ankle;    // 实际值 - 参考值
+
+    if(ank_e > 0.3f*ankleFuzzy.quantied_e)
+        ank_e = 0.3f*ankleFuzzy.quantied_e;
+    if(ank_e <= (0.0f - 0.3f*ankleFuzzy.quantied_e))
+        ank_e = 0.0f - 0.3f*ankleFuzzy.quantied_e;
+
+    ank_ec =  ank_e - ank_before_e;
+
+    if(ank_ec > 0.3f*ankleFuzzy.quantied_ec)
+        ank_ec = 0.3f*ankleFuzzy.quantied_ec;
+    if(ank_ec <= (0.0f - 0.3f*ankleFuzzy.quantied_ec))
+        ank_ec = 0.0f - 0.3f*ankleFuzzy.quantied_ec;
+
+    ank_before_e = ank_ec;
+
+
+    if(fuzzy50 == 1) {                                                          // 进入模糊计算，每10ms计算一次，设初始值为1，则可以第一次进入
+        fuzzytimer.reset();                                                     // 计时清0
+        fuzzy_control(ank_e, ank_ec, &ankleFuzzy,&FuzzyRuleKp[0], &FuzzyRuleKd[0]);
+    }//出模糊计算
+    cal_command.q_des_ankle = pos_ankle;
+    cal_command.qd_des_ankle = 0;
+    cal_command.kp_ankle = ankleFuzzy.outKp + tagertPID.ankle_target_Kp;
+    cal_command.kd_ankle = ankleFuzzy.outKd + tagertPID.ankle_target_Kd;
+    cal_command.tau_ankle_ff = 0;
+    pc.printf("outKp:%f\toutKd:%f\tpos_ankle:%f\tankle_real_p:%f\ttime:%f\n", cal_command.kp_ankle, cal_command.kd_ankle, pos_ankle, ankle_real_p, tim.read());
+}
+
+void torque_control_ankle(){
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////膝关节模糊控制//////////////////////////////
+void fuzzy_position_control_knee(float knee_real_p, float pos_knee){
+
+    kn_e = knee_real_p - pos_knee;    // 实际值 - 参考值
+
+    if(kn_e > 0.3f*kneeFuzzy.quantied_e)
+        kn_e = 0.3f*kneeFuzzy.quantied_e;
+    if(kn_e <= (0.0f - 0.3f*kneeFuzzy.quantied_e))
+        kn_e = 0.0f - 0.3f*kneeFuzzy.quantied_e;
+
+    kn_ec =  kn_e - kn_before_e;
+
+    if(kn_ec > 0.3f*kneeFuzzy.quantied_ec)
+        kn_ec = 0.3f*kneeFuzzy.quantied_ec;
+    if(kn_ec <= (0.0f - 0.3f*kneeFuzzy.quantied_ec))
+        kn_ec = 0.0f - 0.3f*kneeFuzzy.quantied_ec;
+
+    kn_before_e = kn_ec;
+
+    if(fuzzy50 == 1) {                                              // 进入模糊计算，每10ms计算一次，设初始值为1，则可以第一次进入
+        fuzzy50 = 0;
+        fuzzy_control(kn_e, kn_ec, &kneeFuzzy, &FuzzyRuleKp[0], &FuzzyRuleKd[0]);
+    }//出模糊计算
+    
+    Tfuzzy = fuzzytimer.read_ms();
+    if(Tfuzzy >= CT)
+        fuzzy50 = 1;            // 间隔10ms后可进入
+    cal_command.q_des_knee = pos_knee;
+    cal_command.qd_des_knee = 0;
+    cal_command.kp_knee = kneeFuzzy.outKp + tagertPID.knee_target_Kp;
+    cal_command.kd_knee = kneeFuzzy.outKd + tagertPID.knee_target_Kd;
+    cal_command.tau_knee_ff = 0;
+    //pc.printf("outKp:%f\toutKd:%f\tpos_knee:%f\tknee_real_p:%f\ttime:%f\n", cal_command.kp_knee, cal_command.kd_knee, pos_knee, knee_real_p, tim.read());
+}
+
+void torque_control_knee(){
+
+}
+
+
+
+

diff -r 000000000000 -r 6aa5928be32a CONTROL/CALCULATE/calculate.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/CALCULATE/calculate.h	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,53 @@
+#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 (50)
+
+
+typedef struct{
+float ankle_target_Kp;
+float ankle_target_Kd;
+float ankle_target_tff;
+
+float knee_target_Kp;
+float knee_target_Kd;
+float knee_target_tff;
+}target_pid;/*模糊PID的s设定值*/
+
+
+extern unsigned int send_enable;
+extern float pos_ankle;
+extern float pos_knee;
+extern target_pid tagertPID;
+
+
+
+
+float position_knee(float res);
+float position_ankle(float res);
+void calculate_ankle_fuzzy(float ankle_target_p, float knee_target_p);
+void calculate_fuzzy(float ankle_target_p, float knee_target_p);
+
+void fuzzy_position_control_ankle(float ankle_real_p, float pos_ankle);
+void fuzzy_position_control_knee(float knee_real_p, float pos_knee);
+
+void torque_control_ankle();
+void torque_control_knee();
+
+
+
+void init_joint_angles();
+
+
+
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 6aa5928be32a CONTROL/control.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,57 @@
+#include "control.h"
+
+
+void control(float ankle_real_p, float knee_real_p)
+{
+    if(command_control_flag == 1)
+        command_control();
+            
+    if(c_lock == 1)                                                             // 处于PC串口锁定模式 
+    {
+        cal_command.q_des_ankle  = SP_ankle;
+        cal_command.qd_des_ankle = 0;
+        cal_command.kp_ankle     = 20;
+        cal_command.kd_ankle     = 1;
+        cal_command.tau_ankle_ff = 0;
+        
+        cal_command.q_des_knee  = SP_knee;
+        cal_command.qd_des_knee = 0;
+        cal_command.kp_knee     = 20;
+        cal_command.kd_knee     = 1;
+        cal_command.tau_knee_ff = 0;        
+    }
+    
+    if(c_lock == 0)
+    {
+        calculate_fuzzy(ankle_real_p, knee_real_p);
+    }
+    
+    // 将计算得到的数值赋给控制器
+    
+    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;
+    
+}
+  
+    
+
+
+
+
+
+
+      
+        
+        
+
+
+

diff -r 000000000000 -r 6aa5928be32a CONTROL/control.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.h	Fri Jun 18 07:17:12 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

diff -r 000000000000 -r 6aa5928be32a DATA_BOARD/data_board.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_BOARD/data_board.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,123 @@
+#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_now_real_before = 0, Gait_now_real = 0, Gait_per_now_real = 0, time_portion_now_real=0.0f;
+unsigned int Gait_reset_flag = 0;   //计时器复位标志
+
+void gait_decode()
+{
+    Gait_now_real_before = Gait_now_real;
+    Gait_now_real = Sfootuse[0];
+    Gait_per_now_real = Sfootuse[1]*100 + Sfootuse[2]*10 + Sfootuse[3];
+    time_portion_now_real = Sfootuse[4]*100000 + Sfootuse[5]*10000 + Sfootuse[6]*1000 + Sfootuse[7]*100 + Sfootuse[8]*10 + Sfootuse[9];
+   
+    if ( mode_flag == 8 ){
+        if(Gait_now_real_before == 4 && Gait_now_real == 1  && tim.read() >= 2){
+            Gait_reset_flag = 1;
+            pc.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
+        }         
+    }else if( mode_flag == 1 ){
+        if((Gait_now_real_before == 4 && (Gait_now_real == 1 || Gait_now_real == 3)) && tim.read() >= 3){
+            mode_flag = 2;
+            Gait_reset_flag = 1;
+            pc.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
+        }          
+    }else if( mode_flag == 2 ){
+        if((Gait_now_real_before == 4 && (Gait_now_real == 1 || Gait_now_real == 3)) && tim.read() >= 3){
+            mode_flag = 1;
+            Gait_reset_flag = 1;
+            pc.printf("%01d---%03d---%06d\n",Gait_now_real,Gait_per_now_real,time_portion_now_real);
+        }          
+    }else{
+        if((Gait_now_real_before == 4 && (Gait_now_real == 1 || Gait_now_real == 3)) && tim.read() >= 3){
+            Gait_reset_flag = 1;
+            pc.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

diff -r 000000000000 -r 6aa5928be32a DATA_BOARD/data_board.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_BOARD/data_board.h	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,35 @@
+#ifndef _DATA_BOARD_H
+#define _DATA_BOARD_H
+
+
+#include "mbed.h"
+#include "math_ops.h"
+#include "data_pc.h"
+#include "timer.h"
+
+extern Serial      foot;
+////////////////////////////////////////////////////////////////////////////////
+
+extern unsigned int Gait_now_real_before, Gait_now_real, Gait_per_now_real, time_portion_now_real;
+extern unsigned int Gait_reset_flag;
+////////////////////////////////////////////////////////////////////////////////
+
+void serial_board_isr();
+
+////////////////////////////////////////////////////////////////////////////////
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+

diff -r 000000000000 -r 6aa5928be32a DATA_COMMAND/data_command.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,168 @@
+#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 SP_ankle = 0, SP_knee = 0;
+//int c_lock = 2;                                                                 // 位置锁定标志位 
+int c_lock = 0;                                                               // 位置锁定标志位     
+
+unsigned int flag_c_A = 0, flag_c_B = 0;
+unsigned int Snum = 0;
+uint16_t Sget[9] = {0};                                                    
+uint16_t Suse[9] = {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 < 9; i++)
+            {
+                Sget[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 'B')
+        {
+            flag_c_B = 1;
+        }
+    
+        if(flag_c_A == 1)
+        {
+            if((flag_c_B != 1) && (Snum < 9))
+            {
+                Sget[Snum] = c;    
+            }
+            
+            Snum++;
+            
+            if((flag_c_B == 1) && (Snum != 10))
+            {                        
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum   = 0;     
+            }
+            
+            if((flag_c_B == 1) && (Snum == 10))
+            {
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum   = 0;
+                
+                for(unsigned int i = 0; i < 9; i++)
+                {
+                    Suse[i] = Sget[i];
+                }  
+                
+                command_control_flag = 1;                                                  // 命令帧接收成功
+                
+//                command.printf("%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]);
+            }
+        }
+    }       
+}
+
+
+void command_control()
+{
+    SP_knee = 10*(Suse[2] - '0') + 1*(Suse[3] - '0') + 0.1*(Suse[4] - '0');
+    SP_ankle = 10*(Suse[6] - '0') + 1*(Suse[7] - '0') + 0.1*(Suse[8] - '0');
+    if(Suse[1] == '+')
+        SP_knee = 0 + SP_knee;
+    if(Suse[1] == '-')
+        SP_knee = 0 - SP_knee;        
+    if(Suse[5] == '+')
+        SP_ankle = 0 + SP_ankle;
+    if(Suse[5] == '-')
+        SP_ankle = 0 - SP_ankle;
+    
+    wait_us(200); 
+    
+      
+    switch(Suse[0])
+    {
+        case('e'):
+            command.printf("\n\rExiting motor mode\r");
+            ExitMotorMode(&knee_txMsg);
+            ExitMotorMode(&ankle_txMsg);
+            c_lock = 2;                                                         // 电机位置锁无效
+            send_enable = 0;                                                    // main不发送CAN位置命令
+            break;
+        case('m'):
+            command.printf("\n\rCommand motor mode\r");                         //Am+010+010B  旋转1rad
+            c_lock = 1;  
+            send_enable = 1;                                                    // main发送CAN位置命令
+            break;
+        case('M'):
+            command.printf("\n\rBOARD motor mode\r");
+            c_lock = 0;                                                         // 电机位置解锁
+            send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定 
+            break;     
+    }   
+    
+    // 电机模式命令在此发送，锁定控制模式的位置量在main中发送   运行到此处 进入/退出/设置零点位置 命令发送完成                 
+    // 解除串口控制，缓冲区清0
+    wait_ms(2);
+    
+    command_control_flag = 0;                                                   //避免下次继续进入command_control函数
+
+    for(unsigned int i = 0; i < 9; i++)
+    {
+        Suse[i] = 0;      
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

diff -r 000000000000 -r 6aa5928be32a DATA_COMMAND/data_command.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.h	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,56 @@
+#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 SP_ankle, SP_knee; 
+extern int c_lock; 
+
+
+void serial_command_isr();
+void command_control();
+
+
+
+
+
+
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

diff -r 000000000000 -r 6aa5928be32a DATA_PC/data_pc.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_PC/data_pc.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,78 @@
+#include "data_pc.h"
+#include "data_command.h"
+
+
+
+//  PC 运动状态---》假肢主控
+
+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 = 8;
+/*
+ * 1---坐           2---起
+ * 3---平地         4---上坡
+ * 5---下坡         6---上楼 
+ * 7---下楼         8---快速
+ */
+
+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

diff -r 000000000000 -r 6aa5928be32a DATA_PC/data_pc.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_PC/data_pc.h	Fri Jun 18 07:17:12 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

diff -r 000000000000 -r 6aa5928be32a LEG_MESSAGE/leg_message.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/leg_message.h	Fri Jun 18 07:17:12 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

diff -r 000000000000 -r 6aa5928be32a LEG_MESSAGE/used_leg_message.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/used_leg_message.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,24 @@
+#include "used_leg_message.h"
+
+
+
+
+leg_control a_control;
+leg_state   a_state;
+
+
+cal_command_t   cal_command; 
+
+
+
+
+
+
+
+
+
+
+
+
+
+

diff -r 000000000000 -r 6aa5928be32a LEG_MESSAGE/used_leg_message.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/used_leg_message.h	Fri Jun 18 07:17:12 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

diff -r 000000000000 -r 6aa5928be32a MODE/mode.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODE/mode.cpp	Fri Jun 18 07:17:12 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();
+}

diff -r 000000000000 -r 6aa5928be32a MODE/mode.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODE/mode.h	Fri Jun 18 07:17:12 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

diff -r 000000000000 -r 6aa5928be32a TIMER/timer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TIMER/timer.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,9 @@
+#include "timer.h"
+
+
+
+Timer fuzzytimer;
+uint64_t Tfuzzy = 0;
+
+Timer tim;
+uint64_t tim_time = 0;
\ No newline at end of file

diff -r 000000000000 -r 6aa5928be32a TIMER/timer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TIMER/timer.h	Fri Jun 18 07:17:12 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;
+extern uint64_t tim_time;
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 6aa5928be32a fuzzy/fuzzy.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fuzzy/fuzzy.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,97 @@
+/************************************************************************************
+//CSDN 副本2 E 0.3 EC 0.3 Kp 3 Kd 0.3
+*************************************************************************************/
+#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                     */
+     /*  -0.3,   -0.2,     -0.1,    0.0,     0.1,     0.2,     0.3 */
+/*-0.3*/2.680, 2.680,   2.000,  2.000,  1.000,  0.000,  0.000,
+/*-0.2*/2.680, 2.680,   2.000,  1.087,  1.000, -0.086, -0.914,
+/*-0.1*/2.000, 2.000,   2.000,  1.000,  0.000, -1.000, -1.000,
+/* 0.0*/2.000, 2.000,   1.000,  0.000, -1.000, -2.000, -2.000,
+/* 0.1*/1.000, 1.000,   0.000, -1.000, -1.000, -2.000, -2.000,
+/* 0.2*/0.914, 0.086,  -1.087, -2.000, -2.000, -2.003, -2.680,
+/* 0.3*/0.000, 0.000,  -2.000, -2.000, -2.000, -2.680, -2.680
+};
+
+const float FuzzyRuleKd[7][7]={
+    /*                  EC                     */
+     /*  -0.3,   -0.2,   -0.1,    0.0,    0.1,    0.2,   0.3 */
+/*-0.3*/0.1000, -0.0784, -0.2680, -0.2680, -0.2680, -0.1675, 0.1000,
+/*-0.2*/0.1000, -0.0784, -0.2680, -0.2003, -0.2003, -0.0870, 0.0086,
+/*-0.1*/0.0000, -0.0914, -0.1999, -0.2000, -0.1000, -0.0914, 0.0000,
+/* 0.0*/0.0000, -0.0914, -0.1000, -0.1000, -0.1000, -0.0914, 0.0000,
+/* 0.1*/0.0000,  0.0000,  0.0000,  0.0000,  0.0000,  0.0000, 0.0000,
+/* 0.2*/0.2680, -0.0664,  0.1087,  0.1087,  0.1000,  0.1093, 0.2680,
+/* 0.3*/0.2680,  0.2003,  0.2000,  0.2000,  0.1000,  0.1093, 0.2680
+};
+
+
+void fuzzy_init(fuzzy_control_t *fuzzy ,float m_e,float m_ec,float gainKp, float gainKd)
+{
+    fuzzy->quantied_e = m_e;
+    fuzzy->quantied_ec = m_ec;
+    fuzzy->quantied_gainKp = gainKp;
+    fuzzy->quantied_gainKd = gainKd;
+    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,-0.3f,0.3f);
+     ec = range(ec,-0.3f,0.3f);
+     etemp = e > 0.3f ? 0.0f : (e < - 0.3f ? 0.0f : (e >= 0.0f ? (e >= 0.2f ? 0.25f: (e >= 0.1f ? 0.15f : 0.05f)) : (e >= -0.1f ? -0.05f : (e >= -0.2f ? -0.15f : (e >= -0.3f ? -0.25f : 0.0f)))));
+     eLeftIndex = (int)((etemp-0.05f)*10 + 3.0f);        
+     eRightIndex = (int)((etemp+0.05f)*10 + 3.0f);
+     eLefttemp =etemp == 0.0f ? 0.0f:(10*((etemp+0.05f)-e));             
+     eRighttemp=etemp == 0.0f ? 0.0f:( 10*(e-(etemp-0.05f)));
+     
+     ectemp = ec > 0.3f ? 0.0f : (ec < - 0.3f ? 0.0f : (ec >= 0.0f ? (ec >= 0.2f ? 0.25f: (ec >= 0.1f ? 0.15f : 0.05f)) : (ec >= -0.1f ? -0.05f : (ec >= -0.2f ? -0.15f : (ec >= -0.3f ? -0.25f : 0.0f)))));
+     ecLeftIndex = (int)((ectemp-0.05f)*10 + 3.0f);        
+     ecRightIndex = (int)((ectemp+0.05f)*10 + 3.0f);
+     ecLefttemp =ectemp == 0.0f ? 0.0f:(10*((ectemp+0.05f)-ec));    
+     ecRighttemp=ectemp == 0.0f ? 0.0f:( 10*(ec-(ectemp-0.05f)));
+
+
+/*************************************反模糊*************************************/
+
+
+
+
+    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_gainKp;
+    fuzzy->outKd = fuzzy->outKd * fuzzy->quantied_gainKd;
+}

diff -r 000000000000 -r 6aa5928be32a fuzzy/fuzzy.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fuzzy/fuzzy.h	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,29 @@
+#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))
+
+
+extern const float FuzzyRuleKp[7][7];
+extern const float FuzzyRuleKd[7][7];
+
+
+typedef struct{
+float quantied_e;
+float quantied_ec;
+float quantied_gainKp;
+float quantied_gainKd;
+float outKp;
+float outKd;
+}fuzzy_control_t;
+void fuzzy_init(fuzzy_control_t *fuzzy ,float m_e,float m_ec,float gainKp, float gainKd);
+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

diff -r 000000000000 -r 6aa5928be32a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,128 @@
+#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 ankle_real_p = 0.0f;
+float ankle_real_v = 0.0f;
+float ankle_real_t = 0.0f;
+float knee_real_p = 0.0f;
+float knee_real_v = 0.0f;
+float knee_real_t = 0.0f;
+
+
+float count=0.0f;
+float result_knee=0.0f;
+float result_ankle=0.0f;
+
+
+
+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(3);
+    EnterMotorMode(&knee_txMsg);
+    EnterMotorMode(&ankle_txMsg);
+    Zero(&knee_txMsg);
+    Zero(&ankle_txMsg);
+
+    NVIC_SetPriority(USART1_IRQn, 1);                                           // command中断优先级高于board
+    NVIC_SetPriority(USART3_IRQn, 3);
+
+    fuzzy_init(&ankleFuzzy, 1.0f, 1.0f, 0.14f, 0.14f);
+    fuzzy_init(&kneeFuzzy, 1.0f, 1.0f, 0.14f, 0.14f);
+
+    fuzzytimer.start();
+    tim.start();
+
+    while(1) {
+
+        ankle_can.read(ankle_rxMsg);
+        unpack_reply(ankle_rxMsg, &a_state);
+        wait_us(10);
+        knee_can.read(knee_rxMsg);
+        unpack_reply(knee_rxMsg, &a_state);
+
+        ankle_real_p = a_state.ankle_state.p;                                   // 从CAN获得的当前位置
+        ankle_real_v = a_state.ankle_state.v;
+        ankle_real_t = a_state.ankle_state.t;
+
+        knee_real_p = a_state.ankle_state.p;                                    // 从CAN获得的当前位置
+        knee_real_v = a_state.ankle_state.v;
+        knee_real_t = a_state.ankle_state.t;
+
+        control(ankle_real_p, knee_real_p);
+
+        if(send_enable == 1) {
+            PackAll();
+            WriteAll();
+////////////////////////////////////////////////////////////////////////////////
+//                            状态输出 与labview配套                           //
+////////////////////////////////////////////////////////////////////////////////
+            command.printf("AB");
+            if(a_state.knee_state.p >= 0)
+                command.printf("+%.3f\t",a_state.knee_state.p);
+            else
+                command.printf("%.3f\t",a_state.knee_state.p);
+            
+            if(a_control.knee.p_des >= 0)
+                command.printf("+%.3f\t",a_control.knee.p_des);
+            else
+                command.printf("%.3f\t",a_control.knee.p_des);
+            
+            if(a_state.knee_state.t >= 0)
+                command.printf("+%.3f\t",a_state.knee_state.t);
+            else
+                command.printf("%.3f\t",a_state.knee_state.t);
+            
+            if(a_state.ankle_state.p >= 0)
+                command.printf("+%.3f\t",a_state.ankle_state.p);
+            else
+                command.printf("%.3f\t",a_state.ankle_state.p);
+            
+            if(a_control.ankle.p_des >= 0)
+                command.printf("+%.3f\t",a_control.ankle.p_des);
+            else
+                command.printf("%.3f\t",a_control.ankle.p_des);
+                 
+            if(a_state.ankle_state.t >= 0)
+                command.printf("+%.3f\n",a_state.ankle_state.t);
+            else
+                command.printf("%.3f\n",a_state.ankle_state.t);    
+            }            
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 6aa5928be32a math_ops/math_ops.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops/math_ops.cpp	Fri Jun 18 07:17:12 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;
+    }

diff -r 000000000000 -r 6aa5928be32a math_ops/math_ops.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops/math_ops.h	Fri Jun 18 07:17:12 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

diff -r 000000000000 -r 6aa5928be32a mbed-dev.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dev.lib	Fri Jun 18 07:17:12 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/benkatz/code/mbed-dev_spine/#97f825502e2a