Revision 0:d80c66cb1b3a, committed 2020-11-10

Comitter:

panzhan

Date:

Tue Nov 10 09:09:58 2020 +0000

Child:

1:a71791b81b8a

Commit message:

panzhan_main_controller_copy2

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.cpp	Tue Nov 10 09:09:58 2020 +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==0x01){
+        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	Tue Nov 10 09:09:58 2020 +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/control.cpp	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,65 @@
+#include "control.h"
+
+
+void control()
+{
+    if(command_control_flag == 1)
+        command_control();
+    
+    if(return_zero == 1)
+    {
+//        gangduceshi();
+    }
+         
+    if(c_lock == 1)                                                             // 处于PC串口锁定模式
+    {
+//        cal_command.q_des_pf  = SP_pf;
+//        cal_command.qd_des_pf = 0.0f;
+//        cal_command.kp_pf     = 200.0f;
+//        cal_command.kd_pf     = 0.0f;
+//        cal_command.tau_pf_ff = 0.0f;
+//        
+//        cal_command.q_des_df  = SP_df;
+//        cal_command.qd_des_df = 0.0f;
+//        cal_command.kp_df     = 200.0f;
+//        cal_command.kd_df     = 0.0f;
+//        cal_command.tau_df_ff = 0.0f;
+    }
+    
+    if(c_lock == 0)
+    {
+        //calculate_pf_kh();
+//        calculate_pf_fuzzy();
+        
+        //calculate_df();
+    }
+    
+    // 将计算得到的数值赋给控制器
+    
+//    a_control.pf.p_des = cal_command.q_des_pf;
+//    a_control.pf.v_des = cal_command.qd_des_pf;
+//    a_control.pf.kp    = cal_command.kp_pf / 7.0f;
+//    a_control.pf.kd    = cal_command.kd_pf / 7.0f;
+//    a_control.pf.t_ff  = cal_command.tau_pf_ff / 7.0f;
+//        
+//    a_control.df.p_des = cal_command.q_des_df;
+//    a_control.df.v_des = cal_command.qd_des_df;
+//    a_control.df.kp    = cal_command.kp_df / 10.0f;
+//    a_control.df.kd    = cal_command.kd_df / 10.0f;
+//    a_control.df.t_ff  = cal_command.tau_df_ff / 10.0f;
+    
+}
+  
+    
+
+
+
+
+
+
+      
+        
+        
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.h	Tue Nov 10 09:09:58 2020 +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"
+
+
+
+
+void control();
+
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_BOARD/data_board.cpp	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,113 @@
+#include "mbed.h"
+#include "data_board.h"
+#include "data_pc.h"
+
+Serial foot(PC_10, PC_11);              // tx, rx U3
+
+
+unsigned int flag_485_A = 0, flag_485_B = 0;
+unsigned int S485num = 0;
+
+uint16_t S485get[10] = {0};                                                    
+uint16_t S485use[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;
+float COP_Y = 0.0f, COP_X = 0.0f;
+
+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;
+float COP_Y_real = 0.0f, COP_X_real = 0.0f;
+
+
+void gait_decode()
+{
+    Gait_now = S485use[0];
+    Gait_per_now = S485use[1]*100 + S485use[2]*10 + S485use[3];
+    time_portion_now = S485use[4]*100000 + S485use[5]*10000 + S485use[6]*1000 + S485use[7]*100 + S485use[8]*10 + S485use[9];
+              
+        
+    Gait_num_valid_real = Gait_num_valid;
+    Gait_now_real = Gait_now;
+    Gait_per_now_real = Gait_per_now;
+    COP_Y_real = COP_Y;
+    COP_X_real = COP_X;
+    Gait_cycle_now_real = Gait_cycle_now;
+    time_portion_now_real=time_portion_now;
+    
+    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++){
+        S485use[i] = 0;
+    }
+}
+
+
+void serial_board_isr(void)
+{
+//    pc.printf("begin\n");
+    while(foot.readable())
+    {       
+        uint16_t c = foot.getc();
+        if(c == 'A')
+        {
+            flag_485_A = 1;
+            
+            flag_485_B = 0;
+            S485num = 0;
+            for(unsigned int i = 0; i < 10; i++)
+            {
+                S485get[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 'B')
+        {
+            flag_485_B = 1;
+        }
+        
+        if(flag_485_A == 1)
+        {
+            if((flag_485_B != 1) && (S485num < 10))
+            {
+                S485get[S485num] = c;    
+            }
+            
+            S485num++;
+            
+            if((flag_485_B == 1) && (S485num != 11))
+            {    
+                flag_485_A = 0;
+                flag_485_B = 0;
+                S485num    = 0;     
+            }
+            
+            if((flag_485_B == 1) && (S485num == 11))
+            {
+                flag_485_A = 0;
+                flag_485_B = 0;
+                S485num    = 0;
+                
+                for(unsigned int i = 0; i < 10; i++)
+                {
+                    S485use[i] = S485get[i] - '0';
+                }
+                
+                gait_decode();
+                pc.printf("OK\n");
+                gait_clear(); 
+            } 
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_BOARD/data_board.h	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,54 @@
+#ifndef _DATA_BOARD_H
+#define _DATA_BOARD_H
+
+
+#include "mbed.h"
+#include "math_ops.h"
+
+
+extern Serial      foot;
+
+
+
+//extern unsigned int flag_485_A, flag_485_B;
+//extern unsigned int S485num;
+//extern uint16_t S485get[10];                                                    
+extern uint16_t S485use[10];
+
+////////////////////////////////////////////////////////////////////////////////
+
+extern unsigned int Gait_num_valid, Gait_now, Gait_per_now, Gait_cycle_now;
+extern float COP_Y, COP_X;
+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;
+extern float COP_Y_real, COP_X_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	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,232 @@
+#include "data_command.h"
+#include "data_pc.h"
+#include "CAN.h"
+
+Serial      command(PA_9, PA_10);
+
+
+int enabled = 0;                                                                // 进入电机模式标志位
+int counter = 0;                                                                // 输出计数器
+int command_control_flag = 0;                                                   // 命令帧标志位
+float SP_pf = 0, SP_df = 0;
+int c_lock = 2;                                                                 // 位置锁定标志位 
+int return_zero = 0;                                                            // 电机回0标志位
+
+unsigned int flag_c_A = 0, flag_c_B = 0;
+unsigned int Snum_c = 0;
+uint16_t Sget[10] = {0};                                                    
+uint16_t Suse[10] = {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_c   = 0;
+            for(unsigned int i = 0; i < 10; i++)
+            {
+                Sget[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 'B')
+        {
+            flag_c_B = 1;
+        }
+    
+        if(flag_c_A == 1)
+        {
+            if((flag_c_B != 1) && (Snum_c < 10))
+            {
+                Sget[Snum_c] = c;    
+            }
+            
+            Snum_c++;
+            
+            if((flag_c_B == 1) && (Snum_c != 11))
+            {                        
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum_c   = 0;     
+            }
+            
+            if((flag_c_B == 1) && (Snum_c == 11))
+            {
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum_c   = 0; 
+                
+                for(unsigned int i = 0; i < 10; i++)
+                {
+                    Suse[i] = Sget[i];
+                }  
+                
+                command_control_flag = 1;                                       // 命令帧接收成功
+                
+                pc.printf("%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]);
+            }
+        }
+    }    
+}
+
+
+void command_control()
+{
+    SP_pf = 10*(Suse[2] - '0') + 1*(Suse[3] - '0') + 0.1*(Suse[4] - '0');
+    SP_df = 10*(Suse[7] - '0') + 1*(Suse[8] - '0') + 0.1*(Suse[9] - '0');
+    if(Suse[1] == '+')
+        SP_pf = 0 + SP_pf;
+    if(Suse[1] == '-')
+        SP_pf = 0 - SP_pf;
+    if(Suse[6] == '+')
+        SP_df = 0 + SP_df;
+    if(Suse[6] == '-')
+        SP_df = 0 - SP_df;
+    
+    
+    // 切换为发送模式
+    wait_us(200);
+    
+    switch(Suse[0])
+    {
+        case('e'):
+            command.printf("\n\rPF exiting motor mode\r");
+//          ExitMotorMode(&PF_can);
+            ExitMotorMode(&knee_txMsg);           
+            ExitMotorMode(&ankle_txMsg);
+            return_zero = 0;                                                    // 停止回0
+            c_lock = 2;                                                         // 电机位置锁无效
+//            send_enable = 0;                                                    // main不发送CAN位置命令
+            break;
+        case('m'):
+            command.printf("\n\rPF entering PC motor mode\r");
+//            EnterMotorMode(&PF_can);
+            EnterMotorMode(&knee_txMsg);           
+            EnterMotorMode(&ankle_txMsg);
+
+            return_zero = 0;
+            c_lock = 1;                                                         // 电机位置锁定 
+//            send_enable = 1;                                                    // main发送CAN位置命令
+            break;
+        case('M'):
+            command.printf("\n\rPF entering BOARD motor mode\r");
+//            EnterMotorMode(&PF_can);
+            EnterMotorMode(&knee_txMsg);           
+            EnterMotorMode(&ankle_txMsg);
+            return_zero = 0;
+            c_lock = 0;                                                         // 电机位置解锁
+//            send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定 
+            break;
+        case('z'):
+            command.printf("\n\rPF zeroing\r");
+//            Zero(&PF_can);
+            Zero(&knee_txMsg);
+            Zero(&ankle_txMsg);
+            return_zero = 0; 
+            c_lock = 2;
+//            send_enable = 0;
+            break;
+        case('r'):
+            command.printf("\n\rPF return zero\r");
+//            EnterMotorMode(&PF_can);
+            EnterMotorMode(&knee_txMsg);           
+            EnterMotorMode(&ankle_txMsg);
+            return_zero = 1;
+            c_lock = 2;
+//            send_enable = 1;
+//            flag_gd = 1;
+            break;      
+    }
+    /*
+    switch(Suse[5])
+    {
+        case('e'):
+            command.printf("DF exiting motor mode\n\r");
+            ExitMotorMode(&DF_can);
+            c_lock = 1;                                                         // 电机位置解锁
+            send_enable = 0;                                                    // main不发送CAN位置命令
+            break;
+        case('m'):
+            command.printf("DF entering PC motor mode\n\r");
+            EnterMotorMode(&DF_can);
+            c_lock = 1;                                                         // 电机位置锁定
+            send_enable = 1;                                                    // main发送CAN位置命令
+            break;
+        case('M'):
+            command.printf("DF entering BOARD motor mode\n\r");
+            EnterMotorMode(&DF_can);
+            c_lock = 0;                                                         // 电机位置解锁
+            send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定
+            break;
+        case('z'):
+            command.printf("DF zeroing\n\r");
+            Zero(&DF_can);
+            break;   
+    }
+    */
+    
+    wait_ms(2);
+    
+//    sf_m_c = 0;                                                                 // 恢复接收模式
+    wait_us(200);
+    
+    
+    
+    // 电机模式命令在此发送，锁定控制模式的位置量在main中发送
+    WriteAll();
+    //wait(1.0f);                        
+    
+    // 解除串口控制，缓冲区清0
+    command_control_flag = 0;
+    for(unsigned int i = 0; i < 10; i++)
+    {
+        Suse[i] = 0;      
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.h	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,67 @@
+#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 enabled;
+extern int counter;
+extern int command_control_flag; 
+extern float SP_pf, SP_df;
+extern int c_lock; 
+extern int return_zero; 
+
+
+extern unsigned int flag_c_A, flag_c_B;
+extern unsigned int Snum_c;
+extern uint16_t Sget[10];                                                    
+extern uint16_t Suse[10];
+
+
+void serial_command_isr();
+void command_control();
+
+
+
+
+
+
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_PC/data_pc.cpp	Tue Nov 10 09:09:58 2020 +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	Tue Nov 10 09:09:58 2020 +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/LEG_MESSAGE/leg_message.h	Tue Nov 10 09:09:58 2020 +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_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	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,24 @@
+#include "used_leg_message.h"
+
+
+
+
+leg_control a_control;
+leg_state   a_state;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/used_leg_message.h	Tue Nov 10 09:09:58 2020 +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;
+
+
+
+
+
+
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODE/mode.cpp	Tue Nov 10 09:09:58 2020 +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	Tue Nov 10 09:09:58 2020 +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/main.cpp	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,105 @@
+#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"
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                             框架搭建完毕                                     //
+////////////////////////////////////////////////////////////////////////////////
+
+//ankle----pf
+//knee-----df
+
+
+int main()
+{
+
+////////////////////////初始化//////////////////////////////////////
+    pc.baud(115200);
+    pc.attach(&serial_pc_isr);
+
+    foot.baud(115200);
+    foot.attach(&serial_board_isr);
+
+//    command.baud(115200);
+//    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    = 0x01;
+////////////////////////////////////////////////////////////////////////////////
+    wait(5);
+
+
+    EnterMotorMode(&knee_txMsg);
+    EnterMotorMode(&ankle_txMsg);
+
+//////////////////////////////////////////position/////////////////////////////////////////
+//        a_control.ankle.p_des=3.14/8;
+//        a_control.ankle.v_des=0;
+//        a_control.ankle.kp=10;
+//        a_control.ankle.kd=0;
+//        a_control.ankle.t_ff=0;
+//
+//        a_control.knee.p_des=3.14/8;
+//        a_control.knee.v_des=0;
+//        a_control.knee.kp=10;
+//        a_control.knee.kd=0;
+//        a_control.knee.t_ff=0;
+//        PackAll();
+//        WriteAll();
+
+
+    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);
+
+        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;
+        knv = a_state.knee_state.v;
+        knt = a_state.knee_state.t;
+
+        pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",ankp,ankv,ankt,knp,knv,knt);
+
+///////////////////////////////////////velocity///////////////////////////////////////////////
+
+//        a_control.ankle.p_des=0;
+//        a_control.ankle.v_des=500*2*3.14/60/49;
+//        a_control.ankle.kp=0;
+//        a_control.ankle.kd=3;
+//        a_control.ankle.t_ff=0;
+//
+//        a_control.knee.p_des=0;
+//        a_control.knee.v_des=500*2*3.14/60/49;
+//        a_control.knee.kp=0;
+//        a_control.knee.kd=3;
+//        a_control.knee.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	Tue Nov 10 09:09:58 2020 +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	Tue Nov 10 09:09:58 2020 +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	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/benkatz/code/mbed-dev_spine/#97f825502e2a