yezhong yezhong
1
Dependencies: mcp2515 mbed-dev-f303
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;
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+