File content as of revision 10:f83b0935a8f0:

#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"


////////////////////////////////////////////////////////////////////////////////
//                             框架搭建完毕                                     //
////////////////////////////////////////////////////////////////////////////////


int main() 
{
     Timer tim;
     //float a=PI/8;
    //float j=0.558,P=0;
////////////////////////初始化//////////////////////////////////////
    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(800000);
    pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    pf_rxMsg.len = 6;
    PF_can.len   = 8;
    PF_can.id    = 0x02;
    
    df_can.frequency(800000);
    df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    df_rxMsg.len = 6;
    DF_can.len   = 8;    
    DF_can.id    = 0x02;
    
/////////////////////////////////////position///////////////////////////////////////////
    wait(6);
      Zero(&PF_can);
      EnterMotorMode(&PF_can);
       
      Zero(&DF_can);//0位置
      EnterMotorMode(&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);
       // pc.printf("%.4f,%.3f,%.3f,%.4f,%.3f,%.4f\n",pfkp,pfkv,pfkt,a_control.pf.p_des,j,P);
        
       pc.printf("%.3f,%.3f,%.3f,%.3f\n",pfkp,pfkv,pfkt,a_control.pf.v_des); 
 /////////////////////////////////////////////////trajectory/////////////////////////////////////////////////////
     /* 
        j=j+0.001;
        P=0.33+(-0.2148)*cos((j+0.1)*10.44)+(-0.1549)*sin((j+0.1)*10.44)+(-0.07744)*cos(2*(j+0.1)*10.44)+(-0.04255)*sin(2*(j+0.1)*10.44);       
        if(j>=0.558&&j<=0.85)
        {
             a_control.pf.p_des=P;
             a_control.pf.v_des=0; 
             a_control.pf.kp=80;
             a_control.pf.kd=0;
             a_control.pf.t_ff=0;
        }
       
        
         PackAll();
        WriteAll();
     */

        
/////////////////////////////////////position///////////////////////////////////////////        
     /*
        a_control.pf.p_des=PI/8;
        a_control.pf.v_des=0;
        a_control.pf.kp=10;
        a_control.pf.kd=0;
        a_control.pf.t_ff=0; 
        PackAll();
        WriteAll();
    */
        
    /*
     t.start();
     t.read(); 
       if(t.read()<2)        
          {
          a_control.pf.p_des=a;
          a_control.pf.v_des=0;
          a_control.pf.kp=10;
          a_control.pf.kd=0;
          a_control.pf.t_ff=0;  
          }                 
     if(t.read()>3) 
     {
          t.reset();
          Zero(&PF_can);
          a=-a;
         }   
    
        PackAll();
        WriteAll();
        
        */
        
  
          
        
 /////////////////////////////////////////////////////////////////////////////////////////////       

///////////////////////////////////////velocity///////////////////////////////////////////////
    tim.start(); 
//    float timeRecord()
    float s =tim.read();
    if(s >= 10.0f) {
            tim.reset();
       }
   
        a_control.pf.p_des=0;
        a_control.pf.v_des=500*2*3.14/60/50*(sin(3.14/5*s)+1);
        a_control.pf.kp=0;
        a_control.pf.kd=5;
        a_control.pf.t_ff=0;

        a_control.df.p_des=0;
         a_control.df.v_des=100*2*3.14/60/50*s;
//        a_control.df.v_des=500*2*3.14/60/50;//50减速比，
        a_control.df.kp=0;
        a_control.df.kd=5;
        a_control.df.t_ff=0;
        
        PackAll();
        WriteAll();       

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