File content as of revision 7:d1b09098579b:

#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"
#include "Moving_Average.h"


#include "CAN3.h"
#include "mcp2515.h"
#include <sstream>
#include <algorithm>


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


int main()
{
     Timer t;
     //float a=PI/8;
    //float j=0.558,P=0;
   
    
////////////////////////初始化//////////////////////////////////////
    pc.baud(115200);                                           //串口打印信息  U2
    pc.attach(&serial_pc_isr);

    //foot.baud(115200);                                          //接收鞋垫信息   U1
   // foot.attach(&serial_board_isr);

//   command.baud(115200);                                      //485通信     U3
//   command.attach(&serial_command_isr);

     zitai_foot.baud(115200);                                      //U4
     zitai_foot.attach(&serial_zitai_foot_isr);
     
    
    pf_can.frequency(800000);
    pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    pf_rxMsg.len = 6;
    pf_txMsg.len   = 8;
    pf_txMsg.id    = 0x01;
    
    df_can.frequency(800000);
    df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
    df_rxMsg.len = 6;
    df_txMsg.len   = 8;
    df_txMsg.id    = 0x02;
    
    df1_rxMsg.len = 6;
    df1_txMsg.len =8;
    df1_txMsg.id =0x03;
    df1_can.frequency(800000);
    
    NVIC_SetPriority(USART1_IRQn, 1);                                           // command中断优先级高于board
    //NVIC_SetPriority(USART3_IRQn, 3);
    
    
    
/////////////////////////////////////position///////////////////////////////////////////
    wait(8);
     Zero(&pf_txMsg);
     // EnterSPEEDMode(&pf_txMsg);
      
     //Zero(&df_txMsg);
     // EnterSPEEDMode(&df_txMsg);
      
      Zero(&df1_txMsg);
      EnterSPEEDMode(&df1_txMsg);
    
    EnterPositionMode(&pf_txMsg);
    //EnterPositionMode(&df_txMsg);  
    EnterPositionMode(&df1_txMsg);
   
      
    //EnterMotorMode(&pf_txMsg);
    //EnterMotorMode(&df_txMsg);
      //EnterMotorMode(&df1_txMsg);
    



    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);
        wait_us(10);
        df1_can.read(&df1_rxMsg);
        unpack_reply(df1_rxMsg, &a_state);

        float pfp = a_state.pf.p;                                                     // 从CAN获得的当前位置
        float pfv = a_state.pf.v;
        float pft = a_state.pf.t;

        float dfp = a_state.df.p;
        float dfv = a_state.df.v;
        float dft = a_state.df.t;
        
        float df1p = a_state.df1.p;
        float df1v = a_state.df1.v;
        float df1t = a_state.df1.t;
    
    
    
       ///////////////////////pf拉力////////////////////////// 
       La_pf_real = LaLi_pf.read()*3.3f*1000;       //读取电压值；单位为mV;
       pf_filter = Moving_Average(10, Ffilter_pf, La_pf_real);
       F_pf=0.1125f*pf_filter+2.141f;
       //F_pf=0.1139*pf_filter;
       La_df_real = LaLi_df.read();
       La_df1_real = LaLi_df1.read();

        //pc.printf("%.3fa%.3f\r\n",F_pf,pfp);     //拉力
         
               
        //pc.printf("%.3f\r\n",dfp);
         //wait(0.1);
       // 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("%.3fa%.3fa%.3fa%.3fa%.3fa%.3f\n\r",pfv,dfv,df1v,a_control.pf.v_des,a_control.df.v_des,a_control.df1.v_des); 
       //pc.printf("%.3fa%.3f\n\r",df1p,a_control.df1.p_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();
     */
        t.start();
        t.read(); 
    /*
      
       if(t.read()<2)
       {  
        a_control.pf.p_des=-PI/32;
        a_control.pf.v_des=0;
        a_control.pf.kp=10;
        a_control.pf.kd=5;
        a_control.pf.t_ff=0;
       }
       if(t.read()>2&&t.read()<4)
        {  
        a_control.pf.p_des=0;
        a_control.pf.v_des=0;
        a_control.pf.kp=10;
        a_control.pf.kd=5;
        a_control.pf.t_ff=0;
       }
        */
        a_control.pf.p_des=PI/32;
        a_control.pf.v_des=0;
        a_control.pf.kp=10;
        a_control.pf.kd=5;
        a_control.pf.t_ff=0;
        
        
        
        a_control.df.p_des=PI/4;
        a_control.df.v_des=0;
        a_control.df.kp=10;
        a_control.df.kd=5;
        a_control.df.t_ff=0;
        
        if(t.read()<2)
        {
        a_control.df1.p_des=PI/4;
        a_control.df1.v_des=0;
        a_control.df1.kp=30;
        a_control.df1.kd=5;
        a_control.df1.t_ff=0;
        }
        
        if(t.read()>2&&t.read()<4)
        {
        a_control.df1.p_des=PI/8;
        a_control.df1.v_des=0;
        a_control.df1.kp=35;
        a_control.df1.kd=5;
        a_control.df1.t_ff=0;
        }
    /*    
        if(t.read()>4&&t.read()<6)
        {
        t.reset();
        }
     */   
        
         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///////////////////////////////////////////////
  /*
        t.start();
        t.read(); 


        a_control.pf.p_des=0;
        a_control.pf.v_des=-500*2*3.14/60/50;
        a_control.pf.kp=10;
        a_control.pf.kd=5;
        a_control.pf.t_ff=0;

        a_control.df.p_des=0;
        a_control.df.v_des=-500*2*3.14/60/50;
        a_control.df.kp=10;
        a_control.df.kd=5;
        a_control.df.t_ff=0;
        
        a_control.df1.p_des=0;
        a_control.df1.v_des=500*2*3.14/60/50;
        //a_control.df1.v_des=(500*2*3.14/60/50)*sin(0.5f*t);
        a_control.df1.kp=10;
        a_control.df1.kd=5;
        a_control.df1.t_ff=0;
        PackAll();
        WriteAll();       
*/
////////////////////////////////////////////////////////////////////////////////////////////////
    }
}