Shao Rui / CH_Communicatuin_Test2

1

Dependencies:   mbed-dev_spine

DATA_COMMAND/data_command.cpp@10:f94e325fc1e6, 2020-03-12 (annotated)

Committer:
ccxx1200
Date:
Thu Mar 12 15:17:18 2020 +0000
Revision:
10:f94e325fc1e6
Parent:
9:bf02fd2d7a0a
Child:
11:360230a179b9
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WXD 5:6a95726e45b0 1 #include "data_command.h"
shaorui 8:95a914f962bd 2 #include "leg_message.h"
WXD 5:6a95726e45b0 3
WXD 6:aad89fd109c2 4 Serial command(PC_12, PD_2);
WXD 6:aad89fd109c2 5 //DigitalOut sf_m_c(PC_12);
shaorui 8:95a914f962bd 6 int e_num=0;
shaorui 8:95a914f962bd 7 int w_num=0;
WXD 5:6a95726e45b0 8 int enabled = 0; // 进入电机模式标志位
WXD 5:6a95726e45b0 9 int counter = 0; // 输出计数器
shaorui 8:95a914f962bd 10 int c_control = 0;
shaorui 8:95a914f962bd 11 int send_enable=0;
ccxx1200 10:f94e325fc1e6 12 int e_state=e_REST_MODE;
ccxx1200 10:f94e325fc1e6 13 int w_state=w_REST_MODE;
ccxx1200 10:f94e325fc1e6 14 int duoji_state= DUOJI_REST__MODE ;
shaorui 8:95a914f962bd 15 int duoji_command=0; // 命令帧标志位
shaorui 8:95a914f962bd 16 float SP_wf = 0, SP_ef = 0;
shaorui 8:95a914f962bd 17 int e_data[2]={0};
shaorui 8:95a914f962bd 18 int w_data[2]={0};
ccxx1200 10:f94e325fc1e6 19 char data[3]={0};
shaorui 8:95a914f962bd 20 int data_num=0;
WXD 5:6a95726e45b0 21 void serial_command_isr()
shaorui 8:95a914f962bd 22 {
ccxx1200 10:f94e325fc1e6 23 while(command.readable()) //从Matlab接收到命令（字符）
WXD 5:6a95726e45b0 24 {
ccxx1200 10:f94e325fc1e6 25 char c =command.getc();//将从matlab接收的命令赋值到c
ccxx1200 10:f94e325fc1e6 26 data[data_num]=c;//把matlab的命令放入数组中，两个电机，数组有两个变量
shaorui 8:95a914f962bd 27 data_num++;
ccxx1200 10:f94e325fc1e6 28 if(data_num>=3)
shaorui 8:95a914f962bd 29 data_num=0;
ccxx1200 10:f94e325fc1e6 30 if(data[0]=='e')//matlab第一个字符（命令）输入‘e'电机复位停止工作//肘关节电机
WXD 5:6a95726e45b0 31 {
ccxx1200 10:f94e325fc1e6 32 e_state=e_REST_MODE;
shaorui 8:95a914f962bd 33 send_enable=0;
ccxx1200 10:f94e325fc1e6 34 printf("\n\r ejoint Motor Reset Mode \r");
ccxx1200 10:f94e325fc1e6 35 }
ccxx1200 10:f94e325fc1e6 36 if(data[1]=='e')//matlab第二个字符（命令）输入‘e'电机复位停止工作//腕关节电机
ccxx1200 10:f94e325fc1e6 37 {
ccxx1200 10:f94e325fc1e6 38 w_state=w_REST_MODE;
ccxx1200 10:f94e325fc1e6 39 send_enable=0;
ccxx1200 10:f94e325fc1e6 40 printf("\n\r wjoint Motor Reset Mode \r");
WXD 5:6a95726e45b0 41 }
shaorui 8:95a914f962bd 42
ccxx1200 10:f94e325fc1e6 43 if(data[2]=='k') //matlab第三个字符（命令）手爪舵机,输入'k'舵机进入工作准备状态
WXD 5:6a95726e45b0 44 {
ccxx1200 10:f94e325fc1e6 45 duoji_state= DUOJI_MODE ;
shaorui 8:95a914f962bd 46
WXD 5:6a95726e45b0 47 }
ccxx1200 10:f94e325fc1e6 48
ccxx1200 10:f94e325fc1e6 49
shaorui 8:95a914f962bd 50
ccxx1200 10:f94e325fc1e6 51 if((e_state==e_REST_MODE)|| (e_state==e_REST_MODE)) //matlab第二次数据（指上一次为复位状态）
WXD 5:6a95726e45b0 52 {
ccxx1200 10:f94e325fc1e6 53 switch(data[0]) //肘关节数据命令
shaorui 8:95a914f962bd 54 {
shaorui 8:95a914f962bd 55 case('m'):
ccxx1200 10:f94e325fc1e6 56 printf("\n\rMotor entering E motor mode\r");
shaorui 8:95a914f962bd 57 EnterMotorMode(&EF_can); // 电机位置锁定
shaorui 8:95a914f962bd 58 send_enable = 0;
ccxx1200 10:f94e325fc1e6 59 e_state=MOTOR_MODE;
shaorui 8:95a914f962bd 60 break;
shaorui 8:95a914f962bd 61
shaorui 8:95a914f962bd 62 case('z'):
shaorui 8:95a914f962bd 63 printf("\n\rMotor zeroing\r");
shaorui 8:95a914f962bd 64 Zero(&EF_can);
shaorui 8:95a914f962bd 65 send_enable = 0;
ccxx1200 10:f94e325fc1e6 66 e_state=ZERO_MODE;
ccxx1200 10:f94e325fc1e6 67 break;
ccxx1200 10:f94e325fc1e6 68 }
ccxx1200 10:f94e325fc1e6 69 switch(data[1]) //腕关节数据命令
ccxx1200 10:f94e325fc1e6 70 {
ccxx1200 10:f94e325fc1e6 71 case('m'):
ccxx1200 10:f94e325fc1e6 72 printf("\n\rMotor entering E motor mode\r");
ccxx1200 10:f94e325fc1e6 73 EnterMotorMode(&WF_can); // 电机位置锁定
ccxx1200 10:f94e325fc1e6 74 send_enable = 0;
ccxx1200 10:f94e325fc1e6 75 w_state=MOTOR_MODE;
ccxx1200 10:f94e325fc1e6 76 break;
ccxx1200 10:f94e325fc1e6 77
ccxx1200 10:f94e325fc1e6 78 case('z'):
ccxx1200 10:f94e325fc1e6 79 printf("\n\rMotor zeroing\r");
ccxx1200 10:f94e325fc1e6 80 Zero(&WF_can);
ccxx1200 10:f94e325fc1e6 81 send_enable = 0;
ccxx1200 10:f94e325fc1e6 82 w_state=ZERO_MODE;
shaorui 8:95a914f962bd 83 break;
shaorui 8:95a914f962bd 84 }
shaorui 8:95a914f962bd 85 }
ccxx1200 10:f94e325fc1e6 86 if((e_state==MOTOR_MODE)&&(data[0]!='m'))//肘关节数据判断成功，电机跟随位置转动
shaorui 8:95a914f962bd 87 {
shaorui 9:bf02fd2d7a0a 88 //a_control.ef.p_des=int(c);//*2*PI/360;
ccxx1200 10:f94e325fc1e6 89 a_control.ef.p_des=int(data[0])*2*PI/360;//matlab给定的肘关节角度信息
shaorui 9:bf02fd2d7a0a 90 send_enable = 1;
shaorui 9:bf02fd2d7a0a 91 printf("f: %.3f\n\r", a_control.ef.p_des);
shaorui 9:bf02fd2d7a0a 92 }
shaorui 9:bf02fd2d7a0a 93
ccxx1200 10:f94e325fc1e6 94 if((w_state==MOTOR_MODE)&&(data[1]!='m'))//腕关节数据判断成功，电机跟随位置转动
shaorui 9:bf02fd2d7a0a 95 {
ccxx1200 10:f94e325fc1e6 96 a_control.wf.p_des=int(data[1])*2*PI/360;//matlab给定的腕关节角度信息
shaorui 8:95a914f962bd 97 send_enable = 1;
ccxx1200 10:f94e325fc1e6 98 printf("f: %.3f\n\r", a_control.wf.p_des);
WXD 5:6a95726e45b0 99 }
ccxx1200 10:f94e325fc1e6 100 if((duoji_state==DUOJI_MODE)&&(data[2]!='k'))//腕关节数据判断成功，电机跟随位置转动
ccxx1200 10:f94e325fc1e6 101 {
ccxx1200 10:f94e325fc1e6 102 duoji_control.p_des=int(data[2]);//matlab给定的舵机（手爪）的位置信息
ccxx1200 10:f94e325fc1e6 103 duoji_command=1;
ccxx1200 10:f94e325fc1e6 104 printf("\n\r Duoji Start\r"); //舵机开始运动到指定的位置
ccxx1200 10:f94e325fc1e6 105 printf("f: %.3f\n\r", duoji_control.p_des);//显示舵机的角度信息
ccxx1200 10:f94e325fc1e6 106 }
ccxx1200 10:f94e325fc1e6 107 }
WXD 5:6a95726e45b0 108 }
WXD 5:6a95726e45b0 109
shaorui 8:95a914f962bd 110 }
shaorui 8:95a914f962bd 111
shaorui 8:95a914f962bd 112
WXD 5:6a95726e45b0 113
WXD 5:6a95726e45b0 114 void command_control()
WXD 5:6a95726e45b0 115 {
shaorui 8:95a914f962bd 116 a_control.ef.v_des=0; //v设置为0
shaorui 8:95a914f962bd 117 a_control.ef.kp=50;//0.0012;//kp通过实验得到
shaorui 8:95a914f962bd 118 a_control.ef.kd= 0;
shaorui 8:95a914f962bd 119 a_control.ef.t_ff= 0;//forwade_torque=0;
shaorui 8:95a914f962bd 120
shaorui 8:95a914f962bd 121 a_control.wf.v_des=0;
ccxx1200 10:f94e325fc1e6 122 a_control.wf.kp=0.0012; //kp通过实验得到
shaorui 8:95a914f962bd 123 a_control.wf.kd= 0;
shaorui 8:95a914f962bd 124 a_control.wf.t_ff= 0;
WXD 5:6a95726e45b0 125
WXD 5:6a95726e45b0 126 }
WXD 5:6a95726e45b0 127
WXD 5:6a95726e45b0 128
WXD 5:6a95726e45b0 129
WXD 5:6a95726e45b0 130
WXD 5:6a95726e45b0 131
WXD 5:6a95726e45b0 132
WXD 5:6a95726e45b0 133
WXD 5:6a95726e45b0 134
WXD 5:6a95726e45b0 135
WXD 5:6a95726e45b0 136
WXD 5:6a95726e45b0 137
WXD 5:6a95726e45b0 138
WXD 5:6a95726e45b0 139
WXD 5:6a95726e45b0 140
WXD 5:6a95726e45b0 141
WXD 5:6a95726e45b0 142
WXD 5:6a95726e45b0 143
WXD 5:6a95726e45b0 144
WXD 5:6a95726e45b0 145
WXD 5:6a95726e45b0 146
WXD 5:6a95726e45b0 147
WXD 5:6a95726e45b0 148
WXD 5:6a95726e45b0 149
WXD 5:6a95726e45b0 150
WXD 5:6a95726e45b0 151
WXD 5:6a95726e45b0 152
WXD 5:6a95726e45b0 153
WXD 5:6a95726e45b0 154
WXD 5:6a95726e45b0 155
WXD 5:6a95726e45b0 156
WXD 5:6a95726e45b0 157
WXD 5:6a95726e45b0 158
WXD 5:6a95726e45b0 159
WXD 5:6a95726e45b0 160
WXD 5:6a95726e45b0 161
WXD 5:6a95726e45b0 162
WXD 5:6a95726e45b0 163
WXD 5:6a95726e45b0 164
WXD 5:6a95726e45b0 165
WXD 5:6a95726e45b0 166