Diff: DATA_COMMAND/data_command.cpp

Revision:

10:f94e325fc1e6

Parent:

9:bf02fd2d7a0a

--- a/DATA_COMMAND/data_command.cpp	Wed Feb 19 05:22:32 2020 +0000
+++ b/DATA_COMMAND/data_command.cpp	Thu Mar 12 15:17:18 2020 +0000
@@ -9,79 +9,102 @@
 int counter = 0;                                                                // 输出计数器
 int c_control = 0; 
 int send_enable=0; 
-int state=REST_MODE; 
+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[2]={0};
+char data[3]={0};
 int data_num=0;                                          
 void serial_command_isr()
 {   
-    while(command.readable())
+    while(command.readable())  //从Matlab接收到命令（字符）
     {
-        char c =command.getc();
-         data[data_num]=c;
+        char c =command.getc();//将从matlab接收的命令赋值到c
+         data[data_num]=c;//把matlab的命令放入数组中，两个电机，数组有两个变量
          data_num++;
-         if(data_num>=2)
+         if(data_num>=3)
          data_num=0;
-        if(data[0]=='e')
+        if(data[0]=='e')//matlab第一个字符（命令）输入‘e'电机复位停止工作//肘关节电机
         {
-            state=REST_MODE;
+            e_state=e_REST_MODE;
             send_enable=0;
-            printf("\n\r Motor Reset Mode \r");    
+            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[1]=='k')
+          if(data[2]=='k') //matlab第三个字符（命令）手爪舵机,输入'k'舵机进入工作准备状态
         {
-            state= DUOJI_MODE ;
+            duoji_state= DUOJI_MODE ;
               
         }
-        if(state==DUOJI_MODE)
-        { 
-        duoji_command=1;
-        printf("\n\r Duoji Start\r"); 
-            
-            }
-        else
-        {
-            duoji_command=0;
-        }
+       
+        
         
-        if(state==REST_MODE)
+        if((e_state==e_REST_MODE)|| (e_state==e_REST_MODE))      //matlab第二次数据（指上一次为复位状态）
             {
-                switch(data[0])
+                switch(data[0]) //肘关节数据命令
              {
                 case('m'):
-                 printf("\n\rMotor entering PC motor mode\r");
+                 printf("\n\rMotor entering E motor mode\r");
                  EnterMotorMode(&EF_can);                                            // 电机位置锁定 
                  send_enable = 0; 
-                 state=MOTOR_MODE; 
+                 e_state=MOTOR_MODE; 
                 break;
             
                 case('z'):
                  printf("\n\rMotor zeroing\r");
                  Zero(&EF_can);
                  send_enable = 0;
-                  state=ZERO_MODE; 
+                  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((state==MOTOR_MODE)&&(data[0]!='m'))
+       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;
+           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((state==MOTOR_MODE)&&(data[2]!='m'))
+       if((w_state==MOTOR_MODE)&&(data[1]!='m'))//腕关节数据判断成功，电机跟随位置转动
         { 
-           a_control.wf.p_des=int(data[2])*2*PI/360;
+           a_control.wf.p_des=int(data[1])*2*PI/360;//matlab给定的腕关节角度信息
             send_enable = 1;
-            printf("f: %.3f\n\r", a_control.ef.p_des);
+            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);//显示舵机的角度信息
+            }
+            }
         }
 
     }
@@ -96,7 +119,7 @@
    a_control.ef.t_ff= 0;//forwade_torque=0;
    
    a_control.wf.v_des=0;
-   a_control.wf.kp=0.0012; 
+   a_control.wf.kp=0.0012; //kp通过实验得到
    a_control.wf.kd= 0;  
    a_control.wf.t_ff= 0;