![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
RH_leg_controller
main.cpp
- Committer:
- OscarLiao
- Date:
- 2018-12-13
- Revision:
- 0:6626810b2241
File content as of revision 0:6626810b2241:
/* This is a program for localized controller of one leg Endpoint space Impedence Control and Force/Torque feedforward Two input was acceptable q_0_E[3] Neutral point for Impedence reference T_0[3] Feedforward of Force/Torque ---------o------↺ q_act_est[0] /| L1 / | ⤸ q_act_est[1] o | L2 // / / ⤸ q_act_est[2] o / MCU target: F303K8 ADC config: 3.3 Vadc common source */ //**************** Wiring Map ********************// // |---------------| | |---------------| // Srl| PA_9 | D1 |Cmd Serail | | VIN | VIN | // Srl| PA_10 | D0 |Cmd Serial | | GND | GND | // | NRST | NRST | | | NRST | NRST | // | GND | GND | | | 5V | 5V | // | PA_12 | D2 | |Srl| A7 | PA_2 |PC Serial // PWM| PB_0 | D3 |Shing Drv |Dou| A6 | PA_7 |CSG // Dou| PB_7 | D4 |Task1 |Din| A5 | PA_6 |CS2 Stretch Cmd // Dou| PB_6 | D5 |Task2 |Din| A4 | PA_5 |CS1 Serial Sync // PWM| PB_1 | D6 |Tigh Drv |Ain| A3 | PA_4 |Shing sense // | PF_0 | D7 | |Ain| A2 | PA_3 |Tigh sense // | PF_1 | D8 | |Ain| A1 | PA_1 |Hip sense // PWM| PA_8 | D9 |Hip Drv | | A0 | PA_0 | // | PA_11 | D10 | | | AREF | AREF |VR source // | PB_5 | D11 | | | 3V3 | 3V3 | // | PB_4 | D12 | |Dou| D13 | PB_3 |led / Serial error // |---------------| | |---------------| #include "mbed.h" //#include "LSM9DS0_SH.h" //#define DEBUG 1 #define DEBUG 0 #define pi 3.141592f #define d2r 0.01745329f #define Rms 5000 //TT rate #define dt 0.015f #define Task_1_NN 2 #define Task_2_NN 199 //Torque-Controller gain #define G1_rad_dc 0.00157f #define Kqff 636.7f //ffoward gain of q_act_est = 1 / G1_rad_dc #define KTff 60.408f //ffoward gain of T_ref_WD = 1 / (K_q * G1_rad_dc) #define Kpt 22.9f #define Ki 151.9f //ADC reference constant #define Kadc 5.817f //adc value to q_spring #define K_spring 13180f //sping constant of single spring #define K_q 10.544f //2*K_spring*R^2 #define R 0.02f //wheely's radius //Endpoint Impedence Contorller coefficcient #define Kpi_Hip 0 #define Kpi_Thigh 2000 #define Kpi_Shin 8 #define Kd_Hip 0.0 #define Kd_Thigh 0.0 #define Kd_Shin 0.0 #define KN 20 #define L1 0.120f //Tigh length #define L2 0.1480f //Shing lenth #define LA (L1*L1+L2*L2) //L1^2 + L2^2 #define LB (L1*L2) //L1*L2 #define LC (L1/L2) //L1/L2 #define L0 0.2f //Length of the leg //Structure #define PWM_Hip 1650 //900~1550~2200 2050 #define PWM_Thigh 1860 //900~1550~2200 1300 #define PWM_Shin 1680 //900~1550~2200 1450 1740 #define Buff_size 16 //Serial Buffer #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡GPIO registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// //╔═════════════════╗ //║ Structure ║ //╚═════════════════╝ DigitalOut led(D13); //detection DigitalOut led2(D12); //detection DigitalOut Task_1(D4); //Task indicate DigitalOut Task_2(D5); DigitalOut CSG(A6); //Stretch interrupt pull down AnalogIn adc1(A1); //adc AnalogIn adc2(A2); //adc AnalogIn adc3(A3); //adc InterruptIn CS1(D2); //Serial synchronizor DigitalIn CS2(A5); //Stretch interrupt //╔═════════════════╗ //║ Servo out ║ //╚═════════════════╝ PwmOut Drive1(D9); //PWM 1/1 PwmOut Drive2(D6); //PWM 1/2N PwmOut Drive3(D3); //PWM 1/3N //╔═════════════════╗ //║ Serial ║ //╚═════════════════╝ Serial Debug(PA_2, PA_15); //Serial reg(TX RX) USB port Serial Cmd(D1, D0); //Serial reg(TX RX) //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of GPIO registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Varible registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// //╔═════════════════╗ //║ Structure ║ //╚═════════════════╝ Ticker TT; //call a timer uint8_t Task_1_count = 0; //1st prior task count uint8_t Task_2_count = 0; //2nd prior task count uint8_t Flag_1 = 0; //1st prior task flag uint8_t Flag_2 = 0; //2nd prior task flag //╔═════════════════╗ //║ ADC ║ //╚═════════════════╝ //╔═════════════════╗ //║ PWM ║ //╚═════════════════╝ const int16_t PWM_base[3] = { //reference command at 0, -45, 45 deg PWM_Hip,PWM_Thigh,PWM_Shin }; int16_t PWM[3] = { //command out PWM_base + q_ref 0, 0, 0 //transfer: 10us to 1 deg }; //╔═════════════════╗ //║ q_actEstimator ║ //╚═════════════════╝ /* q_act_est = q_ref*G1_rad_TUT(z) - q_spring G1_rad_TUT(z) is shown below y(z) 1.316 + 2.633*z^-1 + 1.316*z^-2 b0 + b1*z^-1 + b2*z^-2 ----- = ----------------------------------- * 1e-04 = ------------------------ x(z) 1 - 0.8447*z^-1 + 0.1799*z^-2 1 - a1*z^-1 - a2*z^-2 q_ref═>[Σ]════════╦═>[b0]═>[Σ]═[1e-04]═>y ⇑ ⇓w0 ⇑ ║ [1/z] ║ [Σ]<═[a1]<═╬═>[b1]═>[Σ] ⇑ ⇓w1 ⇑ ║ [1/z] ║ ╚═══[a2]<═╩═>[b2]═══╝ w2 */ float q_act_est[3] = { //output of filter 0, 0, 0 }; float L_act_est = 0.0; float P_act_est = 0.0; float G1_w_n[3][3] = { //G1_rad_TUT's {0, 0, 0}, //{w0, w1, w2} {0, 0, 0}, //second sea {0, 0, 0}, }; const float G1_a[3] = { //{a0, a1, a2} 1.00, 0.8447, -0.1799 }; const float G1_b[3] = { //{b0, b1, b2} 1.316, 2.633, 1.316 }; //╔═════════════════╗//╔══════════════════╗ //║ Comp_ref ║//║ Controller Input ║ //╚═════════════════╝//╚══════════════════╝ /* ---------o------↺ q_act_est[0] /| L1 / | ↻ q_act_est[1] o | L2 // / / ↻ q_act_est[2] o / 1 - z^-1 PDn(z) = P + D*N*--------------------- 1 - (1 - N*Ts)*z^-1 y(z) b0 + b1*z^-1 1 + (-1)*z^-1 ----- = -------------- = ------------------ x(z) 1 - a1*z^-1 1 - 0.7*z^-1 x═>[Σ]═══════╦═>[b0]═>[Σ]═>[D*N]═>y ⇑ ⇓w0 ⇑ ║ [1/z] ║ ╚══[a1]<═╩═>[b1]═══╝ w1 */ float K_c[3] = { //K_c reference impedence Kpi_Hip, Kpi_Thigh, Kpi_Shin //{Hip, Thigh, Shin} }; float B_c[3] = { //B_c = D*N (*20)reference damping (Dmax~0.4)(B_c_max~5) in joint!! Kd_Hip*KN, Kd_Thigh*KN, Kd_Shin*KN //{Hip, Thigh, Shin} }; float q_0_E[3] = { //"Controller Input" from upper controller in endpoint level (Operational Space) 0, 0.2, 0 //{Hip, leg length, leg angle} original point }; float q_0_E_1_gain = 0.2f/255.0f; float q_0_E_2_gain = (pi/4)/128.0f; float T_0[3] = { //"Controller Input" from upper controller in joint level 0, 0, 0 //{T_0_Hip, T_0_Tigh, F_0_Stretch} force feed foward }; float q_err_E[3] = { //error in endpoint level 0, 0, 0 }; float q_err_D_E[3] = { //derivative error in endpoint level 0, 0, 0 }; float Cref_w[3][2] = { //filter storage {0, 0}, {0, 0}, {0, 0}, }; const float Cref_a[2] = { //{a0, a1} 1.00, 0.70 }; const float Cref_b[2] = { //{b0, b1} 1.00, -1.00 }; float J32 = 0.0; float J33 = 0.0; float Phi1_0 = 0.0; float Phi2_0 = 0.0; //╔═════════════════╗ //║ Torque_reg ║ //╚═════════════════╝ /* q_act_est T_ref_WD q_ref = ----------- + -------------------------- + PI( T_ref - 2*K_spring*R^2*q_spring ) G1_rad_dc 2*K_spring*R^2*G1_rad_dc PI(z) = P + I*dt / (z-1) */ const float Kp[3] = { //P gain for Torque REG Kpt, Kpt, Kpt //{Hip, Tigh, Shin} }; const float Kit[3] = { //I gain for Torque REG Kit = Ki*Ts Ki*dt, Ki*dt, Ki*dt //{Hip, Tigh, Shin} }; float T_ref[3] = { //command to T_reg in joint level 0, 0, 0 //{Hip, Tigh, Shin} }; float T_ref_E[3] = { //command to T_reg in end point level 0, 0, 0 //{Hip, Len, Phi} }; float T_ref_WD[3] = { //command without damping reference in joint level 0, 0, 0 }; float T_ref_WD_E[3] = { //command without damping reference in endpoint level 0, 0, 0 }; float T_err[3] = { //T_ref - T_act_est, T_act_est = q_spring * K_q 0, 0, 0 //{Hip, Tigh, Shin} }; float q_ref[3] = { //output of T_reg 0, 0, 0 //{Hip, Thigh, Shin} }; float q_spring[3] = { //read in spring travel 0, 0, 0 }; float q_spring_0[3] = { //neutral point of VR 0, 0, 0 }; float T_reg_I[3] = { //PI_w 0, 0, 0 }; //╔═════════════════╗ //║ I/O Serial ║ //╚═════════════════╝ uint8_t Buff[Buff_size]; uint8_t Recieve_index = 0; uint8_t Cmd_Flag = 0; uint8_t Rx_enable = 0; char buffer[128]; //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Varible registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Function registor≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void init_TIMER(); //set TT_main() rate void TT_main(); //timebase function rated by TT void init_q_spring_0(); //calibrating void init_IO(); //initialize IO state void q_spring_Est(); void q_act_Est(); void Impedence_Ref(); void Torque_Reg(); void Rx_irq(); void Rx_srt(); void Rx_end(); //void NVIC_DisableIRQ(USART1_IRQn); //void NVIC_EnableIRQ(USART1_IRQn); //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Function registor■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡main funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// int main() { Debug.baud(115200); //set baud rate Cmd.baud(115200); //set baud rate wait_ms(10); init_q_spring_0(); //calibrating q_spring reference point wait_ms(600); init_IO(); //initialized value init_TIMER(); //start TT_main Cmd.attach(&Rx_irq,Serial::RxIrq); //start recieving message // NVIC_DisableIRQ(USART1_IRQn); //stay down before CS1 signal CS1.fall(&Rx_srt); //attach the address of Rx_srt to falling edge as start signal of communication CS1.rise(&Rx_end); //attach the address of Rx_end to rise edge //for joint endpoint transform Phi1_0 = (acos((L0*L0+L1*L1-L2*L2)/(2*L0*L1))); Phi2_0 = (Phi1_0 + acos((L0*L0+L2*L2-L1*L1)/(2*L0*L2))); while(1) { //main() loop //Task judging start if(Flag_1 == 1) { //check pending //Task_1 start //read in condition & estimate state q_spring_Est(); q_act_Est(); //calculate control effort Impedence_Ref(); Torque_Reg(); //PWM Drive out process for(int i=0; i<3; i++) { PWM[i] = PWM_base[i] + q_ref[i]; // X + Controll } //PWM[0] = 1550; //PWM[1] = 1850; //PWM[2] = 1670; PWM[0] = constrain(PWM[0],800 ,2100);//safty constrain PWM[1] = constrain(PWM[1],800 ,2100);//safty constrain PWM[2] = constrain(PWM[2],800 ,2100);//safty constrain Drive1.pulsewidth_us(PWM[0]); //drive command Drive2.pulsewidth_us(PWM[1]); Drive3.pulsewidth_us(PWM[2]); //for Serial-Oscilloscope #if DEBUG // q_0[2] = -0.9; Debug.printf("%f\n", T_ref[2]); // Debug.printf("%f, %f, %f\r", q_0[0], q_0[1], q_0[2]); // Debug.printf("%lf\r", adc3.read()); // Debug.printf("%f\n", 123.23); // Debug.printf("%.f\r", q_ref[0]);L_act_est // Debug.printf("%.2f\r", q_spring_0[0]); // Debug.printf("%.2f\r", J33); #endif //Task_1 done Flag_1 = 0; //clear pending Task_1 = 0; //clear sign } if(Cmd_Flag == 1) { //{...}{Hip, Len, Phi}, {'#'} //Received some shit, start decoding led = 1; if(Buff[3] == 'Q') { //q_0 command q_0_E[0] = (Buff[0] - 128); q_0_E[1] = 0.1f + q_0_E_1_gain * Buff[1]; q_0_E[2] = q_0_E_2_gain * (Buff[2]-128); } else if(Buff[3] == 'T') { //T_0 command T_0[0] = 1.0f * (Buff[0] - 128); T_0[1] = 1.0f * (Buff[1] - 128); T_0[2] = 1.0f * (Buff[2] - 128); } else if(Buff[3] == 'K') { K_c[0] = 0.02f * Buff[0]; K_c[1] = 0.02f * Buff[1]; K_c[2] = 0.02f * Buff[2]; } else if (Buff[3] == 'B') { B_c[0] = 0.02f * (Buff[0] - 128); B_c[1] = 0.02f * (Buff[1] - 128); B_c[2] = 0.02f * (Buff[2] - 128); } else { //Show error in communication led2 = 1; } Buff[3] = 0x00; //clear end signals Cmd_Flag = 0; led = 0; //Send out message threw Cmd // Cmd.printf("%c %c %c\r", Dummy[0], Dummy[1], Dummy[2]); } } } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of main funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Timebase funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void init_TIMER() //set TT_main{} rate { TT.attach_us(&TT_main, Rms); } void TT_main() //interrupt function by TT { Task_1_count = Task_1_count + 1; if(Task_1_count > Task_1_NN) { Task_1_count = 0; //Task triggering Flag_1 = 1; Task_1 = 1; //show for indicator } Task_2_count = Task_2_count + 1; if(Task_2_count > Task_2_NN) { Task_2_count = 0; //Task triggering Flag_2 = 1; Task_2 = 1; //show for indicator } } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Timebase funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡init_IO funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void init_IO(void) //initialize { CS1.mode(PullUp); //A4 CS2.mode(PullUp); //A5 CSG = 0; //A6 led = 0; Drive1.period_us(15000); Drive2.period_us(15000); Drive3.period_us(15000); } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of init_IO funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡controller funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// //follow steps for discrete process in j oder //1.update w0[n] threw input x[n] & wi[n] from last sequence // w0[n] = x[n] + Zigma( ai*wi[n] ), i = 1, 2, 3...j-1, j. // y[n] = Zigma( bi*wi[n] ), i = 0, 1, 2...j-1, j. //2.update wi[n+1] for next sequence // wi[n] = wi-1[n], i = j, j-1...2, 1. void q_spring_Est(void) { //read in from adc q_spring[0] = -(Kadc * adc1.read() - q_spring_0[0]); q_spring[1] = -(Kadc * adc2.read() - q_spring_0[1]); q_spring[2] = -(Kadc * adc3.read() - q_spring_0[2]); } void q_act_Est(void) { for(int i=0; i<3; i++) { q_ref[i] = -(PWM[i] - PWM_base[i]); } //q_act_est = q_ref*G1_rad_TUT(z) - q_spring //G1_rad_TUT(z) is done below for(int i=0; i<3; i++) { //update w0[n] threw input x[n] & wi[n] from last sequence G1_w_n[i][0] = q_ref[i] + G1_a[1]*G1_w_n[i][1] + G1_a[2]*G1_w_n[i][2]; q_act_est[i] = G1_b[0]*G1_w_n[i][0] + G1_b[1]*G1_w_n[i][1] + G1_b[2]*G1_w_n[i][2]; q_act_est[i] = q_act_est[i]*0.0001f - q_spring[i]; //update wi[n+1] G1_w_n[i][2] = G1_w_n[i][1]; G1_w_n[i][1] = G1_w_n[i][0]; } //q_act_est[i] is now ready for following app } void Impedence_Ref(void) { //Transform q_act_est from joint to end point q_act_est[1] -= Phi1_0; q_act_est[2] += Phi2_0; L_act_est = sqrt( LA + 2*LB*cos(q_act_est[2]) ); P_act_est = q_act_est[1] + asin( L2*sin(q_act_est[2]) / L_act_est ); //Calculate error in end point level //q_0[i] => {Hip, Tigh, Shin} q_err_E[0] = q_0_E[0] - q_act_est[0]; //Hip motor#1 q_err_E[1] = q_0_E[1] - L_act_est; q_err_E[2] = q_0_E[2] - P_act_est; for(int i=0; i<3; i++) { //Generate T_ref in Operational Space (OSC control perhaps?) //T_ref_E = PDn( q_err_E ); //Compliance T_ref_WD_E[i] = K_c[i] * q_err_E[i]; //Damping //update w0[n] threw input x[n] & wi[n] from last sequence Cref_w[i][0] = q_err_E[i] + Cref_a[1]*Cref_w[i][1]; q_err_D_E[i] = Cref_b[0]*Cref_w[i][0] + Cref_b[1]*Cref_w[i][1]; //update wi[n+1] Cref_w[i][1] = Cref_w[i][0]; //Generate T_ref in endpoint level T_ref_E[i] = T_ref_WD_E[i] + B_c[i]*q_err_D_E[i]; } //Transform torque to joint space, T_ref[i] = Σ( J[i][j] * T_ref_E[j] ) J32 = -LB*sin(q_act_est[2]) / L_act_est; J33 = ( pow(J32,2)/LB + cos(q_act_est[2]) ) / (LC + cos(q_act_est[2])); //Torque of Hip T_ref[0] = T_ref_E[0]; T_ref_WD[0] = T_ref_WD_E[0]; //Torque of Tigh T_ref[1] = T_ref_E[2] + T_0[1]; T_ref_WD[1] = T_ref_WD_E[2] + T_0[1]; //Torque of Shin T_ref[2] = J32*T_ref_E[1] + J33*T_ref_E[2] + J32*T_0[1] + J33*T_0[2]; T_ref_WD[2] = J32*T_ref_WD_E[1] + J33*T_ref_WD_E[2] + J32*T_0[1] + J33*T_0[2]; } void Torque_Reg(void) { if(CS2 == 1) { //Transform q_act_est from end point to joint q_act_est[1] += Phi1_0; q_act_est[2] -= Phi2_0; //Normal command for(int i=0; i<3; i++) { //FeedFoward q_ref[i] = q_act_est[i]*Kqff + T_ref_WD[i]*KTff; //full feedfoward //q_ref[i] = 0; //w/o feedfoward //FeedBack T_err[i] = T_ref[i] - K_q*q_spring[i]; T_reg_I[i] = T_reg_I[i] + T_err[i]*Kit[i]; T_reg_I[i] = constrain(T_reg_I[i], -200, 200); q_ref[i] = -(T_err[i]*Kp[i] + T_reg_I[i] + q_ref[i]); } q_ref[0] = -(q_0_E[0]/128.0f)*225; q_ref[0] = constrain(q_ref[0], -650, 650); q_ref[1] = constrain(q_ref[1], -650, 650); q_ref[2] = constrain(q_ref[2], -650, 650); } else { //Stretch command for(int i=0; i<3; i++) { T_reg_I[i] = 0; q_ref[i] = 0; } } } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of controller funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡init_q_spring_0 funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void init_q_spring_0(void) { for(int i=0; i<1000; i++) { q_spring_0[0] = q_spring_0[0] + Kadc * adc1.read(); q_spring_0[1] = q_spring_0[1] + Kadc * adc2.read(); q_spring_0[2] = q_spring_0[2] + Kadc * adc3.read(); wait_us(100); } q_spring_0[0] = q_spring_0[0] /1000.0f; q_spring_0[1] = q_spring_0[1] /1000.0f; q_spring_0[2] = q_spring_0[2] /1000.0f; } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of init_q_spring_0 funtion■■■■■■■■■■■■■■■■■■■■■■■■// //≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡Rx_xxx funtion≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡≡// void Rx_irq(void) { //Keep clear in Rx_irq and leave most work after Rx_end Buff[Recieve_index] = Cmd.getc(); Recieve_index = Recieve_index + Rx_enable; } void Rx_srt(void) { //start receiving Serial message from Cmd // NVIC_EnableIRQ(USART1_IRQn); Rx_enable = 1; } void Rx_end(void) { //end of receiving Serial message from Cmd // NVIC_DisableIRQ(USART1_IRQn); Rx_enable = 0; Recieve_index = 0; Cmd_Flag = 1; } //■■■■■■■■■■■■■■■■■■■■■■■■■■■end of Rx_irq funtion■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■■//