File content as of revision 2:cd74a8cb03b0:

#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);

    
    pf_can.frequency(1000000);
    pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    pf_rxMsg.len = 6;
    PF_can.len   = 8;
    PF_can.id    = 0x01;
    
    df_can.frequency(1000000);
    df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    df_rxMsg.len = 6;
    DF_can.len   = 8;
    DF_can.id    = 0x01;
    
/////////////////////////////////////position///////////////////////////////////////////
    wait(5);

    EnterMotorMode(&PF_can);
    EnterMotorMode(&DF_can);
    Zero(&PF_can);
    Zero(&DF_can);

    a_control.pf.p_des=PI/8;
    a_control.pf.v_des=0;
    a_control.pf.kp=7;
    a_control.pf.kd=0;
    a_control.pf.t_ff=0;

    a_control.df.p_des=PI/8;
    a_control.df.v_des=0;
    a_control.df.kp=7;
    a_control.df.kd=0;
    a_control.df.t_ff=0;


    PackAll();
    WriteAll();


    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);

        float pfkp = a_state.pf.p;                                                     // 从CAN获得的当前位置
        float pfkv = a_state.pf.v;
        float pfkt = a_state.pf.t;

        float dfp = a_state.df.p;
        float dfv = a_state.df.v;
        float dft = a_state.df.t;

        pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft);

///////////////////////////////////////velocity///////////////////////////////////////////////

//        a_control.pf.p_des=0;
//        a_control.pf.v_des=100*2*3.14/60/49;
//        a_control.pf.kp=0;
//        a_control.pf.kd=4;
//        a_control.pf.t_ff=0;
//
//        a_control.df.p_des=0;
//        a_control.df.v_des=100*2*3.14/60/49;
//        a_control.df.kp=0;
//        a_control.df.kd=4;
//        a_control.df.t_ff=0;
//        PackAll();
//        WriteAll();

////////////////////////////////////////////////////////////////////////////////////////////////
    }
}