int PIDControl(float p_const, float i_const, float d_const, float *i_out, int max_out,
			   int cur_pos, int tar_pos, unsigned long time, int *prev_error);