File content as of revision 3:ac1062c23c49:

#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()
{
    Timer t;
    float b;
////////////////////////初始化//////////////////////////////////////
    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(800000);
    ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    ankle_rxMsg.len = 6;
    ankle_txMsg.len   = 8;
    ankle_txMsg.id    = 0x01;

    knee_can.frequency(800000);
    knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    knee_rxMsg.len = 6;
    knee_txMsg.len   = 8;
    knee_txMsg.id    = 0x01;
////////////////////////////////////////////////////////////////////////////////
    wait(8);

    Zero(&ankle_txMsg);
    EnterSPEEDMode(&ankle_txMsg);
    wait_us(100);
    Zero(&knee_txMsg);
    EnterSPEEDMode(&knee_txMsg);
    
   
    //EnterPositionMode(&knee_txMsg);
   // EnterPositionMode(&ankle_txMsg);
    //EnterMotorMode(&knee_txMsg);
   // EnterMotorMode(&ankle_txMsg);
    



    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);
       //pc.printf("%.3fa%.3fa%.3fa%.3fa%.3fa%.3f\n\r",ankp,ankv,ankt,knp,knv,knt);
       pc.printf("%.3fa%.3fa%.3fa%.3fa%.3fa%.3fa%.3f\n\r",a_control.knee.p_des,a_control.knee.v_des,a_control.knee.t_ff,knp,knv,knt,b);
     
        
//////////////////////////////////////////position/////////////////////////////////////////
 /* 
    t.start();
    t.read(); 
    b=t.read(); 
    a_control.ankle.p_des=PI*sin(t);
    //a_control.ankle.p_des=PI/8;
    a_control.ankle.v_des=0;
    a_control.ankle.kp=7;
    a_control.ankle.kd=5;
    a_control.ankle.t_ff=0;

    a_control.knee.p_des=PI*sin(0.5f*t);
    //a_control.knee.p_des=PI/4;
    a_control.knee.v_des=0;
    a_control.knee.kp=10.5;
    a_control.knee.kd=1;
    a_control.knee.t_ff=0;
    
    // if(a_control.knee.v_des>=0)
    //       a_control.knee.t_ff=2;
     //      else
       //     a_control.knee.t_ff=-2;
            
    PackAll();
    WriteAll();
*/
///////////////////////////////////////velocity///////////////////////////////////////////////
 
        t.start();
        t.read(); 
        
        a_control.ankle.p_des=0;
        a_control.ankle.v_des=-500*2*3.14/60/50;
        //a_control.ankle.v_des=(500*2*3.14/60/50)*sin(t);
        a_control.ankle.kp=10;
        a_control.ankle.kd=1;
        a_control.ankle.t_ff=0;

        a_control.knee.p_des=0;
        a_control.knee.v_des=500*2*3.14/60/50;
        //a_control.knee.v_des=(800*2*3.14/60/50)*sin(0.5f*t);
        a_control.knee.kp=10;
        a_control.knee.kd=1;
        a_control.knee.t_ff=0;
            
        //if(a_control.knee.v_des>=0)
       //    a_control.knee.t_ff=7.5;
        //   else
         //   a_control.knee.t_ff=-5;
           
        PackAll();
        WriteAll();     
        
      

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