Gan likun / yezhong_main_controller_copy1

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

DATA_COMMAND/data_command.cpp@0:d80c66cb1b3a, 2020-11-10 (annotated)

Committer:
panzhan
Date:
Tue Nov 10 09:09:58 2020 +0000
Revision:
0:d80c66cb1b3a
Child:
2:cd74a8cb03b0
panzhan_main_controller_copy2

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);
panzhan 0:d80c66cb1b3a 101 ExitMotorMode(&knee_txMsg);
panzhan 0:d80c66cb1b3a 102 ExitMotorMode(&ankle_txMsg);
panzhan 0:d80c66cb1b3a 103 return_zero = 0; // 停止回0
panzhan 0:d80c66cb1b3a 104 c_lock = 2; // 电机位置锁无效
panzhan 0:d80c66cb1b3a 105 // send_enable = 0; // main不发送CAN位置命令
panzhan 0:d80c66cb1b3a 106 break;
panzhan 0:d80c66cb1b3a 107 case('m'):
panzhan 0:d80c66cb1b3a 108 command.printf("\n\rPF entering PC motor mode\r");
panzhan 0:d80c66cb1b3a 109 // EnterMotorMode(&PF_can);
panzhan 0:d80c66cb1b3a 110 EnterMotorMode(&knee_txMsg);
panzhan 0:d80c66cb1b3a 111 EnterMotorMode(&ankle_txMsg);
panzhan 0:d80c66cb1b3a 112
panzhan 0:d80c66cb1b3a 113 return_zero = 0;
panzhan 0:d80c66cb1b3a 114 c_lock = 1; // 电机位置锁定
panzhan 0:d80c66cb1b3a 115 // send_enable = 1; // main发送CAN位置命令
panzhan 0:d80c66cb1b3a 116 break;
panzhan 0:d80c66cb1b3a 117 case('M'):
panzhan 0:d80c66cb1b3a 118 command.printf("\n\rPF entering BOARD motor mode\r");
panzhan 0:d80c66cb1b3a 119 // EnterMotorMode(&PF_can);
panzhan 0:d80c66cb1b3a 120 EnterMotorMode(&knee_txMsg);
panzhan 0:d80c66cb1b3a 121 EnterMotorMode(&ankle_txMsg);
panzhan 0:d80c66cb1b3a 122 return_zero = 0;
panzhan 0:d80c66cb1b3a 123 c_lock = 0; // 电机位置解锁
panzhan 0:d80c66cb1b3a 124 // send_enable = 0; // M模式下，命令是否发送，命令是什么由calculate决定
panzhan 0:d80c66cb1b3a 125 break;
panzhan 0:d80c66cb1b3a 126 case('z'):
panzhan 0:d80c66cb1b3a 127 command.printf("\n\rPF zeroing\r");
panzhan 0:d80c66cb1b3a 128 // Zero(&PF_can);
panzhan 0:d80c66cb1b3a 129 Zero(&knee_txMsg);
panzhan 0:d80c66cb1b3a 130 Zero(&ankle_txMsg);
panzhan 0:d80c66cb1b3a 131 return_zero = 0;
panzhan 0:d80c66cb1b3a 132 c_lock = 2;
panzhan 0:d80c66cb1b3a 133 // send_enable = 0;
panzhan 0:d80c66cb1b3a 134 break;
panzhan 0:d80c66cb1b3a 135 case('r'):
panzhan 0:d80c66cb1b3a 136 command.printf("\n\rPF return zero\r");
panzhan 0:d80c66cb1b3a 137 // EnterMotorMode(&PF_can);
panzhan 0:d80c66cb1b3a 138 EnterMotorMode(&knee_txMsg);
panzhan 0:d80c66cb1b3a 139 EnterMotorMode(&ankle_txMsg);
panzhan 0:d80c66cb1b3a 140 return_zero = 1;
panzhan 0:d80c66cb1b3a 141 c_lock = 2;
panzhan 0:d80c66cb1b3a 142 // send_enable = 1;
panzhan 0:d80c66cb1b3a 143 // flag_gd = 1;
panzhan 0:d80c66cb1b3a 144 break;
panzhan 0:d80c66cb1b3a 145 }
panzhan 0:d80c66cb1b3a 146 /*
panzhan 0:d80c66cb1b3a 147 switch(Suse[5])
panzhan 0:d80c66cb1b3a 148 {
panzhan 0:d80c66cb1b3a 149 case('e'):
panzhan 0:d80c66cb1b3a 150 command.printf("DF exiting motor mode\n\r");
panzhan 0:d80c66cb1b3a 151 ExitMotorMode(&DF_can);
panzhan 0:d80c66cb1b3a 152 c_lock = 1; // 电机位置解锁
panzhan 0:d80c66cb1b3a 153 send_enable = 0; // main不发送CAN位置命令
panzhan 0:d80c66cb1b3a 154 break;
panzhan 0:d80c66cb1b3a 155 case('m'):
panzhan 0:d80c66cb1b3a 156 command.printf("DF entering PC motor mode\n\r");
panzhan 0:d80c66cb1b3a 157 EnterMotorMode(&DF_can);
panzhan 0:d80c66cb1b3a 158 c_lock = 1; // 电机位置锁定
panzhan 0:d80c66cb1b3a 159 send_enable = 1; // main发送CAN位置命令
panzhan 0:d80c66cb1b3a 160 break;
panzhan 0:d80c66cb1b3a 161 case('M'):
panzhan 0:d80c66cb1b3a 162 command.printf("DF entering BOARD motor mode\n\r");
panzhan 0:d80c66cb1b3a 163 EnterMotorMode(&DF_can);
panzhan 0:d80c66cb1b3a 164 c_lock = 0; // 电机位置解锁
panzhan 0:d80c66cb1b3a 165 send_enable = 0; // M模式下，命令是否发送，命令是什么由calculate决定
panzhan 0:d80c66cb1b3a 166 break;
panzhan 0:d80c66cb1b3a 167 case('z'):
panzhan 0:d80c66cb1b3a 168 command.printf("DF zeroing\n\r");
panzhan 0:d80c66cb1b3a 169 Zero(&DF_can);
panzhan 0:d80c66cb1b3a 170 break;
panzhan 0:d80c66cb1b3a 171 }
panzhan 0:d80c66cb1b3a 172 */
panzhan 0:d80c66cb1b3a 173
panzhan 0:d80c66cb1b3a 174 wait_ms(2);
panzhan 0:d80c66cb1b3a 175
panzhan 0:d80c66cb1b3a 176 // sf_m_c = 0; // 恢复接收模式
panzhan 0:d80c66cb1b3a 177 wait_us(200);
panzhan 0:d80c66cb1b3a 178
panzhan 0:d80c66cb1b3a 179
panzhan 0:d80c66cb1b3a 180
panzhan 0:d80c66cb1b3a 181 // 电机模式命令在此发送，锁定控制模式的位置量在main中发送
panzhan 0:d80c66cb1b3a 182 WriteAll();
panzhan 0:d80c66cb1b3a 183 //wait(1.0f);
panzhan 0:d80c66cb1b3a 184
panzhan 0:d80c66cb1b3a 185 // 解除串口控制，缓冲区清0
panzhan 0:d80c66cb1b3a 186 command_control_flag = 0;
panzhan 0:d80c66cb1b3a 187 for(unsigned int i = 0; i < 10; i++)
panzhan 0:d80c66cb1b3a 188 {
panzhan 0:d80c66cb1b3a 189 Suse[i] = 0;
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
panzhan 0:d80c66cb1b3a 228
panzhan 0:d80c66cb1b3a 229
panzhan 0:d80c66cb1b3a 230
panzhan 0:d80c66cb1b3a 231
panzhan 0:d80c66cb1b3a 232