test4
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
Diff: speed_pid.h
- Revision:
- 0:7cff999a7f5c
- Child:
- 3:7b195612e26d
- Child:
- 4:bf278ddb8504
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/speed_pid.h Mon May 11 08:47:18 2020 +0000 @@ -0,0 +1,59 @@ +#ifndef _SPEED_PID_H_ +#define _SPEED_PID_H_ + +double taget_speed[6]={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 result_i[6]={0,}; +double filter_result_i[6]={0,}; +double output_i[6]={0,}; + +void Speed_I() +{ + 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]; + + 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]; + } +} +double output_p[6]={0,}; +void Speed_P() +{ + for(int i=0;i<6;i++) + output_p[i] = error_speed[i] * Speed_Pgain[i]; +} + +void Speed_PID() +{ + cal_speed_error(); + Speed_I(); + Speed_P(); + + for(int i=0;i<6;i++) + { + Speed_PID_OUTPUT[i] = output_i[i] + output_p[i]; + if(i > 2) + { + Speed_PID_OUTPUT[i] *= -1; + } + } + + //pc.printf("%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f",error_speed[0],filter_dif_encoder_data[0],taget_speed[0],Speed_PID_OUTPUT[0],output_i[0],output_p[0]); + //pc.printf("\n"); + +} + +#endif