110 lines
3.4 KiB
C
110 lines
3.4 KiB
C
|
/*!
|
|||
|
|
Copyright 2017 <EFBFBD><EFBFBD> "<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>" <EFBFBD> <EFBFBD><EFBFBD><EFBFBD> "<EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>"
|
|||
|
|
|
|||
|
|
Licensed under the Apache License, Version 2.0 (the "License");
|
|||
|
|
you may not use this file except in compliance with the License.
|
|||
|
|
You may obtain a copy of the License at
|
|||
|
|
|
|||
|
|
http://www.apache.org/licenses/LICENSE-2.0
|
|||
|
|
|
|||
|
|
Unless required by applicable law or agreed to in writing, software
|
|||
|
|
distributed under the License is distributed on an "AS IS" BASIS,
|
|||
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|||
|
|
See the License for the specific language governing permissions and
|
|||
|
|
limitations under the License.
|
|||
|
|
|
|||
|
|
\file V_pid_reg3.c
|
|||
|
|
\brief <EFBFBD><EFBFBD><EFBFBD>-<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> (<EFBFBD><EFBFBD>. TPidReg3)
|
|||
|
|
\author <EFBFBD><EFBFBD><EFBFBD> "<EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>". http://motorcontrol.ru
|
|||
|
|
\version v 2.0 25/03/2016
|
|||
|
|
|
|||
|
|
\addtogroup V_pid_reg3
|
|||
|
|
@{*/
|
|||
|
|
|
|||
|
|
#include "V_IQmath.h" // Include header for IQmath library
|
|||
|
|
#include "V_pid_reg3_pos.h"
|
|||
|
|
#include "stdlib.h"
|
|||
|
|
#include "main.h"
|
|||
|
|
//! <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//! <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//! <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> pid_reg3.pdf
|
|||
|
|
|
|||
|
|
//! <20><> <20><><EFBFBD><EFBFBD><EFBFBD> pid_ref_reg3 - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, pid_fdb_reg3 - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
//! <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> pid_out_reg3.
|
|||
|
|
|
|||
|
|
//! \memberof TPidReg3
|
|||
|
|
void pid_reg3_calc_pos(TPidReg3_pos *v) {
|
|||
|
|
v->e_reg3 = v->pid_ref_reg3 - v->pid_fdb_reg3;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> - <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
v->e_reg3Dz=v->e_reg3;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
if (v->DeadZone!=0){//<2F><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if (v->e_reg3Dz>0){//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD>
|
|||
|
|
v->e_reg3Dz=v->e_reg3Dz-v->DeadZone;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>
|
|||
|
|
if (v->e_reg3Dz<0)//<2F><> <20><><EFBFBD>, <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
v->e_reg3Dz=0;
|
|||
|
|
}
|
|||
|
|
if (v->e_reg3Dz<0){
|
|||
|
|
v->e_reg3Dz=v->e_reg3Dz+v->DeadZone;
|
|||
|
|
if (v->e_reg3Dz>0)
|
|||
|
|
v->e_reg3Dz=0;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
v->up_reg3 = _IQmpy(v->Kp_reg3, v->e_reg3Dz);
|
|||
|
|
|
|||
|
|
v->uprsat_reg3 = v->up_reg3 + v->ui_reg3 + v->ud_reg3;
|
|||
|
|
|
|||
|
|
if (v->uprsat_reg3 > v->pid_out_max)
|
|||
|
|
v->pid_out_reg3 = v->pid_out_max;
|
|||
|
|
else if (v->uprsat_reg3 < v->pid_out_min)
|
|||
|
|
v->pid_out_reg3 = v->pid_out_min;
|
|||
|
|
else
|
|||
|
|
v->pid_out_reg3 = v->uprsat_reg3;
|
|||
|
|
|
|||
|
|
v->saterr_reg3 = v->pid_out_reg3 - v->uprsat_reg3 + v->saterr_reg3Add;
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD> <20><><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>, <20><> <20><><EFBFBD>.
|
|||
|
|
if (v->Kf_d == 0){
|
|||
|
|
v->e_reg3_filterOut = v->e_reg3;//<2F><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
}
|
|||
|
|
else{//<2F><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
v->e_reg3_filterOut = posspeedEqep.speed_elec;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if ((v->DiffCounter++ + 1) >= v->DiffDelim) {//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>. <20><><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
if (v->KdFilterInitFlag==1){//<2F><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
v->e_reg3_filterOut = posspeedEqep.speed_elec;//<2F><><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
v->KdFilterInitFlag=0;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
}
|
|||
|
|
//v->ud_reg3 = _IQmpy(v->Kd_reg3, (v->e_reg3_filterOut - v->up1_reg3)<<6);//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
v->ud_reg3 = _IQmpy(v->Kd_reg3,-posspeedEqep.speed_filter.output);
|
|||
|
|
v->DiffCounter = 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (v->Ki_reg3 != 0)//<2F><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
v->ui_reg3 = v->ui_reg3
|
|||
|
|
+ _IQmpy(v->Ki_reg3,
|
|||
|
|
v->up_reg3) + _IQmpy(v->Kc_reg3,v->saterr_reg3);
|
|||
|
|
else
|
|||
|
|
v->ui_reg3 = 0;
|
|||
|
|
|
|||
|
|
if (v->Kc_reg3 == 0) {
|
|||
|
|
if (v->ui_reg3 > v->pid_out_max)
|
|||
|
|
v->ui_reg3 = v->pid_out_max;
|
|||
|
|
else if (v->ui_reg3 < v->pid_out_min)
|
|||
|
|
v->ui_reg3 = v->pid_out_min;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//! \memberof TPidReg3
|
|||
|
|
void pid_reg3_reset_pos(TPidReg3_pos *v) {
|
|||
|
|
v->pid_fdb_reg3=0;
|
|||
|
|
v->pid_ref_reg3=0;
|
|||
|
|
v->ui_reg3=0;
|
|||
|
|
v->KdFilterInitFlag=1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/*@}*/
|