Shao Rui / CH_Communicatuin_Test

123

Dependencies:   mbed-dev_spine

DATA_COMMAND/data_command.cpp@8:95a914f962bd, 2020-01-07 (annotated)

Committer:
shaorui
Date:
Tue Jan 07 09:23:24 2020 +0000
Revision:
8:95a914f962bd
Parent:
6:aad89fd109c2
Child:
9:bf02fd2d7a0a
1234

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;
shaorui 8:95a914f962bd 12 int state=REST_MODE;
shaorui 8:95a914f962bd 13 int duoji_command=0; // 命令帧标志位
shaorui 8:95a914f962bd 14 float SP_wf = 0, SP_ef = 0;
shaorui 8:95a914f962bd 15 int e_data[2]={0};
shaorui 8:95a914f962bd 16 int w_data[2]={0};
shaorui 8:95a914f962bd 17 char data[2]={0};
shaorui 8:95a914f962bd 18 int data_num=0;
WXD 5:6a95726e45b0 19 void serial_command_isr()
shaorui 8:95a914f962bd 20 {
WXD 5:6a95726e45b0 21 while(command.readable())
WXD 5:6a95726e45b0 22 {
shaorui 8:95a914f962bd 23 char c =command.getc();
shaorui 8:95a914f962bd 24 data[data_num]=c;
shaorui 8:95a914f962bd 25 data_num++;
shaorui 8:95a914f962bd 26 if(data_num>=2)
shaorui 8:95a914f962bd 27 data_num=0;
shaorui 8:95a914f962bd 28 if(data[0]=='e')
WXD 5:6a95726e45b0 29 {
shaorui 8:95a914f962bd 30 state=REST_MODE;
shaorui 8:95a914f962bd 31 send_enable=0;
shaorui 8:95a914f962bd 32 printf("\n\r Motor Reset Mode \r");
WXD 5:6a95726e45b0 33 }
shaorui 8:95a914f962bd 34
shaorui 8:95a914f962bd 35 if(data[1]=='k')
WXD 5:6a95726e45b0 36 {
shaorui 8:95a914f962bd 37 state= DUOJI_MODE ;
shaorui 8:95a914f962bd 38
WXD 5:6a95726e45b0 39 }
shaorui 8:95a914f962bd 40 if(state==DUOJI_MODE)
shaorui 8:95a914f962bd 41 {
shaorui 8:95a914f962bd 42 duoji_command=1;
shaorui 8:95a914f962bd 43 printf("\n\r Duoji Start\r");
WXD 5:6a95726e45b0 44
WXD 5:6a95726e45b0 45 }
shaorui 8:95a914f962bd 46 else
shaorui 8:95a914f962bd 47 {
shaorui 8:95a914f962bd 48 duoji_command=0;
shaorui 8:95a914f962bd 49 }
shaorui 8:95a914f962bd 50
shaorui 8:95a914f962bd 51 if(state==REST_MODE)
WXD 5:6a95726e45b0 52 {
shaorui 8:95a914f962bd 53 switch(data[0])
shaorui 8:95a914f962bd 54 {
shaorui 8:95a914f962bd 55 case('m'):
shaorui 8:95a914f962bd 56 printf("\n\rMotor entering PC motor mode\r");
shaorui 8:95a914f962bd 57 EnterMotorMode(&EF_can); // 电机位置锁定
shaorui 8:95a914f962bd 58 send_enable = 0;
shaorui 8:95a914f962bd 59 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;
shaorui 8:95a914f962bd 66 state=ZERO_MODE;
shaorui 8:95a914f962bd 67 break;
shaorui 8:95a914f962bd 68 }
shaorui 8:95a914f962bd 69 }
shaorui 8:95a914f962bd 70 if((state==MOTOR_MODE)&&(data[0]!='m'))
shaorui 8:95a914f962bd 71 {
shaorui 8:95a914f962bd 72 a_control.ef.p_des=int(c);//*2*PI/360;
shaorui 8:95a914f962bd 73 send_enable = 1;
shaorui 8:95a914f962bd 74 printf("f: %.3f\n\r", a_control.ef.p_des);
WXD 5:6a95726e45b0 75 }
shaorui 8:95a914f962bd 76
WXD 5:6a95726e45b0 77 }
WXD 5:6a95726e45b0 78
shaorui 8:95a914f962bd 79 }
shaorui 8:95a914f962bd 80
shaorui 8:95a914f962bd 81
WXD 5:6a95726e45b0 82
WXD 5:6a95726e45b0 83 void command_control()
WXD 5:6a95726e45b0 84 {
shaorui 8:95a914f962bd 85 a_control.ef.v_des=0; //v设置为0
shaorui 8:95a914f962bd 86 a_control.ef.kp=50;//0.0012;//kp通过实验得到
shaorui 8:95a914f962bd 87 a_control.ef.kd= 0;
shaorui 8:95a914f962bd 88 a_control.ef.t_ff= 0;//forwade_torque=0;
shaorui 8:95a914f962bd 89
shaorui 8:95a914f962bd 90 a_control.wf.v_des=0;
shaorui 8:95a914f962bd 91 a_control.wf.kp=0.0012;
shaorui 8:95a914f962bd 92 a_control.wf.kd= 0;
shaorui 8:95a914f962bd 93 a_control.wf.t_ff= 0;
WXD 5:6a95726e45b0 94
WXD 5:6a95726e45b0 95 }
WXD 5:6a95726e45b0 96
WXD 5:6a95726e45b0 97
WXD 5:6a95726e45b0 98
WXD 5:6a95726e45b0 99
WXD 5:6a95726e45b0 100
WXD 5:6a95726e45b0 101
WXD 5:6a95726e45b0 102
WXD 5:6a95726e45b0 103
WXD 5:6a95726e45b0 104
WXD 5:6a95726e45b0 105
WXD 5:6a95726e45b0 106
WXD 5:6a95726e45b0 107
WXD 5:6a95726e45b0 108
WXD 5:6a95726e45b0 109
WXD 5:6a95726e45b0 110
WXD 5:6a95726e45b0 111
WXD 5:6a95726e45b0 112
WXD 5:6a95726e45b0 113
WXD 5:6a95726e45b0 114
WXD 5:6a95726e45b0 115
WXD 5:6a95726e45b0 116
WXD 5:6a95726e45b0 117
WXD 5:6a95726e45b0 118
WXD 5:6a95726e45b0 119
WXD 5:6a95726e45b0 120
WXD 5:6a95726e45b0 121
WXD 5:6a95726e45b0 122
WXD 5:6a95726e45b0 123
WXD 5:6a95726e45b0 124
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