Shao Rui
1
Diff: DATA_COMMAND/data_command.cpp
- Revision:
- 5:6a95726e45b0
- Child:
- 6:aad89fd109c2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/DATA_COMMAND/data_command.cpp Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,202 @@
+#include "data_command.h"
+
+
+Serial command(PC_10, PC_11);
+DigitalOut sf_m_c(PC_12);
+
+int enabled = 0; // 进入电机模式标志位
+int counter = 0; // 输出计数器
+int c_control = 0; // 命令帧标志位
+float SP_pf = 0, SP_df = 0;
+int c_lock = 2; // 位置锁定标志位
+
+unsigned int flag_A = 0, flag_B = 0;
+unsigned int Snum = 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_A = 1;
+
+ flag_B = 0;
+ Snum = 0;
+ for(unsigned int i = 0; i < 10; i++)
+ {
+ Sget[i] = 0;
+ }
+
+ break;
+ }
+ if(c == 'B')
+ {
+ flag_B = 1;
+ }
+
+ if(flag_A == 1)
+ {
+ if((flag_B != 1) && (Snum < 10))
+ {
+ Sget[Snum] = c;
+ }
+
+ Snum++;
+
+ if((flag_B == 1) && (Snum != 11))
+ {
+ flag_A = 0;
+ flag_B = 0;
+ Snum = 0;
+ }
+
+ if((flag_B == 1) && (Snum == 11))
+ {
+ flag_A = 0;
+ flag_B = 0;
+ Snum = 0;
+
+ for(unsigned int i = 0; i < 10; i++)
+ {
+ Suse[i] = Sget[i];
+ }
+
+ c_control = 1; // 命令帧接收成功
+
+ //pc.printf("%c%c%c%c%c%c%c%c%c%c\n\r", S340use[0], S340use[1], S340use[2], S340use[3], S340use[4], S340use[5], S340use[6], S340use[7], S340use[8], S340use[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;
+
+
+ sf_m_c = 1; // 切换为发送模式
+ wait_us(200);
+
+ switch(Suse[0])
+ {
+ case('e'):
+ command.printf("\n\rPF exiting motor mode\r");
+ ExitMotorMode(&PF_can);
+ c_lock = 1; // 电机位置解锁
+ send_enable = 0; // main不发送CAN位置命令
+ break;
+ case('m'):
+ command.printf("\n\rPF entering PC motor mode\r");
+ EnterMotorMode(&PF_can);
+ c_lock = 1; // 电机位置锁定
+ send_enable = 1; // main发送CAN位置命令
+ break;
+ case('M'):
+ command.printf("\n\rPF entering BOARD motor mode\r");
+ EnterMotorMode(&PF_can);
+ c_lock = 0; // 电机位置解锁
+ send_enable = 0; // M模式下，命令是否发送，命令是什么由calculate决定
+ break;
+ case('z'):
+ command.printf("\n\rPF zeroing\r");
+ Zero(&PF_can);
+ 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
+ c_control = 0;
+ for(unsigned int i = 0; i < 10; i++)
+ {
+ Suse[i] = 0;
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+