data_command.cpp

00001 #include "data_command.h"
00002 #include "leg_message.h"
00003 
00004 Serial      command(PC_12, PD_2);
00005 //DigitalOut  sf_m_c(PC_12);
00006 int e_num=0;
00007 int w_num=0;
00008 int enabled = 0;                                                                // 进入电机模式标志位
00009 int counter = 0;                                                                // 输出计数器
00010 int c_control = 0; 
00011 int send_enable=0; 
00012 int e_state=e_REST_MODE; 
00013 int w_state=w_REST_MODE; 
00014 int duoji_state= DUOJI_REST__MODE ;
00015 int duoji_command=0;                                                           // 命令帧标志位
00016 float SP_wf = 0, SP_ef = 0; 
00017 int e_data[2]={0};
00018 int w_data[2]={0};  
00019 char data[3]={0};
00020 int data_num=0;                                          
00021 void serial_command_isr()
00022 {   
00023     while(command.readable())  //从Matlab接收到命令（字符）
00024     {
00025         char c =command.getc();//将从matlab接收的命令赋值到c
00026          data[data_num]=c;//把matlab的命令放入数组中，两个电机，数组有两个变量
00027          data_num++;
00028          if(data_num>=3)
00029          data_num=0;
00030         if(data[0]=='e')//matlab第一个字符（命令）输入‘e'电机复位停止工作//肘关节电机
00031         {
00032             e_state=e_REST_MODE;
00033             send_enable=0;
00034             printf("\n\r ejoint Motor Reset Mode \r");    
00035         }
00036         if(data[1]=='e')//matlab第二个字符（命令）输入‘e'电机复位停止工作//腕关节电机
00037         {
00038             w_state=w_REST_MODE;
00039             send_enable=0;
00040             printf("\n\r wjoint Motor Reset Mode \r");    
00041         }
00042         
00043           if(data[2]=='k') //matlab第三个字符（命令）手爪舵机,输入'k'舵机进入工作准备状态
00044         {
00045             duoji_state= DUOJI_MODE ;
00046               
00047         }
00048        
00049         
00050         
00051         if((e_state==e_REST_MODE)|| (e_state==e_REST_MODE))      //matlab第二次数据（指上一次为复位状态）
00052             {
00053                 switch(data[0]) //肘关节数据命令
00054              {
00055                 case('m'):
00056                  printf("\n\rMotor entering E motor mode\r");
00057                  EnterMotorMode(&EF_can);                                            // 电机位置锁定 
00058                  send_enable = 0; 
00059                  e_state=MOTOR_MODE; 
00060                 break;
00061             
00062                 case('z'):
00063                  printf("\n\rMotor zeroing\r");
00064                  Zero(&EF_can);
00065                  send_enable = 0;
00066                   e_state=ZERO_MODE; 
00067                   break;  
00068               }
00069                switch(data[1]) //腕关节数据命令
00070              {
00071                 case('m'):
00072                  printf("\n\rMotor entering E motor mode\r");
00073                  EnterMotorMode(&WF_can);                                            // 电机位置锁定 
00074                  send_enable = 0; 
00075                  w_state=MOTOR_MODE; 
00076                 break;
00077             
00078                 case('z'):
00079                  printf("\n\rMotor zeroing\r");
00080                  Zero(&WF_can);
00081                  send_enable = 0;
00082                   w_state=ZERO_MODE; 
00083                   break;  
00084               }
00085              }
00086        if((e_state==MOTOR_MODE)&&(data[0]!='m'))//肘关节数据判断成功，电机跟随位置转动
00087         { 
00088            //a_control.ef.p_des=int(c);//*2*PI/360;
00089            a_control.ef.p_des=int(data[0])*2*PI/360;//matlab给定的肘关节角度信息
00090             send_enable = 1;
00091             printf("f: %.3f\n\r", a_control.ef.p_des);
00092             }
00093             
00094        if((w_state==MOTOR_MODE)&&(data[1]!='m'))//腕关节数据判断成功，电机跟随位置转动
00095         { 
00096            a_control.wf.p_des=int(data[1])*2*PI/360;//matlab给定的腕关节角度信息
00097             send_enable = 1;
00098             printf("f: %.3f\n\r", a_control.wf.p_des);
00099             }
00100         if((duoji_state==DUOJI_MODE)&&(data[2]!='k'))//腕关节数据判断成功，电机跟随位置转动
00101         { 
00102           shouzhua_control.p_des=int(data[2]);//matlab给定的舵机（手爪）的位置信息
00103           duoji_command=1;
00104           printf("\n\r Duoji Start\r"); //舵机开始运动到指定的位置
00105           printf("f: %.3f\n\r", shouzhua_control.p_des);//显示舵机的角度信息
00106             }
00107             
00108         }
00109 
00110     }
00111 
00112   
00113 
00114 void command_control()
00115 {
00116    a_control.ef.v_des=0;  //v设置为0
00117    a_control.ef.kp=50;//0.0012;//kp通过实验得到
00118    a_control.ef.kd= 0;   
00119    a_control.ef.t_ff= 0;//forwade_torque=0;
00120    
00121    a_control.wf.v_des=0;
00122    a_control.wf.kp=0.0012; //kp通过实验得到
00123    a_control.wf.kd= 0;  
00124    a_control.wf.t_ff= 0;
00125     
00126 }
00127 
00128 
00129 
00130 
00131 
00132 
00133 
00134 
00135 
00136 
00137 
00138 
00139 
00140 
00141 
00142 
00143 
00144 
00145 
00146 
00147 
00148 
00149 
00150 
00151 
00152 
00153 
00154 
00155 
00156 
00157 
00158 
00159 
00160 
00161 
00162 
00163 
00164 
00165 
00166