test4
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
speed_pid.h
- Committer:
- lsh3146
- Date:
- 2020-12-08
- Revision:
- 3:7b195612e26d
- Parent:
- 0:7cff999a7f5c
File content as of revision 3:7b195612e26d:
#ifndef _SPEED_PID_H_ #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,}; double ex_Speed_PID_OUTPUT[7]={0,}; double result_i[6]={0,}; double filter_result_i[6]={0,}; double output_i[6]={0,}; void cal_speed_error() { for(int i=0;i<6;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; 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() { 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