chen xin / CH_Communicatuin_Test2

1

DATA_COMMAND/data_command.cpp@6:aad89fd109c2, 2019-12-02 (annotated)

Committer:
WXD
Date:
Mon Dec 02 14:22:07 2019 +0000
Revision:
6:aad89fd109c2
Parent:
5:6a95726e45b0
Child:
8:95a914f962bd
123

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WXD 5:6a95726e45b0 1 #include "data_command.h"
WXD 5:6a95726e45b0 2
WXD 5:6a95726e45b0 3
WXD 6:aad89fd109c2 4 Serial command(PC_12, PD_2);
WXD 6:aad89fd109c2 5 //DigitalOut sf_m_c(PC_12);
WXD 5:6a95726e45b0 6
WXD 5:6a95726e45b0 7 int enabled = 0; // 进入电机模式标志位
WXD 5:6a95726e45b0 8 int counter = 0; // 输出计数器
WXD 5:6a95726e45b0 9 int c_control = 0; // 命令帧标志位
WXD 5:6a95726e45b0 10 float SP_pf = 0, SP_df = 0;
WXD 5:6a95726e45b0 11 int c_lock = 2; // 位置锁定标志位
WXD 6:aad89fd109c2 12 int return_zero = 0; // 电机回0标志位
WXD 5:6a95726e45b0 13
WXD 5:6a95726e45b0 14 unsigned int flag_A = 0, flag_B = 0;
WXD 5:6a95726e45b0 15 unsigned int Snum = 0;
WXD 5:6a95726e45b0 16 uint16_t Sget[10] = {0};
WXD 5:6a95726e45b0 17 uint16_t Suse[10] = {0};
WXD 5:6a95726e45b0 18
WXD 5:6a95726e45b0 19
WXD 5:6a95726e45b0 20 void serial_command_isr()
WXD 5:6a95726e45b0 21 {
WXD 5:6a95726e45b0 22 while(command.readable())
WXD 5:6a95726e45b0 23 {
WXD 5:6a95726e45b0 24 uint8_t c = command.getc();
WXD 5:6a95726e45b0 25 if(c == 'A')
WXD 5:6a95726e45b0 26 {
WXD 5:6a95726e45b0 27 flag_A = 1;
WXD 5:6a95726e45b0 28
WXD 5:6a95726e45b0 29 flag_B = 0;
WXD 5:6a95726e45b0 30 Snum = 0;
WXD 5:6a95726e45b0 31 for(unsigned int i = 0; i < 10; i++)
WXD 5:6a95726e45b0 32 {
WXD 5:6a95726e45b0 33 Sget[i] = 0;
WXD 5:6a95726e45b0 34 }
WXD 5:6a95726e45b0 35
WXD 5:6a95726e45b0 36 break;
WXD 5:6a95726e45b0 37 }
WXD 5:6a95726e45b0 38 if(c == 'B')
WXD 5:6a95726e45b0 39 {
WXD 5:6a95726e45b0 40 flag_B = 1;
WXD 5:6a95726e45b0 41 }
WXD 5:6a95726e45b0 42
WXD 5:6a95726e45b0 43 if(flag_A == 1)
WXD 5:6a95726e45b0 44 {
WXD 5:6a95726e45b0 45 if((flag_B != 1) && (Snum < 10))
WXD 5:6a95726e45b0 46 {
WXD 5:6a95726e45b0 47 Sget[Snum] = c;
WXD 5:6a95726e45b0 48 }
WXD 5:6a95726e45b0 49
WXD 5:6a95726e45b0 50 Snum++;
WXD 5:6a95726e45b0 51
WXD 5:6a95726e45b0 52 if((flag_B == 1) && (Snum != 11))
WXD 5:6a95726e45b0 53 {
WXD 5:6a95726e45b0 54 flag_A = 0;
WXD 5:6a95726e45b0 55 flag_B = 0;
WXD 5:6a95726e45b0 56 Snum = 0;
WXD 5:6a95726e45b0 57 }
WXD 5:6a95726e45b0 58
WXD 5:6a95726e45b0 59 if((flag_B == 1) && (Snum == 11))
WXD 5:6a95726e45b0 60 {
WXD 5:6a95726e45b0 61 flag_A = 0;
WXD 5:6a95726e45b0 62 flag_B = 0;
WXD 5:6a95726e45b0 63 Snum = 0;
WXD 5:6a95726e45b0 64
WXD 5:6a95726e45b0 65 for(unsigned int i = 0; i < 10; i++)
WXD 5:6a95726e45b0 66 {
WXD 5:6a95726e45b0 67 Suse[i] = Sget[i];
WXD 5:6a95726e45b0 68 }
WXD 5:6a95726e45b0 69
WXD 5:6a95726e45b0 70 c_control = 1; // 命令帧接收成功
WXD 5:6a95726e45b0 71
WXD 5:6a95726e45b0 72 //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]);
WXD 5:6a95726e45b0 73 }
WXD 5:6a95726e45b0 74 }
WXD 5:6a95726e45b0 75 }
WXD 5:6a95726e45b0 76 }
WXD 5:6a95726e45b0 77
WXD 5:6a95726e45b0 78
WXD 5:6a95726e45b0 79 void command_control()
WXD 5:6a95726e45b0 80 {
WXD 5:6a95726e45b0 81 SP_pf = 10*(Suse[2] - '0') + 1*(Suse[3] - '0') + 0.1*(Suse[4] - '0');
WXD 5:6a95726e45b0 82 SP_df = 10*(Suse[7] - '0') + 1*(Suse[8] - '0') + 0.1*(Suse[9] - '0');
WXD 5:6a95726e45b0 83 if(Suse[1] == '+')
WXD 5:6a95726e45b0 84 SP_pf = 0 + SP_pf;
WXD 5:6a95726e45b0 85 if(Suse[1] == '-')
WXD 5:6a95726e45b0 86 SP_pf = 0 - SP_pf;
WXD 5:6a95726e45b0 87 if(Suse[6] == '+')
WXD 5:6a95726e45b0 88 SP_df = 0 + SP_df;
WXD 5:6a95726e45b0 89 if(Suse[6] == '-')
WXD 5:6a95726e45b0 90 SP_df = 0 - SP_df;
WXD 5:6a95726e45b0 91
WXD 5:6a95726e45b0 92
WXD 6:aad89fd109c2 93 //sf_m_c = 1; // 切换为发送模式
WXD 5:6a95726e45b0 94 wait_us(200);
WXD 5:6a95726e45b0 95
WXD 5:6a95726e45b0 96 switch(Suse[0])
WXD 5:6a95726e45b0 97 {
WXD 5:6a95726e45b0 98 case('e'):
WXD 5:6a95726e45b0 99 command.printf("\n\rPF exiting motor mode\r");
WXD 5:6a95726e45b0 100 ExitMotorMode(&PF_can);
WXD 6:aad89fd109c2 101 return_zero = 0; // 停止回0
WXD 6:aad89fd109c2 102 c_lock = 2; // 电机位置锁无效
WXD 5:6a95726e45b0 103 send_enable = 0; // main不发送CAN位置命令
WXD 5:6a95726e45b0 104 break;
WXD 5:6a95726e45b0 105 case('m'):
WXD 5:6a95726e45b0 106 command.printf("\n\rPF entering PC motor mode\r");
WXD 5:6a95726e45b0 107 EnterMotorMode(&PF_can);
WXD 6:aad89fd109c2 108 return_zero = 0;
WXD 6:aad89fd109c2 109 c_lock = 1; // 电机位置锁定
WXD 5:6a95726e45b0 110 send_enable = 1; // main发送CAN位置命令
WXD 5:6a95726e45b0 111 break;
WXD 5:6a95726e45b0 112 case('M'):
WXD 5:6a95726e45b0 113 command.printf("\n\rPF entering BOARD motor mode\r");
WXD 5:6a95726e45b0 114 EnterMotorMode(&PF_can);
WXD 6:aad89fd109c2 115 return_zero = 0;
WXD 5:6a95726e45b0 116 c_lock = 0; // 电机位置解锁
WXD 6:aad89fd109c2 117 send_enable = 0; // M模式下，命令是否发送，命令是什么由calculate决定
WXD 5:6a95726e45b0 118 break;
WXD 5:6a95726e45b0 119 case('z'):
WXD 5:6a95726e45b0 120 command.printf("\n\rPF zeroing\r");
WXD 5:6a95726e45b0 121 Zero(&PF_can);
WXD 6:aad89fd109c2 122 return_zero = 0;
WXD 6:aad89fd109c2 123 c_lock = 2;
WXD 6:aad89fd109c2 124 send_enable = 0;
WXD 6:aad89fd109c2 125 break;
WXD 6:aad89fd109c2 126 case('r'):
WXD 6:aad89fd109c2 127 command.printf("\n\rPF return zero\r");
WXD 6:aad89fd109c2 128 EnterMotorMode(&PF_can);
WXD 6:aad89fd109c2 129 return_zero = 1;
WXD 6:aad89fd109c2 130 c_lock = 2;
WXD 6:aad89fd109c2 131 send_enable = 1;
WXD 6:aad89fd109c2 132 break;
WXD 5:6a95726e45b0 133 }
WXD 6:aad89fd109c2 134 /*
WXD 5:6a95726e45b0 135 switch(Suse[5])
WXD 5:6a95726e45b0 136 {
WXD 5:6a95726e45b0 137 case('e'):
WXD 5:6a95726e45b0 138 command.printf("DF exiting motor mode\n\r");
WXD 5:6a95726e45b0 139 ExitMotorMode(&DF_can);
WXD 5:6a95726e45b0 140 c_lock = 1; // 电机位置解锁
WXD 5:6a95726e45b0 141 send_enable = 0; // main不发送CAN位置命令
WXD 5:6a95726e45b0 142 break;
WXD 5:6a95726e45b0 143 case('m'):
WXD 5:6a95726e45b0 144 command.printf("DF entering PC motor mode\n\r");
WXD 5:6a95726e45b0 145 EnterMotorMode(&DF_can);
WXD 5:6a95726e45b0 146 c_lock = 1; // 电机位置锁定
WXD 5:6a95726e45b0 147 send_enable = 1; // main发送CAN位置命令
WXD 5:6a95726e45b0 148 break;
WXD 5:6a95726e45b0 149 case('M'):
WXD 5:6a95726e45b0 150 command.printf("DF entering BOARD motor mode\n\r");
WXD 5:6a95726e45b0 151 EnterMotorMode(&DF_can);
WXD 5:6a95726e45b0 152 c_lock = 0; // 电机位置解锁
WXD 5:6a95726e45b0 153 send_enable = 0; // M模式下，命令是否发送，命令是什么由calculate决定
WXD 5:6a95726e45b0 154 break;
WXD 5:6a95726e45b0 155 case('z'):
WXD 5:6a95726e45b0 156 command.printf("DF zeroing\n\r");
WXD 5:6a95726e45b0 157 Zero(&DF_can);
WXD 5:6a95726e45b0 158 break;
WXD 5:6a95726e45b0 159 }
WXD 6:aad89fd109c2 160 */
WXD 5:6a95726e45b0 161
WXD 5:6a95726e45b0 162 wait_ms(2);
WXD 6:aad89fd109c2 163 //sf_m_c = 0; // 恢复接收模式
WXD 5:6a95726e45b0 164 wait_us(200);
WXD 5:6a95726e45b0 165
WXD 5:6a95726e45b0 166
WXD 5:6a95726e45b0 167 // 电机模式命令在此发送，锁定控制模式的位置量在main中发送
WXD 5:6a95726e45b0 168 WriteAll();
WXD 5:6a95726e45b0 169 wait(1.0f);
WXD 5:6a95726e45b0 170
WXD 5:6a95726e45b0 171 // 解除串口控制，缓冲区清0
WXD 5:6a95726e45b0 172 c_control = 0;
WXD 5:6a95726e45b0 173 for(unsigned int i = 0; i < 10; i++)
WXD 5:6a95726e45b0 174 {
WXD 5:6a95726e45b0 175 Suse[i] = 0;
WXD 5:6a95726e45b0 176 }
WXD 5:6a95726e45b0 177 }
WXD 5:6a95726e45b0 178
WXD 5:6a95726e45b0 179
WXD 5:6a95726e45b0 180
WXD 5:6a95726e45b0 181
WXD 5:6a95726e45b0 182
WXD 5:6a95726e45b0 183
WXD 5:6a95726e45b0 184
WXD 5:6a95726e45b0 185
WXD 5:6a95726e45b0 186
WXD 5:6a95726e45b0 187
WXD 5:6a95726e45b0 188
WXD 5:6a95726e45b0 189
WXD 5:6a95726e45b0 190
WXD 5:6a95726e45b0 191
WXD 5:6a95726e45b0 192
WXD 5:6a95726e45b0 193
WXD 5:6a95726e45b0 194
WXD 5:6a95726e45b0 195
WXD 5:6a95726e45b0 196
WXD 5:6a95726e45b0 197
WXD 5:6a95726e45b0 198
WXD 5:6a95726e45b0 199
WXD 5:6a95726e45b0 200
WXD 5:6a95726e45b0 201
WXD 5:6a95726e45b0 202
WXD 5:6a95726e45b0 203
WXD 5:6a95726e45b0 204
WXD 5:6a95726e45b0 205
WXD 5:6a95726e45b0 206
WXD 5:6a95726e45b0 207
WXD 5:6a95726e45b0 208
WXD 5:6a95726e45b0 209
WXD 5:6a95726e45b0 210
WXD 5:6a95726e45b0 211
WXD 5:6a95726e45b0 212
WXD 5:6a95726e45b0 213
WXD 5:6a95726e45b0 214
WXD 5:6a95726e45b0 215
WXD 5:6a95726e45b0 216
WXD 5:6a95726e45b0 217
WXD 5:6a95726e45b0 218