chen xin
1
Revision 10:f94e325fc1e6, committed 2020-03-12
- Comitter:
- ccxx1200
- Date:
- Thu Mar 12 15:17:18 2020 +0000
- Parent:
- 9:bf02fd2d7a0a
- Commit message:
- 1
Changed in this revision
--- a/DATA_COMMAND/data_command.cpp Wed Feb 19 05:22:32 2020 +0000
+++ b/DATA_COMMAND/data_command.cpp Thu Mar 12 15:17:18 2020 +0000
@@ -9,79 +9,102 @@
int counter = 0; // 输出计数器
int c_control = 0;
int send_enable=0;
-int state=REST_MODE;
+int e_state=e_REST_MODE;
+int w_state=w_REST_MODE;
+int duoji_state= DUOJI_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};
+char data[3]={0};
int data_num=0;
void serial_command_isr()
{
- while(command.readable())
+ while(command.readable()) //从Matlab接收到命令（字符）
{
- char c =command.getc();
- data[data_num]=c;
+ char c =command.getc();//将从matlab接收的命令赋值到c
+ data[data_num]=c;//把matlab的命令放入数组中，两个电机，数组有两个变量
data_num++;
- if(data_num>=2)
+ if(data_num>=3)
data_num=0;
- if(data[0]=='e')
+ if(data[0]=='e')//matlab第一个字符（命令）输入‘e'电机复位停止工作//肘关节电机
{
- state=REST_MODE;
+ e_state=e_REST_MODE;
send_enable=0;
- printf("\n\r Motor Reset Mode \r");
+ printf("\n\r ejoint Motor Reset Mode \r");
+ }
+ if(data[1]=='e')//matlab第二个字符（命令）输入‘e'电机复位停止工作//腕关节电机
+ {
+ w_state=w_REST_MODE;
+ send_enable=0;
+ printf("\n\r wjoint Motor Reset Mode \r");
}
- if(data[1]=='k')
+ if(data[2]=='k') //matlab第三个字符（命令）手爪舵机,输入'k'舵机进入工作准备状态
{
- state= DUOJI_MODE ;
+ duoji_state= DUOJI_MODE ;
}
- if(state==DUOJI_MODE)
- {
- duoji_command=1;
- printf("\n\r Duoji Start\r");
-
- }
- else
- {
- duoji_command=0;
- }
+
+
- if(state==REST_MODE)
+ if((e_state==e_REST_MODE)|| (e_state==e_REST_MODE)) //matlab第二次数据（指上一次为复位状态）
{
- switch(data[0])
+ switch(data[0]) //肘关节数据命令
{
case('m'):
- printf("\n\rMotor entering PC motor mode\r");
+ printf("\n\rMotor entering E motor mode\r");
EnterMotorMode(&EF_can); // 电机位置锁定
send_enable = 0;
- state=MOTOR_MODE;
+ e_state=MOTOR_MODE;
break;
case('z'):
printf("\n\rMotor zeroing\r");
Zero(&EF_can);
send_enable = 0;
- state=ZERO_MODE;
+ e_state=ZERO_MODE;
+ break;
+ }
+ switch(data[1]) //腕关节数据命令
+ {
+ case('m'):
+ printf("\n\rMotor entering E motor mode\r");
+ EnterMotorMode(&WF_can); // 电机位置锁定
+ send_enable = 0;
+ w_state=MOTOR_MODE;
+ break;
+
+ case('z'):
+ printf("\n\rMotor zeroing\r");
+ Zero(&WF_can);
+ send_enable = 0;
+ w_state=ZERO_MODE;
break;
}
}
- if((state==MOTOR_MODE)&&(data[0]!='m'))
+ if((e_state==MOTOR_MODE)&&(data[0]!='m'))//肘关节数据判断成功，电机跟随位置转动
{
//a_control.ef.p_des=int(c);//*2*PI/360;
- a_control.ef.p_des=int(data[0])*2*PI/360;
+ a_control.ef.p_des=int(data[0])*2*PI/360;//matlab给定的肘关节角度信息
send_enable = 1;
printf("f: %.3f\n\r", a_control.ef.p_des);
}
- if((state==MOTOR_MODE)&&(data[2]!='m'))
+ if((w_state==MOTOR_MODE)&&(data[1]!='m'))//腕关节数据判断成功，电机跟随位置转动
{
- a_control.wf.p_des=int(data[2])*2*PI/360;
+ a_control.wf.p_des=int(data[1])*2*PI/360;//matlab给定的腕关节角度信息
send_enable = 1;
- printf("f: %.3f\n\r", a_control.ef.p_des);
+ printf("f: %.3f\n\r", a_control.wf.p_des);
}
-
+ if((duoji_state==DUOJI_MODE)&&(data[2]!='k'))//腕关节数据判断成功，电机跟随位置转动
+ {
+ duoji_control.p_des=int(data[2]);//matlab给定的舵机（手爪）的位置信息
+ duoji_command=1;
+ printf("\n\r Duoji Start\r"); //舵机开始运动到指定的位置
+ printf("f: %.3f\n\r", duoji_control.p_des);//显示舵机的角度信息
+ }
+ }
}
}
@@ -96,7 +119,7 @@
a_control.ef.t_ff= 0;//forwade_torque=0;
a_control.wf.v_des=0;
- a_control.wf.kp=0.0012;
+ a_control.wf.kp=0.0012; //kp通过实验得到
a_control.wf.kd= 0;
a_control.wf.t_ff= 0;
--- a/DATA_COMMAND/data_command.h Wed Feb 19 05:22:32 2020 +0000 +++ b/DATA_COMMAND/data_command.h Thu Mar 12 15:17:18 2020 +0000 @@ -6,13 +6,18 @@ #include "CAN.h" #include "used_leg_message.h" #include "mode.h" -#define REST_MODE 0 +#define e_REST_MODE 0 +#define w_REST_MODE 4 +#define DUOJI_REST__MODE 5 #define MOTOR_MODE 1 #define ZERO_MODE 2 #define DUOJI_MODE 3 + extern Serial command; extern DigitalOut sf_m_c; -extern int state; +extern int e_state; +extern int w_state; +extern int duoji_state; extern int send_enable; extern int enabled; extern int counter; @@ -21,7 +26,7 @@ extern float SP_ef, SP_wf; void serial_command_isr(); void command_control(); -extern char data[2]; +extern char data[3]; extern int data_num;
--- a/LEG_MESSAGE/leg_message.h Wed Feb 19 05:22:32 2020 +0000
+++ b/LEG_MESSAGE/leg_message.h Thu Mar 12 15:17:18 2020 +0000
@@ -22,6 +22,12 @@
struct ankle_state{ // 系统状态结构体
joint_state ef, wf; // 状态成员: pf, df状态
};
+
+ struct duoji_control{ // 关节控制结构体
+ float p_des, t_des; // 控制量为: p_des, v_des, kp, kd, t_ff
+ };
+
+
/*
struct cal_data_t
{
--- a/LEG_MESSAGE/used_leg_message.cpp Wed Feb 19 05:22:32 2020 +0000 +++ b/LEG_MESSAGE/used_leg_message.cpp Thu Mar 12 15:17:18 2020 +0000 @@ -3,7 +3,7 @@ ankle_state a_state; ankle_control a_control; - +duoji_control shouzhua_control; //cal_data_t cal_data; // spine-->up //cal_command_t cal_command; // up-->spine
--- a/LEG_MESSAGE/used_leg_message.h Wed Feb 19 05:22:32 2020 +0000 +++ b/LEG_MESSAGE/used_leg_message.h Thu Mar 12 15:17:18 2020 +0000 @@ -8,7 +8,7 @@ extern ankle_state a_state; extern ankle_control a_control; - +extern duoji_control shouzhua_control; //extern cal_data_t cal_data; // spine-->up //extern cal_command_t cal_command; // up-->spine
--- a/main.cpp Wed Feb 19 05:22:32 2020 +0000
+++ b/main.cpp Thu Mar 12 15:17:18 2020 +0000
@@ -75,13 +75,10 @@
//send_enable = 0;
}
if(duoji_command==1)
- {moveServo(1, 2000, 1000); //1s移动1号舵机至2000位置
+ {moveServo(1, duoji_control.p_des, 1000); //1s移动1号舵机至指定的位置
printf("Move Sucessfully1 ! \n\r");
- wait(2);
- moveServo(1, 500, 1000); //1s移动1号舵机至500位置
- //BaterryOut();
+ wait(1);
printf("Move Sucessfully2 ! \n\r");
- wait(2);
}
--- a/mbed-dev.lib Wed Feb 19 05:22:32 2020 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/benkatz/code/mbed-dev_spine/#97f825502e2a