Revision 0:e923de71caa5, committed 2022-06-24

Comitter:

yezhong

Date:

Fri Jun 24 07:58:26 2022 +0000

Commit message:

111111

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,130 @@
+#include "CAN.h"
+#include "used_leg_message.h"
+#include "CAN3.h"
+#include "mcp2515.h"
+
+
+
+//定义设备
+CAN pf_can(PB_8, PB_9);                                                         // CAN Rx pin name, CAN Tx pin name
+CAN df_can(PB_12, PB_13);                                                       // CAN Rx pin name, CAN Tx pin name
+// 定义CAN消息
+CANMessage pf_rxMsg, df_rxMsg;                                                  // 主控收到的CAN消息
+CANMessage pf_txMsg, df_txMsg;                                                    //主控发送的CAN消息
+
+SPI spi(PA_7,PA_6,PA_5);// mosi, miso, scl                           //SPI转can
+CAN3 df1_can(spi,PA_4); //spi, ncs, itr
+
+CANMessage df1_txMsg;                                                  // 主控收到的CAN消息
+CANMessage df1_rxMsg;                                                  // 主控发送的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(&pf_txMsg, a_control.pf); 
+    pack_cmd(&df_txMsg, a_control.df); 
+    pack_cmd(&df1_txMsg, a_control.df1); 
+}
+
+
+// 写联合打包的数据
+void WriteAll(){
+    pf_can.write(pf_txMsg);
+    wait(.00002);
+    df_can.write(df_txMsg);
+    wait(.00002);
+    df1_can.write(&df1_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, ankle_state * ankle){
+    /// 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==0x01){
+        ankle->pf.p = p;
+        ankle->pf.v = v;
+        ankle->pf.t = t;
+        }
+    else if(id==0x02){
+        ankle->df.p = p;
+        ankle->df.v = v;
+        ankle->df.t = t;
+        }
+    else if(id==0x03){
+        ankle->df1.p = p;
+        ankle->df1.v = v;
+        ankle->df1.t = t;
+        }
+} 
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.h	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,45 @@
+#ifndef _CAN_H
+#define _CAN_H
+
+// 头文件
+#include "mbed.h"
+#include "math_ops.h"
+#include "leg_message.h"
+#include "used_leg_message.h"
+#include "CAN3.h"
+#include "mcp2515.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 pf_can, df_can;                                                      //设备的外部申明
+extern CANMessage pf_rxMsg, df_rxMsg;                                           // 主控收到的CAN消息
+extern CANMessage pf_txMsg, df_txMsg;                                               // 主控发送的CAN消息
+
+extern CAN3 df1_can;
+extern CANMessage df1_txMsg;
+extern CANMessage df1_rxMsg;  
+
+
+// 函数外部申明
+void pack_cmd(CANMessage * msg, joint_control joint);
+void PackAll();
+void WriteAll();
+void unpack_reply(CANMessage msg, ankle_state * ankle);
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,26 @@
+#include "control.h"
+
+
+void control()
+{
+   
+    
+    
+         
+    
+    
+}
+  
+    
+
+
+
+
+
+
+      
+        
+        
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.h	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,20 @@
+#ifndef _CONTROL_H
+#define _CONTROL_H
+
+
+#include "mbed.h"
+#include "CAN.h"
+#include "mode.h"
+#include "data_xiedian.h"
+#include "data_wifi.h"
+#include "used_leg_message.h"
+
+
+
+
+void control();
+
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_PC/data_pc.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,65 @@
+#include "data_pc.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 phase_flag_get[1] = {0};
+uint16_t phase_flag_use[1] = {0};
+unsigned int phase_flag = 0;
+
+void pc_decode(){
+    phase_flag = phase_flag_use[0];
+}
+
+void pc_clear(){
+    phase_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;
+            phase_flag_get[0] = 0;
+            break; 
+        }
+        if(c == 'B'){
+            flag_B = 1;
+        }
+        if(flag_A == 1){
+            if((flag_B != 1) && (pc_num < 1))
+            {
+                phase_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; 
+                
+                phase_flag_use[0] = phase_flag_get[0] - '0'; 
+                pc_decode();
+                
+                pc.printf("%d", phase_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	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,20 @@
+#ifndef _DATA_PC_H
+#define _DATA_PC_H
+
+#include "mbed.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 phase_flag_get[1];
+extern uint16_t phase_flag_use[1];
+extern unsigned int phase_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/DATA_wifi/data_wifi.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,135 @@
+#include "mbed.h"
+#include "data_wifi.h"
+#include "data_pc.h"
+#include "cJSON.h"
+#include "esp8266.h"
+ 
+
+
+Timer ttt;
+
+char RX[33],RXX[33];
+unsigned int m=0;
+
+
+
+void wifi_decode()
+{
+    double zitai_date;
+    float bbb;
+    ttt.start();
+    ttt.read(); 
+    bbb=ttt.read(); 
+      cJSON *root = cJSON_Parse(RXX);
+    if(!root)                        
+    {
+        printf("Error in cjosn: [%s]\n",cJSON_GetErrorPtr());
+    }
+    else
+    {        
+           
+         cJSON *temp1 = cJSON_GetObjectItem(root,"J");  
+         cJSON *temp = cJSON_GetObjectItem(temp1,"a");
+            
+        if(temp != NULL)                                        
+        {
+            if(temp->type == cJSON_Number)                       
+            {
+               zitai_date = temp->valuedouble;                              //double型    valuedoule
+            }
+        }
+                
+    }
+
+    cJSON_Delete(root);       
+       
+     //pc.printf("%.4f,%.3f\r\n",zitai_date,bbb);
+     pc.printf("zitai %.4f\r\n",zitai_date);
+}
+
+void wifi_clear()
+{
+    for(int i = 0; i <m+1; i++){
+        RXX[i] = 0;
+    }
+    m=0;
+}
+
+
+void serial_wifi_isr(void)
+{
+    static unsigned int A=0,B=0,z=0,n=0,b=0;
+    while(wifi.readable())
+    {                 
+        char c=wifi.getc();
+         //pc.putc(c);
+         if((c=='{')&&(b<2))
+        {
+            A = 1;
+            B = 0;
+            z= 0;
+            n=0;
+            for(unsigned int i=0;i<33;i++)
+            {
+                RX[i]=0;      
+            }
+        }
+        if((c == '}')&&(n!=1))
+        {
+            B = 1;
+            RX[z] = '}';
+        }   
+        if(A==1)
+        {
+            
+            if(B!=1)
+            {
+                RX[z] = c;   
+            } 
+              z++;
+              b++;
+              m=z;
+            if(B == 1)
+            {
+                A = 0;
+                B= 0;
+                b=0;
+                RX[z] = '}';
+                z= 0;
+                n=1;
+                for(unsigned int i = 0; i<m+1; i++)
+                {
+                   RXX[i] = RX[i] ;
+                   //pc.printf("%c",RXX[i]);
+                }
+                if((RXX[2]=='J')&&(RXX[8]=='a'))
+                {
+                wifi_decode();
+                }              
+                wifi_clear(); 
+            } 
+        }
+        
+    }
+}
+
+
+
+
+
+
+
+
+
+
+AnalogIn    LaLi_pf(PC_1);
+AnalogIn    LaLi_df(PC_2);
+AnalogIn    LaLi_df1(PC_3);
+
+float La_pf_real = 0.0f, La_df_real = 0.0f,La_df1_real = 0.0f;
+float Ffilter_pf[10 + 2] = {0.0f};
+float pf_filter = 0.0f;
+float F_pf = 0.0f;
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_wifi/data_wifi.h	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,50 @@
+#ifndef _DATA_BOARD_H
+#define _DATA_BOARD_H
+
+
+#include "mbed.h"
+#include "math_ops.h"
+#include "wifi_example.h"
+
+
+
+extern char RX[33],RXX[33];
+extern unsigned int m;
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+void serial_wifi_isr();
+void wifi_decode();
+void wifi_clear();
+
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+extern AnalogIn    LaLi_pf;
+extern AnalogIn    LaLi_df;
+extern AnalogIn    LaLi_df1;
+
+extern float La_pf_real, La_df_real,La_df1_real;
+extern float Ffilter_pf[10 + 2];
+extern float pf_filter ;
+extern float F_pf;
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_xiedian/data_xiedian.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,150 @@
+#include "data_xiedian.h"
+#include "data_pc.h"
+#include "CAN.h"
+
+Serial      xiedian(PC_10, PC_11);
+
+
+
+
+
+unsigned int flag_foot_A = 0, flag_foot_B = 0;
+unsigned int Sfootnum = 0;
+
+uint8_t Sfootget[45] = {0};                                                    
+uint8_t Sfootuse[45] = {0};
+
+uint16_t shou[28]={0};
+float shou1[28]={0};
+Timer vv;
+
+
+
+
+
+void xiedian_decode()
+{    
+      vv.start();
+      vv.read(); 
+    float  b=vv.read(); 
+/*
+    for(int i = 0; i < 6; i++)
+    {
+    shou[i]=(Sfootuse[2*i]<<8)|Sfootuse[2*i+1];
+    }
+    for(int i = 0; i < 11; i++)
+    {
+    shou[2*i+6]=(Sfootuse[3*i+12]<<4)|(Sfootuse[3*i+13]>>4);
+    shou[2*i+7]=((Sfootuse[3*i+13]<<8)&0xFFF)|Sfootuse[3*i+14];
+    }
+    
+    for(int i = 0; i < 28; i++)
+    {
+    //shou1[i]=(float)shou[i]/4096*3.3;
+    shou1[i]=4096-shou[i];
+    }        
+    pc.printf("%2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %2.2f %.3f\r\n",shou1[6],shou1[7],shou1[8],shou1[9],shou1[10],shou1[11],shou1[12],shou1[13],shou1[14],shou1[15],shou1[16],shou1[17],shou1[18],shou1[19],shou1[20],shou1[21],shou1[22],shou1[23],shou1[24],shou1[25],shou1[26],b);
+*/
+   for(int i = 0; i < 2; i++)
+    {
+    shou[2*i]=(Sfootuse[3*i]<<4)|(Sfootuse[3*i+1]>>4);
+    shou[2*i+1]=((Sfootuse[3*i+1]<<8)&0xFFF)|Sfootuse[3*i+2];
+    }
+   for(int i = 0; i < 4; i++)
+    {
+    shou1[i]=4096-shou[i];
+    }        
+   // pc.printf("%2.2f %2.2f %2.2f %2.2f %0.3f\r\n",shou1[0],shou1[1],shou1[2],shou1[3],b);
+     pc.printf("xiedian:%2.2f %2.2f %2.2f %2.2f\r\n",shou1[0],shou1[1],shou1[2],shou1[3]);
+}
+
+
+void xiedian_clear()
+{
+    //for(int i = 0; i < 45; i++)
+    for(int i = 0; i < 6; i++)
+    {
+        Sfootuse[i] = 0;
+    }
+}
+
+
+void serial_xiedian_isr(void)
+{
+   // pc.printf("begin\n");
+   
+    while(xiedian.readable())
+    {       
+        uint8_t c = xiedian.getc();
+        if(c ==0x0A)
+        {
+            flag_foot_A = 1;
+            flag_foot_B = 0;
+            Sfootnum = 0;
+            //for(unsigned int i = 0; i < 45; i++)
+            for(unsigned int i = 0; i < 6; i++)
+            {
+                Sfootget[i] = 0;      
+            }
+            
+            break;                               //直接结束；回到起始位置；
+        }
+        if(c ==0x0B)
+        {
+            flag_foot_B = 1;
+        }
+        
+        if(flag_foot_A == 1)
+        {
+            //if((flag_foot_B != 1) && (Sfootnum < 45))
+            if((flag_foot_B != 1) && (Sfootnum < 6))
+            {
+                Sfootget[Sfootnum] = c;    
+            }
+            
+            Sfootnum++;
+            
+            //if((flag_foot_B == 1) && (Sfootnum != 46))
+            if((flag_foot_B == 1) && (Sfootnum != 7))
+            {    
+                flag_foot_A = 0;
+                flag_foot_B = 0;
+                Sfootnum    = 0;     
+            }
+            
+            //if((flag_foot_B == 1) && (Sfootnum == 46))
+            if((flag_foot_B == 1) && (Sfootnum == 7))
+            {
+                flag_foot_A = 0;
+                flag_foot_B = 0;
+                Sfootnum    = 0;
+                
+              //  for(unsigned int i = 0; i < 45; i++)
+                for(unsigned int i = 0; i < 6; i++)
+                {
+         //           Sfootuse[i] = Sfootget[i] - '0';
+                      Sfootuse[i] = Sfootget[i] ;
+                }
+                
+                xiedian_decode();
+                xiedian_clear(); 
+            } 
+        }
+    }
+    
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_xiedian/data_xiedian.h	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,70 @@
+#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      xiedian;
+
+
+
+extern unsigned int flag_foot_A, flag_foot_B;
+extern unsigned int Sfootnum;
+extern uint8_t Sfootget[45];                                                    
+extern uint8_t Sfootuse[45];
+extern uint16_t shou[28];
+extern float shou1[28];
+
+
+
+
+
+
+
+void xiedian_decode();
+void xiedian_clear();
+
+
+
+void serial_xiedian_isr();
+
+
+
+
+
+
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/leg_message.h	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,60 @@
+#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 ankle_control{                                                           // 系统控制结构体
+    joint_control pf, df,df1;                                                       // 控制成员: pf, df关节
+    };
+ 
+struct joint_state{                                                             // 关节状态结构体
+    float p, v, t;                                                              // 状态成员:  p\v\t
+    };
+    
+struct ankle_state{                                                             // 系统状态结构体
+    joint_state pf, df,df1;                                                         // 状态成员: pf, df状态
+    };
+
+
+//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_pf;
+//    float q_des_df;
+//
+//    float qd_des_pf;
+//    float qd_des_df;
+//
+//    float kp_pf;
+//    float kp_df;
+//
+//    float kd_pf;
+//    float kd_df;
+//
+//    float tau_pf_ff;
+//    float tau_df_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	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,24 @@
+#include "used_leg_message.h"
+
+
+
+
+ankle_state     a_state;
+ankle_control   a_control;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/used_leg_message.h	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,28 @@
+#ifndef _USED_LEG_MESSAGE_H
+#define _USED_LEG_MESSAGE_H
+
+
+
+#include "mbed.h"
+#include "leg_message.h"
+
+
+
+
+
+
+
+
+
+extern ankle_state     a_state;
+extern ankle_control   a_control;
+
+
+
+
+
+
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MA/Moving_Average.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,40 @@
+#include "mbed.h"
+#include "Moving_Average.h"
+
+
+
+// Source是不断变化的
+float Moving_Average(unsigned int NN, float *FILTER, float Source)
+{
+    
+    float AVER = 0.0f; 
+    float SUM = 0; 
+    unsigned char i;
+    
+    
+    FILTER[NN] = Source;    // 将接受到的数据放在均值滤波数组的“最后（倒数第2个）”元素里面
+
+    
+    // 把第一次进入的 Source 赋值给 FILTER[0] ~ FILTER[N-1]
+    if(FILTER[NN+1] == 0)   // FILTER[N+1]等于0，表示第一次进入该函数
+    {
+        for(i = 0; i < NN; i++)
+        {
+            FILTER[i] = Source;
+        }
+
+        FILTER[NN+1] = 1;   // 设置标记，已经进入过该函数，以后不再全部赋值   
+    }
+    
+
+    for(i = 0; i < NN; i++)
+    {
+        FILTER[i] = FILTER[i + 1];
+        SUM += FILTER[i];
+    }
+    
+    AVER = SUM / NN;
+    SUM = 0;
+
+    return AVER;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MA/Moving_Average.h	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,9 @@
+#ifndef __Moving_Average_H
+#define __Moving_Average_H
+
+
+float  Moving_Average(unsigned int NN, float *FILTER, float Source);
+
+
+
+#endif /* __Moving_Average_H */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODE/mode.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,69 @@
+#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();
+}
+// 进入速度模式
+void EnterSPEEDMode(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] = 0xFA;
+    WriteAll();
+}
+// 进入位置模式
+void EnterPositionMode(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] = 0xFB;
+    WriteAll();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODE/mode.h	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,24 @@
+#ifndef _MODE_H
+#define _MODE_H
+
+
+#include "mbed.h"
+
+
+void Zero(CANMessage * msg);
+void EnterMotorMode(CANMessage * msg);
+void ExitMotorMode(CANMessage * msg);
+void EnterSPEEDMode(CANMessage * msg);
+void EnterPositionMode(CANMessage * msg);
+
+
+
+
+
+
+
+
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/cJSON_lib.lib	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/TaylorGy/code/cJSON_lib/#c809010834e2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/esp8266.lib	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/yezhong/code/esp8266/#e8b9ee1156c8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,300 @@
+#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_xiedian.h"
+#include "mode.h"
+#include "data_wifi.h"
+#include "Moving_Average.h"
+#include "wifi_example.h"
+#include "esp8266.h"
+
+
+#include "CAN3.h"
+#include "mcp2515.h"
+#include <sstream>
+#include <algorithm>
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                             框架搭建完毕                                     //
+////////////////////////////////////////////////////////////////////////////////
+ 
+
+
+int main()
+{
+     Timer t;
+     //float a=PI/8;
+    //float j=0.558,P=0;
+   
+    
+////////////////////////初始化//////////////////////////////////////
+    pc.baud(115200);                                           //串口打印信息  U2
+    //pc.attach(&serial_pc_isr);
+
+
+
+       xiedian.baud(115200);                                      //   U3
+       xiedian.attach(&serial_xiedian_isr);
+    
+     
+
+     wifi.baud(115200);             
+    // connectInit();                                //U1 wifi接收姿态传感器
+     wifi.attach(&serial_wifi_isr);
+      
+   
+    
+    pf_can.frequency(800000);
+    pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    pf_rxMsg.len = 6;
+    pf_txMsg.len   = 8;
+    pf_txMsg.id    = 0x01;
+    
+    df_can.frequency(800000);
+    df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    df_rxMsg.len = 6;
+    df_txMsg.len   = 8;
+    df_txMsg.id    = 0x02;
+    
+    df1_rxMsg.len = 6;
+    df1_txMsg.len =8;
+    df1_txMsg.id =0x03;
+    df1_can.frequency(800000);
+    
+    NVIC_SetPriority(USART1_IRQn, 3);                                           // command中断优先级高于board
+    NVIC_SetPriority(USART2_IRQn, 1);  
+    //NVIC_SetPriority(USART3_IRQn, 3);
+    
+    
+    
+/////////////////////////////////////position///////////////////////////////////////////
+    //wait(8);
+     Zero(&pf_txMsg);
+     // EnterSPEEDMode(&pf_txMsg);
+      
+     //Zero(&df_txMsg);
+     // EnterSPEEDMode(&df_txMsg);
+      
+      Zero(&df1_txMsg);
+      EnterSPEEDMode(&df1_txMsg);
+    
+    EnterPositionMode(&pf_txMsg);
+    //EnterPositionMode(&df_txMsg);  
+    EnterPositionMode(&df1_txMsg);
+   
+      
+    //EnterMotorMode(&pf_txMsg);
+    //EnterMotorMode(&df_txMsg);
+      //EnterMotorMode(&df1_txMsg);
+    
+
+           
+
+    while(1) {
+      
+      
+        pf_can.read(pf_rxMsg);                                                                            
+        unpack_reply(pf_rxMsg, &a_state);
+        wait_us(10);
+        df_can.read(df_rxMsg);
+        unpack_reply(df_rxMsg, &a_state);
+        wait_us(10);
+        df1_can.read(&df1_rxMsg);
+        unpack_reply(df1_rxMsg, &a_state);
+
+        float pfp = a_state.pf.p;                                                     // 从CAN获得的当前位置
+        float pfv = a_state.pf.v;
+        float pft = a_state.pf.t;
+
+        float dfp = a_state.df.p;
+        float dfv = a_state.df.v;
+        float dft = a_state.df.t;
+        
+        float df1p = a_state.df1.p;
+        float df1v = a_state.df1.v;
+        float df1t = a_state.df1.t;
+    
+    
+    
+       ///////////////////////pf拉力////////////////////////// 
+       La_pf_real = LaLi_pf.read()*3.3f*1000;       //读取电压值；单位为mV;
+       pf_filter = Moving_Average(10, Ffilter_pf, La_pf_real);
+       F_pf=0.1125f*pf_filter+2.141f;
+       //F_pf=0.1139*pf_filter;
+       La_df_real = LaLi_df.read();
+       La_df1_real = LaLi_df1.read();
+
+        //pc.printf("%.3fa%.3f\r\n",F_pf,pfp);     //拉力
+         
+               
+        //pc.printf("%.3f\r\n",dfp);
+         //wait(0.1);
+       // pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft);
+       // pc.printf("%.4f,%.3f,%.3f,%.4f,%.3f,%.4f\n",pfkp,pfkv,pfkt,a_control.pf.p_des,j,P);
+       //pc.printf("%.3fa%.3fa%.3fa%.3fa%.3fa%.3f\n\r",pfv,dfv,df1v,a_control.pf.v_des,a_control.df.v_des,a_control.df1.v_des); 
+       //pc.printf("%.3fa%.3f\n\r",df1p,a_control.df1.p_des); 
+ /////////////////////////////////////////////////trajectory/////////////////////////////////////////////////////
+     /* 
+        j=j+0.001;
+        P=0.33+(-0.2148)*cos((j+0.1)*10.44)+(-0.1549)*sin((j+0.1)*10.44)+(-0.07744)*cos(2*(j+0.1)*10.44)+(-0.04255)*sin(2*(j+0.1)*10.44);       
+        if(j>=0.558&&j<=0.85)
+        {
+             a_control.pf.p_des=P;
+             a_control.pf.v_des=0; 
+             a_control.pf.kp=80;
+             a_control.pf.kd=0;
+             a_control.pf.t_ff=0;
+        }
+       
+        
+         PackAll();
+        WriteAll();
+     */
+        t.start();
+        t.read(); 
+    /*
+      
+       if(t.read()<2)
+       {  
+        a_control.pf.p_des=-PI/32;
+        a_control.pf.v_des=0;
+        a_control.pf.kp=10;
+        a_control.pf.kd=5;
+        a_control.pf.t_ff=0;
+       }
+       if(t.read()>2&&t.read()<4)
+        {  
+        a_control.pf.p_des=0;
+        a_control.pf.v_des=0;
+        a_control.pf.kp=10;
+        a_control.pf.kd=5;
+        a_control.pf.t_ff=0;
+       }
+        */
+        a_control.pf.p_des=PI/32;
+        a_control.pf.v_des=0;
+        a_control.pf.kp=10;
+        a_control.pf.kd=5;
+        a_control.pf.t_ff=0;
+        
+        
+        
+        a_control.df.p_des=PI/4;
+        a_control.df.v_des=0;
+        a_control.df.kp=10;
+        a_control.df.kd=5;
+        a_control.df.t_ff=0;
+        
+        if(t.read()<2)
+        {
+        a_control.df1.p_des=PI/4;
+        a_control.df1.v_des=0;
+        a_control.df1.kp=30;
+        a_control.df1.kd=5;
+        a_control.df1.t_ff=0;
+        }
+        
+        if(t.read()>2&&t.read()<4)
+        {
+        a_control.df1.p_des=PI/8;
+        a_control.df1.v_des=0;
+        a_control.df1.kp=35;
+        a_control.df1.kd=5;
+        a_control.df1.t_ff=0;
+        }
+    /*    
+        if(t.read()>4&&t.read()<6)
+        {
+        t.reset();
+        }
+     */   
+        
+        PackAll();
+        WriteAll();
+
+
+
+
+
+
+
+
+        
+/////////////////////////////////////position///////////////////////////////////////////        
+     /*
+        a_control.pf.p_des=PI/8;
+        a_control.pf.v_des=0;
+        a_control.pf.kp=10;
+        a_control.pf.kd=0;
+        a_control.pf.t_ff=0; 
+        PackAll();
+        WriteAll();
+    */
+        
+    /*
+     t.start();
+     t.read(); 
+     
+       if(t.read()<2)        
+          {
+          a_control.pf.p_des=a;
+          a_control.pf.v_des=0;
+          a_control.pf.kp=10;
+          a_control.pf.kd=0;
+          a_control.pf.t_ff=0;  
+          }                 
+     if(t.read()>3) 
+     {
+          t.reset();
+          Zero(&PF_can);
+          a=-a;
+         }   
+    
+        PackAll();
+        WriteAll();
+        
+        */
+        
+  
+          
+        
+ /////////////////////////////////////////////////////////////////////////////////////////////       
+
+///////////////////////////////////////velocity///////////////////////////////////////////////
+  /*
+        t.start();
+        t.read(); 
+
+
+        a_control.pf.p_des=0;
+        a_control.pf.v_des=-500*2*3.14/60/50;
+        a_control.pf.kp=10;
+        a_control.pf.kd=5;
+        a_control.pf.t_ff=0;
+
+        a_control.df.p_des=0;
+        a_control.df.v_des=-500*2*3.14/60/50;
+        a_control.df.kp=10;
+        a_control.df.kd=5;
+        a_control.df.t_ff=0;
+        
+        a_control.df1.p_des=0;
+        a_control.df1.v_des=500*2*3.14/60/50;
+        //a_control.df1.v_des=(500*2*3.14/60/50)*sin(0.5f*t);
+        a_control.df1.kp=10;
+        a_control.df1.kd=5;
+        a_control.df1.t_ff=0;
+        PackAll();
+        WriteAll();       
+*/
+////////////////////////////////////////////////////////////////////////////////////////////////
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/math_ops/math_ops.cpp	Fri Jun 24 07:58:26 2022 +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	Fri Jun 24 07:58:26 2022 +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	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/benkatz/code/mbed-dev-f303/#36facd806e4a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mcp2515.lib	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/yezhong/code/mcp2515-/#c0b402665e50

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wifi_example.lib	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/yezhong/code/yezhong_main_controller_copy_3/#ff4d7f4f379c