Gan likun / yezhong_main_controller_copy001

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

DATA_COMMAND/data_command.cpp@10:f83b0935a8f0, 2022-07-09 (annotated)

Committer:
ganlikun
Date:
Sat Jul 09 11:58:13 2022 +0000
Revision:
10:f83b0935a8f0
Parent:
2:cd74a8cb03b0
Child:
7:d1b09098579b
00000

Who changed what in which revision?

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