File content as of revision 10:f94e325fc1e6:

#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 e_state=e_REST_MODE; 
int w_state=w_REST_MODE; 
int duoji_state= DUOJI_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[3]={0};
int data_num=0;                                          
void serial_command_isr()
{   
    while(command.readable())  //从Matlab接收到命令（字符）
    {
        char c =command.getc();//将从matlab接收的命令赋值到c
         data[data_num]=c;//把matlab的命令放入数组中，两个电机，数组有两个变量
         data_num++;
         if(data_num>=3)
         data_num=0;
        if(data[0]=='e')//matlab第一个字符（命令）输入‘e'电机复位停止工作//肘关节电机
        {
            e_state=e_REST_MODE;
            send_enable=0;
            printf("\n\r ejoint Motor Reset Mode \r");    
        }
        if(data[1]=='e')//matlab第二个字符（命令）输入‘e'电机复位停止工作//腕关节电机
        {
            w_state=w_REST_MODE;
            send_enable=0;
            printf("\n\r wjoint Motor Reset Mode \r");    
        }
        
          if(data[2]=='k') //matlab第三个字符（命令）手爪舵机,输入'k'舵机进入工作准备状态
        {
            duoji_state= DUOJI_MODE ;
              
        }
       
        
        
        if((e_state==e_REST_MODE)|| (e_state==e_REST_MODE))      //matlab第二次数据（指上一次为复位状态）
            {
                switch(data[0]) //肘关节数据命令
             {
                case('m'):
                 printf("\n\rMotor entering E motor mode\r");
                 EnterMotorMode(&EF_can);                                            // 电机位置锁定 
                 send_enable = 0; 
                 e_state=MOTOR_MODE; 
                break;
            
                case('z'):
                 printf("\n\rMotor zeroing\r");
                 Zero(&EF_can);
                 send_enable = 0;
                  e_state=ZERO_MODE; 
                  break;  
              }
               switch(data[1]) //腕关节数据命令
             {
                case('m'):
                 printf("\n\rMotor entering E motor mode\r");
                 EnterMotorMode(&WF_can);                                            // 电机位置锁定 
                 send_enable = 0; 
                 w_state=MOTOR_MODE; 
                break;
            
                case('z'):
                 printf("\n\rMotor zeroing\r");
                 Zero(&WF_can);
                 send_enable = 0;
                  w_state=ZERO_MODE; 
                  break;  
              }
             }
       if((e_state==MOTOR_MODE)&&(data[0]!='m'))//肘关节数据判断成功，电机跟随位置转动
        { 
           //a_control.ef.p_des=int(c);//*2*PI/360;
           a_control.ef.p_des=int(data[0])*2*PI/360;//matlab给定的肘关节角度信息
            send_enable = 1;
            printf("f: %.3f\n\r", a_control.ef.p_des);
            }
            
       if((w_state==MOTOR_MODE)&&(data[1]!='m'))//腕关节数据判断成功，电机跟随位置转动
        { 
           a_control.wf.p_des=int(data[1])*2*PI/360;//matlab给定的腕关节角度信息
            send_enable = 1;
            printf("f: %.3f\n\r", a_control.wf.p_des);
            }
        if((duoji_state==DUOJI_MODE)&&(data[2]!='k'))//腕关节数据判断成功，电机跟随位置转动
        { 
          duoji_control.p_des=int(data[2]);//matlab给定的舵机（手爪）的位置信息
          duoji_command=1;
          printf("\n\r Duoji Start\r"); //舵机开始运动到指定的位置
          printf("f: %.3f\n\r", duoji_control.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; //kp通过实验得到
   a_control.wf.kd= 0;  
   a_control.wf.t_ff= 0;
    
}