lee sanghun / Mbed 2 deprecated 202001131705

45

Dependencies:   mbed BufferedSerial LS7366LIB FastPWM

speed_pid.h@0:c21936a3520a, 2020-03-23 (annotated)

Committer:
lsh2205
Date:
Mon Mar 23 08:38:40 2020 +0000
Revision:
0:c21936a3520a
ss

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsh2205 0:c21936a3520a 1 double taget_speed[6]={0,};
lsh2205 0:c21936a3520a 2 double error_speed[6]={0,};
lsh2205 0:c21936a3520a 3 double Speed_PID_OUTPUT[6]={0,};
lsh2205 0:c21936a3520a 4
lsh2205 0:c21936a3520a 5 void cal_speed_error()
lsh2205 0:c21936a3520a 6 {
lsh2205 0:c21936a3520a 7 for(int i=0;i<6;i++)
lsh2205 0:c21936a3520a 8 error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
lsh2205 0:c21936a3520a 9 }
lsh2205 0:c21936a3520a 10
lsh2205 0:c21936a3520a 11 double result_i[6]={0,};
lsh2205 0:c21936a3520a 12 double filter_result_i[6]={0,};
lsh2205 0:c21936a3520a 13 double output_i[6]={0,};
lsh2205 0:c21936a3520a 14
lsh2205 0:c21936a3520a 15 void Speed_I()
lsh2205 0:c21936a3520a 16 {
lsh2205 0:c21936a3520a 17 for(int i=0;i<6;i++)
lsh2205 0:c21936a3520a 18 {
lsh2205 0:c21936a3520a 19 if(filter_dif_encoder_data[i]<taget_speed[i])
lsh2205 0:c21936a3520a 20 result_i[i]+=1*Speed_Igain[i];
lsh2205 0:c21936a3520a 21 else if(filter_dif_encoder_data[i]>taget_speed[i])
lsh2205 0:c21936a3520a 22 result_i[i]-=1*Speed_Igain[i];
lsh2205 0:c21936a3520a 23
lsh2205 0:c21936a3520a 24 filter_result_i[i] = filter_result_i[i]*(1-Speed_I_input_filter[i]) + result_i[i]*Speed_I_input_filter[i];
lsh2205 0:c21936a3520a 25 output_i[i] = filter_result_i[i];
lsh2205 0:c21936a3520a 26 }
lsh2205 0:c21936a3520a 27 }
lsh2205 0:c21936a3520a 28 double output_p[6]={0,};
lsh2205 0:c21936a3520a 29 void Speed_P()
lsh2205 0:c21936a3520a 30 {
lsh2205 0:c21936a3520a 31 for(int i=0;i<6;i++)
lsh2205 0:c21936a3520a 32 output_p[i] = - error_speed[i] * Speed_Pgain[i];
lsh2205 0:c21936a3520a 33 }
lsh2205 0:c21936a3520a 34
lsh2205 0:c21936a3520a 35 void Speed_PID()
lsh2205 0:c21936a3520a 36 {
lsh2205 0:c21936a3520a 37 cal_speed_error();
lsh2205 0:c21936a3520a 38 Speed_I();
lsh2205 0:c21936a3520a 39 Speed_P();
lsh2205 0:c21936a3520a 40
lsh2205 0:c21936a3520a 41 for(int i=0;i<6;i++)
lsh2205 0:c21936a3520a 42 Speed_PID_OUTPUT[i] = output_i[i] + output_p[i];
lsh2205 0:c21936a3520a 43 }