Diff: DATA_COMMAND/data_command.cpp

Revision:

0:d80c66cb1b3a

Child:

2:cd74a8cb03b0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.cpp	Tue Nov 10 09:09:58 2020 +0000
@@ -0,0 +1,232 @@
+#include "data_command.h"
+#include "data_pc.h"
+#include "CAN.h"
+
+Serial      command(PA_9, PA_10);
+
+
+int enabled = 0;                                                                // 进入电机模式标志位
+int counter = 0;                                                                // 输出计数器
+int command_control_flag = 0;                                                   // 命令帧标志位
+float SP_pf = 0, SP_df = 0;
+int c_lock = 2;                                                                 // 位置锁定标志位 
+int return_zero = 0;                                                            // 电机回0标志位
+
+unsigned int flag_c_A = 0, flag_c_B = 0;
+unsigned int Snum_c = 0;
+uint16_t Sget[10] = {0};                                                    
+uint16_t Suse[10] = {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_c   = 0;
+            for(unsigned int i = 0; i < 10; i++)
+            {
+                Sget[i] = 0;      
+            }
+            
+            break;  
+        }
+        if(c == 'B')
+        {
+            flag_c_B = 1;
+        }
+    
+        if(flag_c_A == 1)
+        {
+            if((flag_c_B != 1) && (Snum_c < 10))
+            {
+                Sget[Snum_c] = c;    
+            }
+            
+            Snum_c++;
+            
+            if((flag_c_B == 1) && (Snum_c != 11))
+            {                        
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum_c   = 0;     
+            }
+            
+            if((flag_c_B == 1) && (Snum_c == 11))
+            {
+                flag_c_A = 0;
+                flag_c_B = 0;
+                Snum_c   = 0; 
+                
+                for(unsigned int i = 0; i < 10; i++)
+                {
+                    Suse[i] = Sget[i];
+                }  
+                
+                command_control_flag = 1;                                       // 命令帧接收成功
+                
+                pc.printf("%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]);
+            }
+        }
+    }    
+}
+
+
+void command_control()
+{
+    SP_pf = 10*(Suse[2] - '0') + 1*(Suse[3] - '0') + 0.1*(Suse[4] - '0');
+    SP_df = 10*(Suse[7] - '0') + 1*(Suse[8] - '0') + 0.1*(Suse[9] - '0');
+    if(Suse[1] == '+')
+        SP_pf = 0 + SP_pf;
+    if(Suse[1] == '-')
+        SP_pf = 0 - SP_pf;
+    if(Suse[6] == '+')
+        SP_df = 0 + SP_df;
+    if(Suse[6] == '-')
+        SP_df = 0 - SP_df;
+    
+    
+    // 切换为发送模式
+    wait_us(200);
+    
+    switch(Suse[0])
+    {
+        case('e'):
+            command.printf("\n\rPF exiting motor mode\r");
+//          ExitMotorMode(&PF_can);
+            ExitMotorMode(&knee_txMsg);           
+            ExitMotorMode(&ankle_txMsg);
+            return_zero = 0;                                                    // 停止回0
+            c_lock = 2;                                                         // 电机位置锁无效
+//            send_enable = 0;                                                    // main不发送CAN位置命令
+            break;
+        case('m'):
+            command.printf("\n\rPF entering PC motor mode\r");
+//            EnterMotorMode(&PF_can);
+            EnterMotorMode(&knee_txMsg);           
+            EnterMotorMode(&ankle_txMsg);
+
+            return_zero = 0;
+            c_lock = 1;                                                         // 电机位置锁定 
+//            send_enable = 1;                                                    // main发送CAN位置命令
+            break;
+        case('M'):
+            command.printf("\n\rPF entering BOARD motor mode\r");
+//            EnterMotorMode(&PF_can);
+            EnterMotorMode(&knee_txMsg);           
+            EnterMotorMode(&ankle_txMsg);
+            return_zero = 0;
+            c_lock = 0;                                                         // 电机位置解锁
+//            send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定 
+            break;
+        case('z'):
+            command.printf("\n\rPF zeroing\r");
+//            Zero(&PF_can);
+            Zero(&knee_txMsg);
+            Zero(&ankle_txMsg);
+            return_zero = 0; 
+            c_lock = 2;
+//            send_enable = 0;
+            break;
+        case('r'):
+            command.printf("\n\rPF return zero\r");
+//            EnterMotorMode(&PF_can);
+            EnterMotorMode(&knee_txMsg);           
+            EnterMotorMode(&ankle_txMsg);
+            return_zero = 1;
+            c_lock = 2;
+//            send_enable = 1;
+//            flag_gd = 1;
+            break;      
+    }
+    /*
+    switch(Suse[5])
+    {
+        case('e'):
+            command.printf("DF exiting motor mode\n\r");
+            ExitMotorMode(&DF_can);
+            c_lock = 1;                                                         // 电机位置解锁
+            send_enable = 0;                                                    // main不发送CAN位置命令
+            break;
+        case('m'):
+            command.printf("DF entering PC motor mode\n\r");
+            EnterMotorMode(&DF_can);
+            c_lock = 1;                                                         // 电机位置锁定
+            send_enable = 1;                                                    // main发送CAN位置命令
+            break;
+        case('M'):
+            command.printf("DF entering BOARD motor mode\n\r");
+            EnterMotorMode(&DF_can);
+            c_lock = 0;                                                         // 电机位置解锁
+            send_enable = 0;                                                    // M模式下，命令是否发送，命令是什么由calculate决定
+            break;
+        case('z'):
+            command.printf("DF zeroing\n\r");
+            Zero(&DF_can);
+            break;   
+    }
+    */
+    
+    wait_ms(2);
+    
+//    sf_m_c = 0;                                                                 // 恢复接收模式
+    wait_us(200);
+    
+    
+    
+    // 电机模式命令在此发送，锁定控制模式的位置量在main中发送
+    WriteAll();
+    //wait(1.0f);                        
+    
+    // 解除串口控制，缓冲区清0
+    command_control_flag = 0;
+    for(unsigned int i = 0; i < 10; i++)
+    {
+        Suse[i] = 0;      
+    }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+