Diff: DATA_COMMAND/data_command.cpp

Revision:

0:dd5d4837292c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.cpp	Wed Jun 09 01:41:45 2021 +0000
@@ -0,0 +1,162 @@
+#include "data_command.h"
+#include "data_pc.h"
+#include "CAN.h"
+
+Serial      command(PC_10, PC_11);                                              // U3
+
+
+int enabled = 0;                                                                // 进入电机模式标志位
+int counter = 0;                                                                // 输出计数器
+int command_control_flag = 0;                                                   // 命令帧标志位
+
+float  v_pid = 0;
+float  p_pid = 0;
+float  kp_pid = 0;
+float  kd_pid = 0;
+float  tff_pid = 0;
+
+int c_lock = 0;                                                                 // 位置锁定标志位 
+
+
+unsigned int flag_c_A = 0, flag_c_B = 0;
+unsigned int Snum = 0;
+uint16_t Sget[14] = {0};                                                    
+uint16_t Suse[14] = {0};
+
+
+void serial_command_isr()
+{
+    
+    while(command.readable())
+    {
+        uint8_t c = command.getc();
+        if(c == 'A')
+        {
+            flag_c_A = 1;           
+            flag_c_B = 0;
+            Snum   = 0;
+            for(unsigned int i = 0; i < 14; i++)
+            {
+                Sget[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 'B')
+        {
+            flag_c_B = 1;
+        }
+    
+        if(flag_c_A == 1)
+        {
+            if((flag_c_B != 1) && (Snum < 14))
+            {
+                Sget[Snum] = c;    
+            }
+            
+            Snum++;
+            
+            if((flag_c_B == 1) && (Snum != 15))
+            {                        
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum   = 0;     
+            }
+            
+            if((flag_c_B == 1) && (Snum == 15))
+            {
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum   = 0;
+                
+                for(unsigned int i = 0; i < 14; i++)
+                {
+                    Suse[i] = Sget[i];
+                }  
+                
+                command_control_flag = 1;                                                  // 命令帧接收成功
+                
+//                command.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c\n\r", Suse[0], Suse[1], Suse[2], Suse[3], Suse[4], Suse[5], Suse[6], Suse[7], Suse[8], Suse[9], Suse[10], Suse[11], Suse[12], Suse[13]);
+            }
+        }
+    }       
+}
+
+
+void command_control()
+{
+    v_pid = 100*(Suse[1] - '0') + 10*(Suse[2] - '0') + (Suse[3] - '0');
+    p_pid = (Suse[4] - '0') + 0.1*(Suse[5] - '0') + 0.01*(Suse[6] - '0');
+    kp_pid = 100*(Suse[7] - '0') + 10*(Suse[8] - '0') + (Suse[9] - '0');
+    kd_pid = (Suse[10] - '0') + 0.1*(Suse[11] - '0');
+    tff_pid = (Suse[12] - '0') + 0.1*(Suse[13] - '0');
+
+    
+    command.printf("%.0f\t%.2f\t%.0f\t%.2f\t%.2f\n", v_pid, p_pid, kp_pid, kd_pid, tff_pid);   //Am0001000100000B
+    
+    wait_us(200); 
+    
+      
+    switch(Suse[0])
+    {
+        case('m'):
+            command.printf("\n\rCommand motor mode\r");                         //Am+010+010B  旋转1rad
+            c_lock = 1;  
+            send_enable = 1;  
+//            EnterMotorMode(&knee_txMsg);
+//            EnterMotorMode(&ankle_txMsg);                                                  // main发送CAN位置命令
+            break; 
+    }   
+    
+    // 电机模式命令在此发送，锁定控制模式的位置量在main中发送   运行到此处 进入/退出/设置零点位置 命令发送完成                 
+    // 解除串口控制，缓冲区清0
+    wait_ms(2);
+    
+    command_control_flag = 0;                                                   //避免下次继续进入command_control函数
+
+    for(unsigned int i = 0; i < 9; i++)
+    {
+        Suse[i] = 0;      
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+