test4
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
Diff: speed_pid.h
- Revision:
- 3:7b195612e26d
- Parent:
- 0:7cff999a7f5c
--- a/speed_pid.h Tue Jul 28 01:42:16 2020 +0000 +++ b/speed_pid.h Tue Dec 08 01:27:11 2020 +0000 @@ -2,38 +2,77 @@ #define _SPEED_PID_H_ double taget_speed[6]={0,}; +double ex_taget_speed[6]={0,}; +double dif_taget_speed[6]={0,}; +double filterd_dif_taget_speed[6]={0,0,0,0,0,0,}; + double error_speed[6]={0,}; double Speed_PID_OUTPUT[7]={0,}; - -void cal_speed_error() -{ - for(int i=0;i<6;i++) - error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i]; - -} +double ex_Speed_PID_OUTPUT[7]={0,}; double result_i[6]={0,}; double filter_result_i[6]={0,}; double output_i[6]={0,}; -void Speed_I() + +void cal_speed_error() { for(int i=0;i<6;i++) { - if(filter_dif_encoder_data[i]<taget_speed[i]) - result_i[i]+=1*Speed_Igain[i]; - else if(filter_dif_encoder_data[i]>taget_speed[i]) - result_i[i]-=1*Speed_Igain[i]; + if(i < 3 && taget_speed[i] == 0) + { + error_speed[i] = 0; + } + else + { + error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i]; + } + } + for(int i=0;i<6;i++) + { + dif_taget_speed[i] = ex_taget_speed[i] - taget_speed[i]; + ex_taget_speed[i]=taget_speed[i]; + + filterd_dif_taget_speed[i] = filterd_dif_taget_speed[i]*0.99 + dif_taget_speed[i]*0.01; + } + +} + + +void Speed_I() +{ + for(int i=0;i<6;i++) + { + result_i[i]+=error_speed[i]*Speed_Igain[i]; + + + if(result_i[i]>500) + result_i[i]=500; + if(result_i[i]<-500) + result_i[i]=-500; - filter_result_i[i] = filter_result_i[i]*(1-Speed_I_input_filter[i]) + result_i[i]*Speed_I_input_filter[i]; - output_i[i] = - filter_result_i[i]; + output_i[i] = result_i[i]; } } double output_p[6]={0,}; void Speed_P() { for(int i=0;i<6;i++) + { output_p[i] = error_speed[i] * Speed_Pgain[i]; + } + + for(int i=0;i<3;i++) + { + int p_range = motor_gain[i]*500; + + if(output_p[i]>p_range) + output_p[i]=p_range; + if(output_p[i]<-p_range) + output_p[i]=-p_range; + } + + } void Speed_PID()