File content as of revision 1:a71791b81b8a:

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

        float ankp = a_state.ankle_state.p;                                                     // 从CAN获得的当前位置
        float ankv = a_state.ankle_state.v;
        float ankt = a_state.ankle_state.t;

        float knp = a_state.knee_state.p;
        float knv = a_state.knee_state.v;
        float 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=100*2*3.14/60/49;
//        a_control.ankle.kp=0;
//        a_control.ankle.kd=4;
//        a_control.ankle.t_ff=0;
//
//        a_control.knee.p_des=0;
//        a_control.knee.v_des=100*2*3.14/60/49;
//        a_control.knee.kp=0;
//        a_control.knee.kd=4;
//        a_control.knee.t_ff=0;
//        PackAll();
//        WriteAll();

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