void PID_activate(CONTROLLER *ctrl, float feedback, float *output) { float err = ctrl->Expected_output - feedback ; *output = PID_Controller(&ctrl->para, err); }
// Setting PWM of the fans void TIM2_IRQHandler(void) { if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) { TIM_ClearITPendingBit(TIM2, TIM_IT_Update); if(WORK == 1) { temp = get_temperature(1); set_ventillator_PWM(1, 100 * PID_Controller(&setpoint_1, temp, &error_previous_1, &actual_error_1, &P_1, &I_1, &D_1, Fan_1_PWM)); temp = get_temperature(3); set_ventillator_PWM(3, 100 * PID_Controller(&setpoint_3, temp, &error_previous_3, &actual_error_3, &P_3, &I_3, &D_3, Fan_3_PWM)); } else { set_ventillator_PWM(1,0); set_ventillator_PWM(3,0); } } }
void PID_activate_with_limitation(CONTROLLER *ctrl, float feedback, float *output, float up_limit, float down_limit) { float err = ctrl->Expected_output - feedback , temp_out = 0; //normalization err = err/ctrl->Expected_output; temp_out = PID_Controller(&ctrl->para, err); if (temp_out > up_limit) { temp_out = up_limit; } else if (temp_out < down_limit) { temp_out = down_limit; } //de-normalization *output = ctrl->Expected_output*temp_out; }
int main(void) { int i, ret_val; FILE *fp_e, *fp_i, *fp_d, *fp_p, *fp_u, *fp_u2, *fp_e2; float diff_E, diff_U, tot_diff_E, tot_diff_U; // PID input signals bool ResetN =1; data_type din[2], coeff[6], dout[2]; // CLOSED LOOP SYSTEM STATES float yd_prev = 0; float x1_prev = 0; // derivation states float yi_prev = 0; float x2_prev = 0; // integration states float y_z1 = 0; float y_z2 = 0; float u_z1 = 0; float u_z2 = 0; // closed loop system states float ref_out_prev = 0; float out_prev = 0; // PID AND PLANT SIGNALS float w[N]; float u[N], y[N], e[N]; float ref_e[N], ref_u[N], ref_y[N], ref_yi[N], ref_yd[N]; float ref_y_z1 = 0; float ref_y_z2 = 0; float ref_u_z1 = 0; float ref_u_z2 = 0; #ifdef PID_DEBUG float yd[N], yi[N]; data_type reg_yd = 0; data_type reg_yi = 0; float diff_P, diff_D, diff_I; float tot_diff_P = 0; float tot_diff_D = 0; float tot_diff_I = 0; #endif tot_diff_E = 0; tot_diff_U = 0; // STEP FUNCTION w[0] = 0; for (i=1; i<N; i++) w[i] = 1; // file I/O if ( ( fp_e = fopen ( (char *) "./test_data/e_res.txt", "w" ) ) == NULL ) { fprintf (stderr, "Cannot open input file %s\n", (char *) "e_res.txt"); exit (-1 ); } if ( ( fp_i = fopen ( (char *) "./test_data/i_res.txt", "w" ) ) == NULL ) { fprintf (stderr, "Cannot open input file %s\n", (char *) "i_res.txt"); exit (-1 ); } if ( ( fp_d = fopen ( (char *) "./test_data/d_res.txt", "w" ) ) == NULL ) { fprintf (stderr, "Cannot open input file %s\n", (char *) "d_res.txt"); exit (-1 ); } if ( ( fp_p = fopen ( (char *) "./test_data/yp_res.txt", "w" ) ) == NULL ) { fprintf (stderr, "Cannot open output file %s\n", (char *) "yp_res.txt"); exit (-1 ); } if ( ( fp_u = fopen ( (char *) "./test_data/u_res.txt", "w" ) ) == NULL ) { fprintf (stderr, "Cannot open output file %s\n", (char *) "u_res.txt"); exit (-1 ); } if ( ( fp_u2 = fopen ( (char *) "./test_data/pid_u_res.txt", "w" ) ) == NULL ) { fprintf (stderr, "Cannot open output file %s\n", (char *) "pid_u_res.txt"); exit (-1 ); } if ( ( fp_e2 = fopen ( (char *) "./test_data/pid_e_res.txt", "w" ) ) == NULL ) { fprintf (stderr, "Cannot open input file %s\n", (char *) "pid_e_res.txt"); exit (-1 ); } //Gi = coeff[0]; Gd = coeff[1]; C = coeff[2]; Gp = coeff[3]; //max_lim = coeff[4];//min_lim = coeff[6]; coeff[0] = (data_type) Gi; coeff[1] = (data_type) Gd; coeff[2] = (data_type) C; coeff[3] = (data_type) Gp; coeff[4] = (data_type) max_clip; coeff[5] = (data_type) min_clip; #ifdef PID_FIXEDPOINT fprintf(stdout, "RUNNING IN 24-bit FIXED POINT\n"); #else fprintf(stdout, "RUNNING IN 32-bit FLOATING POINT\n"); #endif #ifdef PID_DEBUG fprintf(stdout, "RUNNING IN DEBUG MODE\n"); #else fprintf(stdout, "RUNNING IN NORMAL MODE\n"); #endif // TEST BENCH for (i = 0; i<N; i++) { // error between input and output signals ref_e[i] = w[i] - ref_out_prev; // PID IN CLOSED LOOP ref_e[i] = (ref_e[i] > max_clip) ? max_clip : ref_e[i]; ref_e[i] = (ref_e[i] < min_clip) ? min_clip : ref_e[i]; float x1 = Gd*ref_e[i]; float x2 = Gi*ref_e[i]; // derivation stage // Yd(n-1) = -C * Yd(n-1) + X1(n) - X1(n-1) ref_yd[i] = -C*yd_prev + x1 - x1_prev; yd_prev = ref_yd[i]; x1_prev = x1; // integration // Ti(n) = X2(n) + X2(n-1) // Yi(n) = CLIP( Yi(n-1) + Ti(n) ) float ti = x2_prev + x2; ti = ti + yi_prev; ti = (ti > max_clip) ? max_clip : ti; ti = (ti < min_clip) ? min_clip : ti; ref_yi[i] = ti; yi_prev = ti; x2_prev = x2; // PID control output signal ref_u[i] = ref_e[i] * Gp + ref_yd[i] + ref_yi[i]; ref_u[i] = (ref_u[i] > max_clip) ? max_clip : ref_u[i]; ref_u[i] = (ref_u[i] < min_clip) ? min_clip : ref_u[i]; // plant ref_y[i] = 1.903*ref_y_z1 -0.9048*ref_y_z2 + 2.38e-005 *ref_u[i] + 4.76e-005 *ref_u_z1 + 2.38e-005*ref_u_z2; ref_y_z2 = ref_y_z1; ref_y_z1 = ref_y[i]; ref_u_z2 = ref_u_z1; ref_u_z1 = ref_u[i]; ref_out_prev = ref_y[i]; fprintf(fp_u, "%f\n", ref_u[i]); // C ref results of the ideal PID fprintf(fp_e, "%f\n", ref_e[i]); // C ref results of the ideal PID // EFFECTIVE Design Under Test din[0] = (data_type) w[i]; din[1] = (data_type) out_prev; #ifndef PID_DEBUG PID_Controller(ResetN, coeff, din, dout); #else PID_Controller(ResetN, coeff, din, dout, ®_yd, ®_yi); yd[i] = (float) reg_yd; yi[i] = (float) reg_yi; #endif //#ifndef PID_DEBUG u[i] = (float) dout[0]; e[i] = (float) dout[1]; // plant y[i] = 1.903*y_z1 -0.9048*y_z2 + 2.38e-005 *u[i] + 4.76e-005 *u_z1 + 2.38e-005*u_z2; y_z2 = y_z1; y_z1 = y[i]; u_z2 = u_z1; u_z1 = u[i]; out_prev = y[i]; #ifdef PID_DEBUG fprintf(fp_d, "%f\n", (float) yd[i]); fprintf(fp_i, "%f\n", (float) yi[i]); fprintf(fp_p, "%f\n", (float) y[i]); #endif fprintf(fp_u2, "%f\n", (float) u[i]); fprintf(fp_e2, "%f\n", (float) e[i]); // CHECK RESULTS // ERROR SIGNAL diff_E = (float) fabs( (float) e[i] - (float) ref_e[i]); tot_diff_E += diff_E; // PID OUTPUT diff_U = (float) fabs( (float) u[i] - (float) ref_u[i]); tot_diff_U += diff_U; #ifdef PID_DEBUG // INTEGRATOR OUTPUT diff_I = (float) fabs( (float) yi[i] - (float) ref_yi[i]); tot_diff_I += diff_I; // DERIVATOR OUTPUT diff_D = (float) fabs( (float) yd[i] - (float) ref_yd[i]); tot_diff_D += diff_D; // PLANT OUTPUT diff_P = (float) fabs( (float) y[i] - (float) ref_y[i]); tot_diff_P += diff_P; #endif } // for (i = 0; i<N; i++) fclose(fp_e); fclose(fp_d); fclose(fp_i); fclose(fp_u); fclose(fp_p); fclose(fp_u2); fclose(fp_e2); #ifdef PID_DEBUG fprintf(stdout, "\n TEST RESULT D=%f \n", tot_diff_D); fprintf(stdout, "\n TEST RESULT I=%f \n", tot_diff_I); fprintf(stdout, "\n TEST RESULT P=%f \n", tot_diff_P); #endif fprintf(stdout, "\n TEST RESULT U=%f \n", tot_diff_U); fprintf(stdout, "\n TEST RESULT E=%f \n", tot_diff_E); tot_diff_E += tot_diff_U; // total error during simulation fprintf(stdout, "\n TEST RESULT TOTAL ERROR =%f \n", tot_diff_E); if (tot_diff_E < PID_THRESHOLD) // MY THRESHOLD { ret_val = 0; fprintf(stdout, "\n TEST PASSED! \n"); } else { ret_val = 1; fprintf(stdout, "\n TEST FAILED! \n"); } return ret_val; }