Shao Rui
1
Diff: DATA_COMMAND/data_command.cpp
- Revision:
- 8:95a914f962bd
- Parent:
- 6:aad89fd109c2
- Child:
- 9:bf02fd2d7a0a
--- a/DATA_COMMAND/data_command.cpp Tue Dec 10 09:23:12 2019 +0000
+++ b/DATA_COMMAND/data_command.cpp Tue Jan 07 09:23:24 2020 +0000
@@ -1,179 +1,97 @@
#include "data_command.h"
-
+#include "leg_message.h"
Serial command(PC_12, PD_2);
//DigitalOut sf_m_c(PC_12);
-
+int e_num=0;
+int w_num=0;
int enabled = 0; // 进入电机模式标志位
int counter = 0; // 输出计数器
-int c_control = 0; // 命令帧标志位
-float SP_pf = 0, SP_df = 0;
-int c_lock = 2; // 位置锁定标志位
-int return_zero = 0; // 电机回0标志位
-
-unsigned int flag_A = 0, flag_B = 0;
-unsigned int Snum = 0;
-uint16_t Sget[10] = {0};
-uint16_t Suse[10] = {0};
-
-
+int c_control = 0;
+int send_enable=0;
+int state=REST_MODE;
+int duoji_command=0; // 命令帧标志位
+float SP_wf = 0, SP_ef = 0;
+int e_data[2]={0};
+int w_data[2]={0};
+char data[2]={0};
+int data_num=0;
void serial_command_isr()
-{
+{
while(command.readable())
{
- uint8_t c = command.getc();
- if(c == 'A')
+ char c =command.getc();
+ data[data_num]=c;
+ data_num++;
+ if(data_num>=2)
+ data_num=0;
+ if(data[0]=='e')
{
- flag_A = 1;
-
- flag_B = 0;
- Snum = 0;
- for(unsigned int i = 0; i < 10; i++)
- {
- Sget[i] = 0;
- }
-
- break;
+ state=REST_MODE;
+ send_enable=0;
+ printf("\n\r Motor Reset Mode \r");
}
- if(c == 'B')
+
+ if(data[1]=='k')
{
- flag_B = 1;
+ state= DUOJI_MODE ;
+
}
-
- if(flag_A == 1)
- {
- if((flag_B != 1) && (Snum < 10))
- {
- Sget[Snum] = c;
- }
+ if(state==DUOJI_MODE)
+ {
+ duoji_command=1;
+ printf("\n\r Duoji Start\r");
- Snum++;
-
- if((flag_B == 1) && (Snum != 11))
- {
- flag_A = 0;
- flag_B = 0;
- Snum = 0;
}
-
- if((flag_B == 1) && (Snum == 11))
+ else
+ {
+ duoji_command=0;
+ }
+
+ if(state==REST_MODE)
{
- 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]);
+ switch(data[0])
+ {
+ case('m'):
+ printf("\n\rMotor entering PC motor mode\r");
+ EnterMotorMode(&EF_can); // 电机位置锁定
+ send_enable = 0;
+ state=MOTOR_MODE;
+ break;
+
+ case('z'):
+ printf("\n\rMotor zeroing\r");
+ Zero(&EF_can);
+ send_enable = 0;
+ state=ZERO_MODE;
+ break;
+ }
+ }
+ if((state==MOTOR_MODE)&&(data[0]!='m'))
+ {
+ a_control.ef.p_des=int(c);//*2*PI/360;
+ send_enable = 1;
+ printf("f: %.3f\n\r", a_control.ef.p_des);
}
+
}
- }
-}
+ }
+
+
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);
+ a_control.ef.v_des=0; //v设置为0
+ a_control.ef.kp=50;//0.0012;//kp通过实验得到
+ a_control.ef.kd= 0;
+ a_control.ef.t_ff= 0;//forwade_torque=0;
+
+ a_control.wf.v_des=0;
+ a_control.wf.kp=0.0012;
+ a_control.wf.kd= 0;
+ a_control.wf.t_ff= 0;
- switch(Suse[0])
- {
- case('e'):
- command.printf("\n\rPF exiting motor mode\r");
- ExitMotorMode(&PF_can);
- 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);
- 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);
- return_zero = 0;
- c_lock = 0; // 电机位置解锁
- send_enable = 0; // M模式下，命令是否发送，命令是什么由calculate决定
- break;
- case('z'):
- command.printf("\n\rPF zeroing\r");
- Zero(&PF_can);
- return_zero = 0;
- c_lock = 2;
- send_enable = 0;
- break;
- case('r'):
- command.printf("\n\rPF return zero\r");
- EnterMotorMode(&PF_can);
- return_zero = 1;
- c_lock = 2;
- send_enable = 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
- c_control = 0;
- for(unsigned int i = 0; i < 10; i++)
- {
- Suse[i] = 0;
- }
}
@@ -215,4 +133,3 @@
-