Shao Rui / CH_Communicatuin_Test

123

Dependencies:   mbed-dev_spine

DATA_COMMAND/data_command.cpp@9:bf02fd2d7a0a, 2020-02-19 (annotated)

Committer:
shaorui
Date:
Wed Feb 19 05:22:32 2020 +0000
Revision:
9:bf02fd2d7a0a
Parent:
8:95a914f962bd 
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;
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 9:bf02fd2d7a0a 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 9:bf02fd2d7a0a 72 //a_control.ef.p_des=int(c);//*2*PI/360;
shaorui 9:bf02fd2d7a0a 73 a_control.ef.p_des=int(data[0])*2*PI/360;
shaorui 9:bf02fd2d7a0a 74 send_enable = 1;
shaorui 9:bf02fd2d7a0a 75 printf("f: %.3f\n\r", a_control.ef.p_des);
shaorui 9:bf02fd2d7a0a 76 }
shaorui 9:bf02fd2d7a0a 77
shaorui 9:bf02fd2d7a0a 78 if((state==MOTOR_MODE)&&(data[2]!='m'))
shaorui 9:bf02fd2d7a0a 79 {
shaorui 9:bf02fd2d7a0a 80 a_control.wf.p_des=int(data[2])*2*PI/360;
shaorui 8:95a914f962bd 81 send_enable = 1;
shaorui 8:95a914f962bd 82 printf("f: %.3f\n\r", a_control.ef.p_des);
WXD 5:6a95726e45b0 83 }
shaorui 8:95a914f962bd 84
WXD 5:6a95726e45b0 85 }
WXD 5:6a95726e45b0 86
shaorui 8:95a914f962bd 87 }
shaorui 8:95a914f962bd 88
shaorui 8:95a914f962bd 89
WXD 5:6a95726e45b0 90
WXD 5:6a95726e45b0 91 void command_control()
WXD 5:6a95726e45b0 92 {
shaorui 8:95a914f962bd 93 a_control.ef.v_des=0; //v设置为0
shaorui 8:95a914f962bd 94 a_control.ef.kp=50;//0.0012;//kp通过实验得到
shaorui 8:95a914f962bd 95 a_control.ef.kd= 0;
shaorui 8:95a914f962bd 96 a_control.ef.t_ff= 0;//forwade_torque=0;
shaorui 8:95a914f962bd 97
shaorui 8:95a914f962bd 98 a_control.wf.v_des=0;
shaorui 8:95a914f962bd 99 a_control.wf.kp=0.0012;
shaorui 8:95a914f962bd 100 a_control.wf.kd= 0;
shaorui 8:95a914f962bd 101 a_control.wf.t_ff= 0;
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
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