12
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; + } +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +