test4
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
speed_pid.h
- Committer:
- gohgwaja
- Date:
- 2020-05-11
- Revision:
- 0:7cff999a7f5c
- Child:
- 3:7b195612e26d
- Child:
- 4:bf278ddb8504
File content as of revision 0:7cff999a7f5c:
#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