Revision 5:6a95726e45b0, committed 2019-10-08

Comitter:

WXD

Date:

Tue Oct 08 04:35:23 2019 +0000

Parent:

4:f0a4a87b6730

Child:

6:aad89fd109c2

Commit message:

qqq

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.cpp	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,111 @@
+#include "CAN.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_can, DF_can;                                                      // 主控发送的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_can, a_control.pf); 
+    pack_cmd(&DF_can, a_control.df); 
+}
+
+
+// 写联合打包的数据
+void WriteAll(){
+    pf_can.write(PF_can);
+    wait(.00002);
+    df_can.write(DF_can);
+    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==0x0a){
+        ankle->pf.p = p;
+        ankle->pf.v = v;
+        ankle->pf.t = t;
+        }
+    else if(id==0x0b){
+        ankle->df.p = p;
+        ankle->df.v = v;
+        ankle->df.t = t;
+        }
+} 
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CAN/CAN.h	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,41 @@
+#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  800.0f
+#define KD_MIN  0.0f
+#define KD_MAX  100.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_can, DF_can;                                               // 主控发送的CAN消息
+
+
+// 函数外部申明
+void pack_cmd(CANMessage * msg, joint_control joint);
+void PackAll();
+void WriteAll();
+void unpack_reply(CANMessage msg, ankle_state * ankle);
+
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/CALCULATE/calculate.cpp	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,68 @@
+#include "calculate.h"
+
+
+float pf_position = 0;
+float df_position = 0;
+
+
+void calculate_pf()
+{
+    //send_enable = 1;
+    
+    
+    
+    
+    
+    
+    
+    
+    cal_command.q_des_pf  = pf_position;
+    cal_command.qd_des_pf = 0.0f;
+    cal_command.kp_pf     = 800.0f;
+    cal_command.kd_pf     = 100.0f;
+    cal_command.tau_pf_ff = 0.0f;
+}
+
+
+void calculate_df()
+{    
+    //send_enable = 1;
+
+
+
+
+
+
+    cal_command.q_des_df  = df_position;
+    cal_command.qd_des_df = 0.0f;
+    cal_command.kp_df     = 800.0f;
+    cal_command.kd_df     = 100.0f;
+    cal_command.tau_df_ff = 0.0f;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/CALCULATE/calculate.h	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,24 @@
+#ifndef _CALCULATE_H
+#define _CALCULATE_H
+
+
+#include "mbed.h"
+#include "data_command.h"
+#include "data_board.h"
+#include "used_leg_message.h"
+
+
+extern float pf_position;
+extern float df_position;
+
+
+void calculate_pf();
+void calculate_df();
+
+
+
+
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.cpp	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,59 @@
+#include "control.h"
+
+
+unsigned int send_enable = 0;
+
+
+void control()
+{
+    if(c_control == 1)
+        command_control();
+        
+    if(c_lock == 1)                                                             // 处于PC串口锁定模式
+    {
+        cal_command.q_des_pf  = SP_pf;
+        cal_command.qd_des_pf = 0.0f;
+        cal_command.kp_pf     = 800.0f;
+        cal_command.kd_pf     = 100.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     = 800.0f;
+        cal_command.kd_df     = 100.0f;
+        cal_command.tau_df_ff = 0.0f;
+    }
+    
+    if(c_lock == 0)
+    {
+        calculate_pf();
+        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;
+    a_control.pf.kd    = cal_command.kd_pf;
+    a_control.pf.t_ff  = cal_command.tau_pf_ff;
+        
+    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;
+    a_control.df.kd    = cal_command.kd_df;
+    a_control.df.t_ff  = cal_command.tau_df_ff;
+}
+  
+    
+
+
+
+
+
+
+      
+        
+        
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.h	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,23 @@
+#ifndef _CONTROL_H
+#define _CONTROL_H
+
+
+#include "mbed.h"
+#include "CAN.h"
+#include "mode.h"
+#include "data_command.h"
+#include "data_board.h"
+#include "calculate.h"
+#include "used_leg_message.h"
+
+
+extern unsigned int send_enable;
+
+
+void control();
+
+
+
+
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_BOARD/data_board.cpp	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,196 @@
+#include "mbed.h"
+#include "data_board.h"
+
+
+Serial      board(PA_9, PA_10);
+DigitalOut  sf_m_b(PA_11);
+
+
+unsigned int flag_485_A = 0, flag_485_B = 0;
+unsigned int S485num = 0;
+uint16_t S485get[8] = {0};                                                    
+uint16_t S485use[8] = {0};
+
+
+void serial_board_isr(void)
+{
+    while(board.readable())
+    {
+        uint16_t c = board.getc();
+        if(c == 0x80)
+        {
+            flag_485_A = 1;
+            
+            flag_485_B = 0;
+            S485num = 0;
+            for(unsigned int i = 0; i < 8; i++)
+            {
+                S485get[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 0x40)
+        {
+            flag_485_B = 1;
+        }
+        
+        if(flag_485_A == 1)
+        {
+            if((flag_485_B != 1) && (S485num < 8))
+            {
+                S485get[S485num] = c;    
+            }
+            
+            S485num++;
+            
+            if((flag_485_B == 1) && (S485num != 9))
+            {    
+                flag_485_A = 0;
+                flag_485_B = 0;
+                S485num    = 0;     
+            }
+            
+            if((flag_485_B == 1) && (S485num == 9))
+            {
+                flag_485_A = 0;
+                flag_485_B = 0;
+                S485num    = 0;
+                
+                for(unsigned int i = 0; i < 8; i++)
+                {
+                    S485use[i] = S485get[i];
+                }  
+            } 
+        }
+    }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+unsigned int Gait_num_valid = 0, Gait_now = 0;
+uint16_t Gait_per_now_int = 0, COP_Y_int = 0, COP_X_int = 0;
+float Gait_per_now = 0, COP_Y = 0, COP_X = 0;
+
+unsigned int Gait_now_0before = 0;
+float Gait_per_now_0before = 0, COP_Y_0before = 0, COP_X_0before = 0;
+
+unsigned int Gait_num_valid_real = 0, Gait_now_real = 0;
+float Gait_per_now_real = 0, COP_Y_real = 0, COP_X_real = 0;
+
+
+void gait_decode()
+{
+    Gait_num_valid = S485use[0];
+    
+    Gait_now = S485use[1];
+    
+    Gait_per_now_int = (S485use[2]<<6)|S485use[3];
+    Gait_per_now     = uint_to_float(Gait_per_now_int, 0.0f, 999.0f, 12);
+    
+    COP_Y_int = (S485use[4]<<6)|S485use[5];
+    COP_Y     = uint_to_float(COP_Y_int, 0.0f, 270.0f, 12);
+    
+    COP_X_int = (S485use[6]<<6)|S485use[7];
+    COP_X     = uint_to_float(COP_X_int, 0.0f, 100.0f, 12);
+    
+    if(Gait_now != 0)
+        Gait_now_0before = Gait_now;
+    if(Gait_now == 0)
+        Gait_now = Gait_now_0before;
+        
+    if(Gait_per_now != 0)
+        Gait_per_now_0before = Gait_per_now;
+    if(Gait_per_now == 0)
+        Gait_per_now = Gait_per_now_0before;    
+        
+    if(COP_Y != 0)
+        COP_Y_0before = COP_Y;
+    if(COP_Y == 0)
+        COP_Y = COP_Y_0before;
+        
+    if(COP_X != 0)
+        COP_X_0before = COP_X;
+    if(COP_X == 0)
+        COP_X = COP_X_0before;
+        
+    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;
+}
+
+
+void gait_clear()
+{
+    for(int i = 0; i < 8; i++)
+        S485use[i] = 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+
+float ad_pf = 0, ad_df = 0;
+uint16_t ad_pf_int = 0, ad_df_int = 0;
+uint8_t ad_pf_forward = 0, ad_pf_backward = 0, ad_df_forward = 0, ad_df_backward = 0;
+uint8_t ad_null = 0x3f;
+
+float ad_pf_0before = 0, ad_df_0before = 0;
+
+float ad_pf_real = 0, ad_df_real = 0;
+
+
+void ad_decode()
+{
+    ad_pf_int = (S485use[0]<<6)|S485use[1];
+    ad_df_int = (S485use[2]<<6)|S485use[3];
+    
+    ad_pf = uint_to_float(ad_pf_int, 0.0f, 5.0f, 12);
+    ad_df = uint_to_float(ad_df_int, 0.0f, 5.0f, 12);
+    
+    if(ad_pf != 0)
+        ad_pf_0before = ad_pf;
+    if(ad_pf == 0)
+        ad_pf = ad_pf_0before;
+        
+    if(ad_df != 0)
+        ad_df_0before = ad_df;
+    if(ad_df == 0)
+        ad_df = ad_df_0before;
+        
+    ad_pf_real = ad_pf;
+    ad_df_real = ad_df;
+}
+
+
+void ad_clear()
+{
+    for(int i = 0; i < 8; i++)
+        S485use[i] = 0;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_BOARD/data_board.h	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,103 @@
+#ifndef _DATA_BOARD_H
+#define _DATA_BOARD_H
+
+
+#include "mbed.h"
+#include "math_ops.h"
+
+
+extern Serial      board;
+extern DigitalOut  sf_m_b;
+
+
+extern unsigned int flag_485_A, flag_485_B;
+extern unsigned int S485num;
+extern uint16_t S485get[8];                                                    
+extern uint16_t S485use[8];
+
+
+void serial_board_isr();
+
+////////////////////////////////////////////////////////////////////////////////
+
+extern unsigned int Gait_num_valid, Gait_now;
+extern float Gait_per_now, COP_Y, COP_X;
+extern uint16_t Gait_per_now_int, COP_Y_int, COP_X_int;
+
+extern unsigned int Gait_now_0before;
+extern float Gait_per_now_0before, COP_Y_0before, COP_X_0before;
+
+extern unsigned int Gait_num_valid_real, Gait_now_real;
+extern float Gait_per_now_real, COP_Y_real, COP_X_real;
+
+
+void gait_decode();
+void gait_clear();
+
+////////////////////////////////////////////////////////////////////////////////
+
+extern float ad_pf, ad_df;
+extern uint16_t ad_pf_int, ad_df_int;
+extern uint8_t ad_pf_forward, ad_pf_backward, ad_df_forward, ad_df_backward;    
+extern uint8_t ad_null; 
+
+extern float ad_pf_0before, ad_df_0before;
+
+extern float ad_pf_real, ad_df_real;
+
+
+void ad_decode();
+void ad_clear();
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.cpp	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,202 @@
+#include "data_command.h"
+
+
+Serial      command(PC_10, PC_11);
+DigitalOut  sf_m_c(PC_12);
+
+int enabled = 0;                                                                // 进入电机模式标志位
+int counter = 0;                                                                // 输出计数器
+int c_control = 0;                                                              // 命令帧标志位
+float SP_pf = 0, SP_df = 0;
+int c_lock = 2;                                                                 // 位置锁定标志位 
+
+unsigned int flag_A = 0, flag_B = 0;
+unsigned int Snum = 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_A = 1;
+            
+            flag_B = 0;
+            Snum   = 0;
+            for(unsigned int i = 0; i < 10; i++)
+            {
+                Sget[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 'B')
+        {
+            flag_B = 1;
+        }
+    
+        if(flag_A == 1)
+        {
+            if((flag_B != 1) && (Snum < 10))
+            {
+                Sget[Snum] = c;    
+            }
+            
+            Snum++;
+            
+            if((flag_B == 1) && (Snum != 11))
+            {                        
+                flag_A = 0;
+                flag_B = 0;
+                Snum   = 0;     
+            }
+            
+            if((flag_B == 1) && (Snum == 11))
+            {
+                flag_A = 0;
+                flag_B = 0;
+                Snum   = 0;
+                
+                for(unsigned int i = 0; i < 10; i++)
+                {
+                    Suse[i] = Sget[i];
+                }  
+                
+                c_control = 1;                                                  // 命令帧接收成功
+                
+                //pc.printf("%c%c%c%c%c%c%c%c%c%c\n\r", S340use[0], S340use[1], S340use[2], S340use[3], S340use[4], S340use[5], S340use[6], S340use[7], S340use[8], S340use[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;
+    
+    
+    sf_m_c = 1;                                                                 // 切换为发送模式
+    wait_us(200);
+    
+    switch(Suse[0])
+    {
+        case('e'):
+            command.printf("\n\rPF exiting motor mode\r");
+            ExitMotorMode(&PF_can);
+            c_lock = 1;                                                         // 电机位置解锁
+            send_enable = 0;                                                    // main不发送CAN位置命令
+            break;
+        case('m'):
+            command.printf("\n\rPF entering PC motor mode\r");
+            EnterMotorMode(&PF_can);
+            c_lock = 1;                                                         // 电机位置锁定
+            send_enable = 1;                                                    // main发送CAN位置命令
+            break;
+        case('M'):
+            command.printf("\n\rPF entering BOARD motor mode\r");
+            EnterMotorMode(&PF_can);
+            c_lock = 0;                                                         // 电机位置解锁
+            send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定
+            break;
+        case('z'):
+            command.printf("\n\rPF zeroing\r");
+            Zero(&PF_can);
+            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
+    c_control = 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 Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,66 @@
+#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 DigitalOut  sf_m_c;
+
+
+extern int enabled;
+extern int counter;
+extern int c_control; 
+extern float SP_pf, SP_df;
+extern int c_lock; 
+
+
+extern unsigned int flag_A, flag_B;
+extern unsigned int Snum;
+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 Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,26 @@
+#include "data_pc.h"
+
+
+Serial pc(PA_2, PA_3);
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_PC/data_pc.h	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,30 @@
+#ifndef _DATA_PC_H
+#define _DATA_PC_H
+
+
+#include "mbed.h"
+
+
+extern Serial pc;
+
+
+
+
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/leg_message.h	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,72 @@
+#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;                                                       // 控制成员: pf, df关节
+    };
+
+struct joint_state{                                                             // 关节状态结构体
+    float p, v, t;                                                              // 状态成员:  p\v\t
+    };
+    
+struct ankle_state{                                                             // 系统状态结构体
+    joint_state pf, df;                                                         // 状态成员: 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	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,23 @@
+#include "used_leg_message.h"
+
+
+ankle_state     a_state;
+ankle_control   a_control;
+
+//cal_data_t      cal_data;                                                       // spine-->up
+cal_command_t   cal_command;                                                    // up-->spine
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LEG_MESSAGE/used_leg_message.h	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,31 @@
+#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;
+
+//extern cal_data_t      cal_data;                                                // spine-->up
+extern cal_command_t   cal_command;                                             // up-->spine
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODE/mode.cpp	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,67 @@
+#include "mode.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 Oct 08 04:35:23 2019 +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

--- a/leg_message.h	Wed May 02 18:19:22 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,60 +0,0 @@
-#ifndef _leg_message
-#define _leg_message
-
-#include <stdint.h>
-
-// 60 bytes
-// 30 16-bit words
-struct spi_data_t
-{
-    float q_abad[2];
-    float q_hip[2];
-    float q_knee[2];
-    float qd_abad[2];
-    float qd_hip[2];
-    float qd_knee[2];
-    int32_t flags[2];
-    int32_t checksum;
-};
-
-// 132 bytes
-// 66 16-bit words
-struct spi_command_t
-{
-    float q_des_abad[2];
-    float q_des_hip[2];
-    float q_des_knee[2];
-    float qd_des_abad[2];
-    float qd_des_hip[2];
-    float qd_des_knee[2];
-    float kp_abad[2];
-    float kp_hip[2];
-    float kp_knee[2];
-    float kd_abad[2];
-    float kd_hip[2];
-    float kd_knee[2];
-    float tau_abad_ff[2];
-    float tau_hip_ff[2];
-    float tau_knee_ff[2];
-    int32_t flags[2];
-    int32_t checksum;
-};
-
-
-
-struct joint_control{
-    float p_des, v_des, kp, kd, t_ff;
-    };
-    
-struct joint_state{
-    float p, v, t;
-    };
-    
-struct leg_state{
-    joint_state a, h, k;
-    };
-struct leg_control{
-    joint_control a, h, k;
-    }
-    ;
-#endif
\ No newline at end of file

--- a/main.cpp	Wed May 02 18:19:22 2018 +0000
+++ b/main.cpp	Tue Oct 08 04:35:23 2019 +0000
@@ -1,625 +1,106 @@
-
-
 #include "mbed.h"
+#include <cstring>
 #include "math_ops.h"
-#include <cstring>
 #include "leg_message.h"
-
-// length of receive/transmit buffers
-#define RX_LEN 66
-#define TX_LEN 66
-
-// length of outgoing/incoming messages
-#define DATA_LEN 30
-#define CMD_LEN  66
-
-// Master CAN ID ///
-#define CAN_ID 0x0
-
-
-/// Value Limits ///
- #define P_MIN -12.5f
- #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
- 
- /// Joint Soft Stops ///
- #define A_LIM_P 1.5f
- #define A_LIM_N -1.5f
- #define H_LIM_P 5.0f
- #define H_LIM_N -5.0f
- #define K_LIM_P 0.2f
- #define K_LIM_N 7.7f
- #define KP_SOFTSTOP 100.0f
- #define KD_SOFTSTOP 0.4f;
-
-#define ENABLE_CMD 0xFFFF
-#define DISABLE_CMD 0x1F1F
-
-spi_data_t spi_data; // data from spine to up
-spi_command_t spi_command; // data from up to spine
-
-// spi buffers
-uint16_t rx_buff[RX_LEN];
-uint16_t tx_buff[TX_LEN];
-
-DigitalOut led(PC_5);
-
-
-Serial       pc(PA_2, PA_3);
-CAN          can1(PB_12, PB_13);  // CAN Rx pin name, CAN Tx pin name
-CAN          can2(PB_8, PB_9);  // CAN Rx pin name, CAN Tx pin name
-
-CANMessage   rxMsg1, rxMsg2;
-CANMessage   txMsg1, txMsg2;
-CANMessage   a1_can, a2_can, h1_can, h2_can, k1_can, k2_can;    //TX Messages
-int                     ledState;
-Ticker                  sendCAN;
-int                     counter = 0;
-volatile bool           msgAvailable = false;
-Ticker loop;
-
-int spi_enabled = 0;
-InterruptIn cs(PA_4);
-DigitalIn estop(PA_14);
-//SPISlave spi(PA_7, PA_6, PA_5, PA_4);
-
-
-leg_state l1_state, l2_state;;
-leg_control l1_control, l2_control;
-
-uint16_t x = 0;
-uint16_t x2 = 0;
-uint16_t count = 0;
-uint16_t counter2 = 0;
-
-int control_mode = 1;
-int is_standing = 0;
-int enabled = 0;
-
- // generates fake spi data from spi command
-void test_control();
-void control();
-
-
-/// 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;
-     }
-     
-/// 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 * leg){
-    /// 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==1){
-        leg->a.p = p;
-        leg->a.v = v;
-        leg->a.t = t;
-        }
-    else if(id==2){
-        leg->h.p = p;
-        leg->h.v = v;
-        leg->h.t = t;
-        }
-    else if(id==3){
-        leg->k.p = p;
-        leg->k.v = v;
-        leg->k.t = t;
-        }
-    } 
-
- void rxISR1() {
-    can1.read(rxMsg1);                    // read message into Rx message storage
-    unpack_reply(rxMsg1, &l1_state);
-}
-void rxISR2(){
-    can2.read(rxMsg2);
-    unpack_reply(rxMsg2, &l2_state);
-    }
-void PackAll(){
-    pack_cmd(&a1_can, l1_control.a); 
-    pack_cmd(&a2_can, l2_control.a); 
-    pack_cmd(&h1_can, l1_control.h); 
-    pack_cmd(&h2_can, l2_control.h); 
-    pack_cmd(&k1_can, l1_control.k); 
-    pack_cmd(&k2_can, l2_control.k); 
-    
-    }
-void WriteAll(){
-    //toggle = 1;
-    can1.write(a1_can);
-    wait(.00002);
-    can2.write(a2_can);
-    wait(.00002);
-    can1.write(h1_can);
-    wait(.00002);
-    can2.write(h2_can);
-    wait(.00002);
-    can1.write(k1_can);
-    wait(.00002);
-    can2.write(k2_can);
-    wait(.00002);
-    //toggle = 0;
-    }
-
-void sendCMD(){
-    counter ++;
-
-    PackAll();
-
-    if(counter>100){
-        printf("%.3f %.3f %.3f   %.3f %.3f %.3f\n\r", l1_state.a.p, l1_state.h.p, l1_state.k.p, l2_state.a.p, l2_state.h.p, l2_state.k.p);
-        counter = 0 ;
-        }
-    
-    WriteAll();
-    
-    }
-
+#include "CAN.h"
+#include "used_leg_message.h"
+#include "data_command.h"
+#include "data_pc.h"
+#include "data_board.h"
+#include "mode.h"
+#include "control.h"
+#include "calculate.h"
 
 
+int main()
+{
+    pc.baud(115200);
     
-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();
-    }
+    command.baud(115200);
+    command.attach(&serial_command_isr);
+    sf_m_c = 0;                                                                 // 主要为接收模式
+    
+    board.baud(115200);
+    board.attach(&serial_board_isr);     
+    sf_m_b = 1;                                                                 // 主要为发送模式
     
-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 serial_isr(){
-     /// handle keyboard commands from the serial terminal ///
-     while(pc.readable()){
-        char c = pc.getc();
-        //led = !led;
-        switch(c){
-            case(27):
-                //loop.detach();
-                printf("\n\r exiting motor mode \n\r");
-                ExitMotorMode(&a1_can);
-                ExitMotorMode(&a2_can);
-                ExitMotorMode(&h1_can);
-                ExitMotorMode(&h2_can);
-                ExitMotorMode(&k1_can);
-                ExitMotorMode(&k2_can);
-                enabled = 0;
-                break;
-            case('m'):
-                printf("\n\r entering motor mode \n\r");
-                EnterMotorMode(&a1_can);
-                EnterMotorMode(&a2_can);
-                EnterMotorMode(&h1_can);
-                EnterMotorMode(&h2_can);
-                EnterMotorMode(&k1_can);
-                EnterMotorMode(&k2_can);
-                wait(.5);
-                enabled = 1;
-                //loop.attach(&sendCMD, .001);
-                break;
-            case('s'):
-                printf("\n\r standing \n\r");
-                counter2 = 0;
-                is_standing = 1;
-                //stand();
-                break;
-            case('z'):
-                printf("\n\r zeroing \n\r");
-                Zero(&a1_can);
-                Zero(&a2_can);
-                Zero(&h1_can);
-                Zero(&h2_can);
-                Zero(&k1_can);
-                Zero(&k2_can);
-                break;
-            }
-        }
-        WriteAll();
-        
-    }
+    pf_can.frequency(1000000);
+    pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    pf_rxMsg.len = 6;
+    PF_can.len   = 8;
+    PF_can.id    = 0x0a;
     
-uint32_t xor_checksum(uint32_t* data, size_t len)
-{
-    uint32_t t = 0;
-    for(int i = 0; i < len; i++)   
-        t = t ^ data[i];
-    return t;
-}
-
-void spi_isr(void)
-{
-    GPIOC->ODR |= (1 << 8);
-    GPIOC->ODR &= ~(1 << 8);
-    int bytecount = 0;
-    SPI1->DR = tx_buff[0];
-    while(cs == 0) {
-        if(SPI1->SR&0x1) {
-            rx_buff[bytecount] = SPI1->DR;
-            bytecount++;
-            if(bytecount<TX_LEN) {
-                SPI1->DR = tx_buff[bytecount];
-            }
-        }
-
-    }
-    
-    // after reading, save into spi_command
-    // should probably check checksum first!
-    uint32_t calc_checksum = xor_checksum((uint32_t*)rx_buff,32);
-    for(int i = 0; i < CMD_LEN; i++)
-    {
-        ((uint16_t*)(&spi_command))[i] = rx_buff[i];
-    }
+    df_can.frequency(1000000);
+    df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    df_rxMsg.len = 6;
+    DF_can.len   = 8;
+    DF_can.id    = 0x0b;
     
-    // run control, which fills in tx_buff for the next iteration
-    if(calc_checksum != spi_command.checksum){
-        spi_data.flags[1] = 0xdead;}
-        
-    //test_control();
-    //spi_data.q_abad[0] = 12.0f;
-    control();
-    PackAll();
-    WriteAll();
-
-
-    //for (int i = 0; i<TX_LEN; i++) {
-     //   tx_buff[i] = 2*rx_buff[i];
-    //}
-//    for (int i=0; i<TX_LEN; i++) {
-//        //printf("%d ", rx_buff[i]);
-//    }
-    //printf("\n\r");
-}
-
-int softstop_joint(joint_state state, joint_control * control, float limit_p, float limit_n){
-    if((state.p)>=limit_p){
-        //control->p_des = limit_p;
-        control->v_des = 0.0f;
-        control->kp = 0;
-        control->kd = KD_SOFTSTOP;
-        control->t_ff += KP_SOFTSTOP*(limit_p - state.p);
-        return 1;
-    }
-    else if((state.p)<=limit_n){
-        //control->p_des = limit_n;
-        control->v_des = 0.0f;
-        control->kp = 0;
-        control->kd = KD_SOFTSTOP;
-        control->t_ff += KP_SOFTSTOP*(limit_n - state.p);
-        return 1;
-    }
-    return 0;
-    
-    }
+    NVIC_SetPriority(USART1_IRQn, 3);                                           // pc中断优先级高于board
+    NVIC_SetPriority(USART2_IRQn, 4);
     
     
-void control()
-{
-    
-    if(((spi_command.flags[0]&0x1)==1)  && (enabled==0)){
-        enabled = 1;
-        EnterMotorMode(&a1_can);
-        can1.write(a1_can);
-        EnterMotorMode(&a2_can);
-        can2.write(a2_can);
-        EnterMotorMode(&k1_can);
-        can1.write(k1_can);
-        EnterMotorMode(&k2_can);
-        can2.write(k2_can);
-        EnterMotorMode(&h1_can);
-        can1.write(h1_can);
-        EnterMotorMode(&h2_can);
-        can2.write(h2_can);
-        printf("e\n\r");
-        return;
-    }
-    else if((((spi_command.flags[0]&0x1))==0)  && (enabled==1)){
-         enabled = 0;
-        ExitMotorMode(&a1_can);
-        can1.write(a1_can);
-        ExitMotorMode(&a2_can);
-        can2.write(a2_can);
-        ExitMotorMode(&h1_can);
-        can1.write(h1_can);
-        ExitMotorMode(&h2_can);
-        can2.write(h2_can);
-        ExitMotorMode(&k1_can);
-        can1.write(k1_can);
-        ExitMotorMode(&k2_can);
-        can2.write(k2_can);
-        printf("x\n\r");
-        return;
+    while(1)
+    {
+        counter++;
+        
+        //////////////////////// CAN获取电机位置速度信息 //////////////////////////
+        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);
+        /**********************************************************************/
+        
+        /////////////////////// 485获取步态，力传感器信息 //////////////////////////
+        sf_m_b = 1;                                                               // 发送模式                                                            
+        wait_us(200);                                                           // 等待选择引脚电压稳定                                                 
+        board.printf("A1B");
+        wait_ms(1);                                                             // mbed特色，需要等待数据完全发送
+        sf_m_b = 0;                                                               // 接收模式
+        wait_us(200);                                                           // 等待选择引脚电压稳定                                                    
+        wait_ms(3);                                                             // 等待从机上传数据                                                                                                       
+        gait_decode();
+        //pc.printf("A1B: %d - %d - %f - %f - %f\n\r", Gait_num_valid, Gait_now, Gait_per_now, COP_Y, COP_X);
+        gait_clear();
+
+        sf_m_b = 1;                                                               // 发送模式                                                            
+        wait_us(200);                                                           // 等待选择引脚电压稳定                                                 
+        board.printf("A2B");
+        wait_ms(1);                                                             // mbed特色，需要等待数据完全发送
+        sf_m_b = 0;                                                               // 接收模式
+        wait_us(200);                                                           // 等待选择引脚电压稳定                                                    
+        wait_ms(4);                                                             // 等待从机上传数据                                                                                                       
+        ad_decode();
+        //pc.printf("A2B: %f - %f\n\r", ad_pf, ad_df);
+        ad_clear();
+        /**********************************************************************/        
+        
+        ////////////////////////// 电机状态控制，命令发送 //////////////////////////
+        control();
+        
+        if(send_enable == 1)
+        {
+            PackAll();
+            WriteAll();
         }
-    
-    spi_data.q_abad[0] = l1_state.a.p;
-    spi_data.q_hip[0] = l1_state.h.p;
-    spi_data.q_knee[0] = l1_state.k.p;
-    spi_data.qd_abad[0] = l1_state.a.v;
-    spi_data.qd_hip[0] = l1_state.h.v;
-    spi_data.qd_knee[0] = l1_state.k.v;
-    
-    spi_data.q_abad[1] = l2_state.a.p;
-    spi_data.q_hip[1] = l2_state.h.p;
-    spi_data.q_knee[1] = l2_state.k.p;
-    spi_data.qd_abad[1] = l2_state.a.v;
-    spi_data.qd_hip[1] = l2_state.h.v;
-    spi_data.qd_knee[1] = l2_state.k.v;
-    
-    
-    
-    if(estop==0){
-        //printf("estopped!!!!\n\r");
-        memset(&l1_control, 0, sizeof(l1_control));
-        memset(&l2_control, 0, sizeof(l2_control));
-        spi_data.flags[0] = 0xdead;
-        spi_data.flags[1] = 0xdead;
-        led = 1;
-        }
-    
-    else{
-        led = 0;
-        
-        memset(&l1_control, 0, sizeof(l1_control));
-        memset(&l2_control, 0, sizeof(l2_control));
-        
-        l1_control.a.p_des = spi_command.q_des_abad[0];
-        l1_control.a.v_des  = spi_command.qd_des_abad[0];
-        l1_control.a.kp = spi_command.kp_abad[0];
-        l1_control.a.kd = spi_command.kd_abad[0];
-        l1_control.a.t_ff = spi_command.tau_abad_ff[0];
+        /**********************************************************************/        
         
-        l1_control.h.p_des = spi_command.q_des_hip[0];
-        l1_control.h.v_des  = spi_command.qd_des_hip[0];
-        l1_control.h.kp = spi_command.kp_hip[0];
-        l1_control.h.kd = spi_command.kd_hip[0];
-        l1_control.h.t_ff = spi_command.tau_hip_ff[0];
-        
-        l1_control.k.p_des = spi_command.q_des_knee[0];
-        l1_control.k.v_des  = spi_command.qd_des_knee[0];
-        l1_control.k.kp = spi_command.kp_knee[0];
-        l1_control.k.kd = spi_command.kd_knee[0];
-        l1_control.k.t_ff = spi_command.tau_knee_ff[0];
-        
-        l2_control.a.p_des = spi_command.q_des_abad[1];
-        l2_control.a.v_des  = spi_command.qd_des_abad[1];
-        l2_control.a.kp = spi_command.kp_abad[1];
-        l2_control.a.kd = spi_command.kd_abad[1];
-        l2_control.a.t_ff = spi_command.tau_abad_ff[1];
+        ////////////////////////////// 输出状态信息 //////////////////////////////
+        /*
+        if(counter >= 20000)
+        {
+            //pc.printf("\n\rpf- %.3f %.3f %.3f \n\r", a_state.pf.p, a_state.pf.v, a_state.pf.t);
+            //pc.printf("df- %.3f %.3f %.3f \n\r", a_state.df.p, a_state.df.v, a_state.df.t);
+            
+            counter = 0;
+        }
+        */
         
-        l2_control.h.p_des = spi_command.q_des_hip[1];
-        l2_control.h.v_des  = spi_command.qd_des_hip[1];
-        l2_control.h.kp = spi_command.kp_hip[1];
-        l2_control.h.kd = spi_command.kd_hip[1];
-        l2_control.h.t_ff = spi_command.tau_hip_ff[1];
-        
-        l2_control.k.p_des = spi_command.q_des_knee[1];
-        l2_control.k.v_des  = spi_command.qd_des_knee[1];
-        l2_control.k.kp = spi_command.kp_knee[1];
-        l2_control.k.kd = spi_command.kd_knee[1];
-        l2_control.k.t_ff = spi_command.tau_knee_ff[1];
-        
-        
-        spi_data.flags[0] = 0;
-        spi_data.flags[1] = 0;
-        spi_data.flags[0] |= softstop_joint(l1_state.a, &l1_control.a, A_LIM_P, A_LIM_N);
-        spi_data.flags[0] |= (softstop_joint(l1_state.h, &l1_control.h, H_LIM_P, H_LIM_N))<<1;
-        //spi_data.flags[0] |= (softstop_joint(l1_state.k, &l1_control.k, K_LIM_P, K_LIM_N))<<2;
-        spi_data.flags[1] |= softstop_joint(l2_state.a, &l2_control.a, A_LIM_P, A_LIM_N);
-        spi_data.flags[1] |= (softstop_joint(l2_state.h, &l2_control.h, H_LIM_P, H_LIM_N))<<1;
-        //spi_data.flags[1] |= (softstop_joint(l2_state.k, &l2_control.k, K_LIM_P, K_LIM_N))<<2;
-        
-        //spi_data.flags[0] = 0xbeef;
-        //spi_data.flags[1] = 0xbeef;
-        //PackAll();
-        //WriteAll();
+        //pc.printf("\n\rPpd: %f - %f\r", SP_pf, SP_df);
+        //pc.printf("\n\rPpd: %f - %f\r", a_state.pf.p, a_state.df.p);
+        pc.printf("A1B: %d - %d - %f - %f - %f\r", Gait_num_valid_real, Gait_now_real, Gait_per_now_real, COP_Y_real, COP_X_real);
+        //pc.printf("A2B: %f - %f\n\r", ad_pf_real, ad_df_real);
+        /**********************************************************************/      
     }
-    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
-    for(int i = 0; i < DATA_LEN; i++){
-        tx_buff[i] = ((uint16_t*)(&spi_data))[i];}
-    
-}
-    
-
-void test_control()
-{
-    for(int i = 0; i < 2; i++)
-    {
-        spi_data.q_abad[i] = spi_command.q_des_abad[i] + 1.f;
-        spi_data.q_knee[i] = spi_command.q_des_knee[i] + 1.f;
-        spi_data.q_hip[i]  = spi_command.q_des_hip[i]  + 1.f;
-        
-        spi_data.qd_abad[i] = spi_command.qd_des_abad[i] + 1.f;
-        spi_data.qd_knee[i] = spi_command.qd_des_knee[i] + 1.f;
-        spi_data.qd_hip[i]  = spi_command.qd_des_hip[i]  + 1.f;
-    }
-    
-    spi_data.flags[0] = 0xdead;
-    //spi_data.flags[1] = 0xbeef;
-    
-    // only do first 56 bytes of message.
-    spi_data.checksum = xor_checksum((uint32_t*)&spi_data,14);
-    
-    for(int i = 0; i < DATA_LEN; i++)
-        tx_buff[i] = ((uint16_t*)(&spi_data))[i];
-}
-
-void init_spi(void){
-    SPISlave *spi = new SPISlave(PA_7, PA_6, PA_5, PA_4);
-    spi->format(16, 0);
-    spi->frequency(12000000);
-    spi->reply(0x0);
-    cs.fall(&spi_isr);
-    printf("done\n\r");
-}
-
-    
-int main() {
-    //wait(.5);
-    //led = 1;
-    pc.baud(921600);
-    pc.attach(&serial_isr);
-    estop.mode(PullUp);
-    //spi.format(16, 0);
-    //spi.frequency(1000000);
-    //spi.reply(0x0);
-    //cs.fall(&spi_isr);
-
-    can1.frequency(1000000);                     // set bit rate to 1Mbps
-    //can1.attach(&rxISR1);                 // attach 'CAN receive-complete' interrupt handler
-    can1.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
-    can2.frequency(1000000);                     // set bit rate to 1Mbps
-    //can2.attach(&rxISR2);                 // attach 'CAN receive-complete' interrupt handler
-    can2.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); //set up can filter
-    
-    memset(&tx_buff, 0, TX_LEN * sizeof(uint16_t));
-    memset(&spi_data, 0, sizeof(spi_data_t));
-    memset(&spi_command,0,sizeof(spi_command_t));
-    
-    
-    NVIC_SetPriority(TIM5_IRQn, 1);
-    //NVIC_SetPriority(CAN1_RX0_IRQn, 3);
-    //NVIC_SetPriority(CAN2_RX0_IRQn, 3);
-    
-    printf("\n\r SPIne\n\r");
-    //printf("%d\n\r", RX_ID << 18);
-    
-    a1_can.len = 8;                         //transmit 8 bytes
-    a2_can.len = 8;                         //transmit 8 bytes
-    h1_can.len = 8;
-    h2_can.len = 8;
-    k1_can.len = 8;
-    k2_can.len = 8;
-    rxMsg1.len = 6;                          //receive 6 bytes
-    rxMsg2.len = 6;                          //receive 6 bytes
-
-    a1_can.id = 0x1;                        
-    a2_can.id = 0x1;                 
-    h1_can.id = 0x2;
-    h2_can.id = 0x2;
-    k1_can.id = 0x3;
-    k2_can.id = 0x3;     
-
-    pack_cmd(&a1_can, l1_control.a); 
-    pack_cmd(&a2_can, l2_control.a); 
-    pack_cmd(&h1_can, l1_control.h); 
-    pack_cmd(&h2_can, l2_control.h); 
-    pack_cmd(&k1_can, l1_control.k); 
-    pack_cmd(&k2_can, l2_control.k); 
-    WriteAll();
-
-
-    // SPI doesn't work if enabled while the CS pin is pulled low
-    // Wait for CS to not be low, then enable SPI
-    if(!spi_enabled){   
-        while((spi_enabled==0) && (cs.read() ==0)){wait_us(10);}
-        init_spi();
-        spi_enabled = 1;
-        }
-            
-    while(1) {
-        counter++;
-        can2.read(rxMsg2);
-        unpack_reply(rxMsg2, &l2_state);
-        can1.read(rxMsg1);                    // read message into Rx message storage
-        unpack_reply(rxMsg1, &l1_state);
-        wait_us(10);
-
-        }
-        
-
-        
-        
-    }
-    
-
-
-
-
+}
\ No newline at end of file

--- a/math_ops.cpp	Wed May 02 18:19:22 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,48 +0,0 @@
-
-#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;
-    }

--- a/math_ops.h	Wed May 02 18:19:22 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,16 +0,0 @@
-#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/math_ops/math_ops.cpp	Tue Oct 08 04:35:23 2019 +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 Oct 08 04:35:23 2019 +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