File content as of revision 8:95a914f962bd:

#include "data_command.h"
#include "leg_message.h"

Serial      command(PC_12, PD_2);
//DigitalOut  sf_m_c(PC_12);
int e_num=0;
int w_num=0;
int enabled = 0;                                                                // 进入电机模式标志位
int counter = 0;                                                                // 输出计数器
int c_control = 0; 
int send_enable=0; 
int state=REST_MODE; 
int duoji_command=0;                                                           // 命令帧标志位
float SP_wf = 0, SP_ef = 0; 
int e_data[2]={0};
int w_data[2]={0};  
char data[2]={0};
int data_num=0;                                          
void serial_command_isr()
{   
    while(command.readable())
    {
        char c =command.getc();
         data[data_num]=c;
         data_num++;
         if(data_num>=2)
         data_num=0;
        if(data[0]=='e')
        {
            state=REST_MODE;
            send_enable=0;
            printf("\n\r Motor Reset Mode \r");    
        }
        
          if(data[1]=='k')
        {
            state= DUOJI_MODE ;
              
        }
        if(state==DUOJI_MODE)
        { 
        duoji_command=1;
        printf("\n\r Duoji Start\r"); 
            
            }
        else
        {
            duoji_command=0;
            }
        
        if(state==REST_MODE)
            {
                switch(data[0])
             {
                case('m'):
                 printf("\n\rMotor entering PC motor mode\r");
                 EnterMotorMode(&EF_can);                                            // 电机位置锁定 
                 send_enable = 0; 
                 state=MOTOR_MODE; 
                break;
            
                case('z'):
                 printf("\n\rMotor zeroing\r");
                 Zero(&EF_can);
                 send_enable = 0;
                  state=ZERO_MODE; 
                  break;  
              }
             }
       if((state==MOTOR_MODE)&&(data[0]!='m'))
        { 
           a_control.ef.p_des=int(c);//*2*PI/360;
            send_enable = 1;
            printf("f: %.3f\n\r", a_control.ef.p_des);
            }
           
        }

    }

  

void command_control()
{
   a_control.ef.v_des=0;  //v设置为0
   a_control.ef.kp=50;//0.0012;//kp通过实验得到
   a_control.ef.kd= 0;   
   a_control.ef.t_ff= 0;//forwade_torque=0;
   
   a_control.wf.v_des=0;
   a_control.wf.kp=0.0012; 
   a_control.wf.kd= 0;  
   a_control.wf.t_ff= 0;
    
}