📄 arithmetics.c
字号:
#include <string.h>
#include <stdio.h>
#include "Arithmetics.h"
#include "qmath.h"
#include "CommonDefine.h"
/*====================================================================================================
PID Function
The PID (比例、积分、微分) function is used in mainly
control applications. PIDCalc performs one iteration of the PID
algorithm.
=====================================================================================================*/
/*====================================================================================================
一、浮点位置式PID
//缺点是：每次PID的结果都需要累加误差，即每次结果都和以前数据有关，累加结果容易溢出
=====================================================================================================*/
/*=====================================================================================================
PosPID Calc
=====================================================================================================*/
double PosPIDCalc( PosPID *pp, double NextPoint ) //当前采样点
{
double dError, Error;
Error = pp->SetPoint - NextPoint; // 当前偏差
pp->SumError += Error; // 当前积分
pp->PrevError = pp->LastError;
pp->LastError = Error;
dError = pp->LastError - pp->PrevError; //当前微分
return (pp->Proportion * Error // 比例项
+ pp->Integral * pp->SumError // 积分项
+ pp->Derivative * dError // 微分项
);
}
/*=====================================================================================================
Initialize PosPID Structure
=====================================================================================================*/
void PosPIDInit (PosPID *pp)
{
memset ( pp,0,sizeof(PosPID)); //将pp内部的所有成员初始化为0 be included <string.h>
}
/*====================================================================================================
二、增量式定点PID
=====================================================================================================*/
/*
int32 IncPIDCalc( IncPID *pp, int16 NowPoint ) //当前采样点 //返回值为ADC定标
{
////// int16 Error;
int32 Error;//////
int32 PIDOut;
int32 FactPIDOut;
int32 PIDTemp;
//注意:实际上PID输出的值是point；但是这个point算出来值的基准是以采样值基准的，
//即基准是3v -> fff0，而我们实际要的得到的控制pwm
//的值是以7.5v对应三角波峰值得到的，即7.5->1875,即3v->750,所以等效变换的话，需要
//将point的值除以fff0/750＝87.36, 为了更简单，我们除以64，即右移6位。这个有点影响
//系统稳定性，需考虑，相当于在PID环节后，又加了一个p环节，其大小是87.36/64
//这个影响可以直接在设计PID参数时考虑
//采用ADC的定标
Error = pp->RefPoint-NowPoint; // 当前偏差 e[k]
// Error = pp->RefPoint+NowPoint; //采样电流反向
//所有的ERROR采用ADC定标
//a0,a1,a2采用Q11定标
//所有输出out定标由ADC定标和Q11定标共同决定
//???????????注意：计算中int16和int32共同，则计算结果应该是int32吧
// PIDTemp=pp->a0*Error-pp->a1*pp->PrevError+pp->a2*pp->PrevPrevError;
// PIDTemp=PIDTemp>>11; //ADC定标
//ADC定标
// PIDOut=pp->PrevPIDOut+PIDTemp;
//增量式PID算法：u(k)=u(k-1)+a0*e(k)-a1*e(k-1)+a2*e(k-2)
// a0,a1,a2由积分常数算
//yybb
////// PIDOut=pp->PrevPIDOut+(pp->a0*Error-pp->a1*pp->PrevError+pp->a2*pp->PrevPrevError);
PIDOut=Error*3;//*KKKK*2;//1.3;//////
//防止溢出,PIDOut是int32，先右移11位，转化为ADC定标，然后防止溢出。如果以int16为范围，对应ADC定标实际上就是3*8v饱和
////// if((PIDOut>>11)>2048)
////// PIDOut=4194304;
////// else if((PIDOut>>11)<-2048)
////// PIDOut=-4194304;
// if(PIDOut>2048)//////
// PIDOut=2048; //////
// else if(PIDOut<-2048)//////
// PIDOut=-2048;//////
//yybb
// if(PIDOut>2048)
// PIDOut=2048;
// else if(PIDOut<-2048)
// PIDOut=-2048;
// ControlCValue=PIDOut; //DA
//更新
////// pp->PrevPIDOut=PIDOut;
////// pp->PrevPrevError=pp->PrevError;
////// pp->PrevError=Error;
//返回值计算,重新统一为ADC定标
//yybb
////// FactPIDOut=PIDOut>>11; //int32的变量，但保证了其值在32767－-32768之间，由于是ADC定标，相当于是24v的值
// FactPIDOut=PIDOut;
////// return FactPIDOut;
return PIDOut;//////
}
*/
int32 CurIncPIDCalc( IncPID *pp, int16 NowPoint ) //当前采样点 //返回值为ADC定标
{
int16 Error;
// int32 Error;//////
int32 PIDOut;
int32 FactPIDOut;
int32 PIDTemp;
//注意:实际上PID输出的值是point；但是这个point算出来值的基准是以采样值基准的，
//即基准是3v -> fff0，而我们实际要的得到的控制pwm
//的值是以7.5v对应三角波峰值得到的，即7.5->1875,即3v->750,所以等效变换的话，需要
//将point的值除以fff0/750＝87.36, 为了更简单，我们除以64，即右移6位。这个有点影响
//系统稳定性，需考虑，相当于在PID环节后，又加了一个p环节，其大小是87.36/64
//这个影响可以直接在设计PID参数时考虑
//采用ADC的定标
Error = pp->RefPoint-NowPoint; // 当前偏差 e[k]
// Error = pp->RefPoint+NowPoint; //采样电流反向
//所有的ERROR采用ADC定标
//a0,a1,a2采用Q11定标
//所有输出out定标由ADC定标和Q11定标共同决定
//???????????注意：计算中int16和int32共同，则计算结果应该是int32吧
// PIDTemp=pp->a0*Error-pp->a1*pp->PrevError+pp->a2*pp->PrevPrevError;
// PIDTemp=PIDTemp>>11; //ADC定标
//ADC定标
// PIDOut=pp->PrevPIDOut+PIDTemp;
//增量式PID算法：u(k)=u(k-1)+a0*e(k)-a1*e(k-1)+a2*e(k-2)
// a0,a1,a2由积分常数算
//yybb
PIDOut=pp->PrevPIDOut+(pp->a0*Error-pp->a1*pp->PrevError+pp->a2*pp->PrevPrevError);
// PIDOut=Error*2;//*KKKK*4;//////
//防止溢出,PIDOut是int32，先右移11位，转化为ADC定标，然后防止溢出。如果以int16为范围，对应ADC定标实际上就是3*8v饱和
if((PIDOut>>11)>2048)
PIDOut=4194304;
else if((PIDOut>>11)<-2048)
PIDOut=-4194304;
// if(PIDOut>2048)//////
// PIDOut=2048; //////
// else if(PIDOut<-2048)//////
// PIDOut=-2048;//////
//yybb
// if(PIDOut>2048)
// PIDOut=2048;
// else if(PIDOut<-2048)
// PIDOut=-2048;
// ControlCValue=PIDOut; //DA
//更新
pp->PrevPIDOut=PIDOut;
pp->PrevPrevError=pp->PrevError;
pp->PrevError=Error;
//返回值计算,重新统一为ADC定标
//yybb
FactPIDOut=PIDOut>>11; //int32的变量，但保证了其值在32767－-32768之间，由于是ADC定标，相当于是24v的值
// FactPIDOut=PIDOut;
return FactPIDOut;
//return PIDOut;//////
}
int32 VolIncPIDCalc( IncPID *pp, int16 NowPoint ) //当前采样点 //返回值为ADC定标
{
int16 Error;
int32 PIDOut;
int32 FactPIDOut;
int32 PIDTemp1;
int32 PIDTemp2;
int32 PIDTemp3;
//注意:实际上PID输出的值是point；但是这个point算出来值的基准是以采样值基准的，
//即基准是3v -> fff0，而我们实际要的得到的控制pwm
//的值是以7.5v对应三角波峰值得到的，即7.5->1875,即3v->750,所以等效变换的话，需要
//将point的值除以fff0/750＝87.36, 为了更简单，我们除以64，即右移6位。这个有点影响
//系统稳定性，需考虑，相当于在PID环节后，又加了一个p环节，其大小是87.36/64
//这个影响可以直接在设计PID参数时考虑
//采用ADC的定标
Error =pp->RefPoint-NowPoint ; //NowPoint-pp->RefPoint ; // 当前偏差 e[k]
//所有的ERROR采用ADC定标
//a0,a1,a2采用Q11定标
//所有输出out定标由ADC定标和Q11定标共同决定
//???????????注意：计算中int16和int32共同，则计算结果应该是int32吧
//PIDTemp1=pp->a0*Error-pp->a1*pp->PrevError+pp->a2*pp->PrevPrevError;
// PIDTemp1=PIDTemp1>>11; //ADC定标
//PIDOut=pp->PrevPIDOut+PIDTemp1; //ADC定标
//adc Q11
//PIDTemp1=pp->a0*Error;
//PIDTemp2=pp->a1*pp->PrevError;
//PIDOut=pp->PrevPIDOut+PIDTemp1-PIDTemp2;
//PIDTemp3=4106*512000;
//PIDTemp3=PIDTemp3>>11;
PIDOut=pp->PrevPIDOut+(pp->a0*Error-pp->a1*pp->PrevError+pp->a2*pp->PrevPrevError);
//增量式PID算法：u(k)=u(k-1)+a0*e(k)-a1*e(k-1)+a2*e(k-2)
// a0,a1,a2由积分常数算
//防止溢出
//yybb
//防止溢出,PIDOut是int32，先右移11位，转化为ADC定标，然后防止溢出。如果以int16为范围，对应ADC定标实际上就是3*8v饱和
if((PIDOut>>11)>50)
PIDOut=102400;
// else if((PIDOut>>11)<-3600)
// PIDOut=-7372800;
else if((PIDOut>>11)<-5000)//-3600)
PIDOut=-10240000;//-7372800;
/*
if((PIDOut>>11)>3600)
PIDOut=7372800;
// else if((PIDOut>>11)<-3600)
// PIDOut=-7372800;
else if((PIDOut>>11)<-500)
PIDOut=-1024000;
*/
/*
//yybb
if(PIDOut>3600)
PIDOut=3600;
else if(PIDOut<=0)
PIDOut=0;
*/
//更新
pp->PrevPIDOut=PIDOut;
pp->PrevPrevError=pp->PrevError;
pp->PrevError=Error;
//yybb
//返回值计算,重新统一为ADC定标 int32类型，但值限定在int16范围
FactPIDOut=PIDOut>>11;
// FactPIDOut=PIDOut;
return FactPIDOut;
}
//由比例积分微分常数计算增量式PID中的临时变量
void IncPIDTempVar( IncPID *pp)
{
pp->a0=(int32)pp->Proportion+pp->Integral+pp->Derivative;
pp->a1=(int32)pp->Proportion+((int32)pp->Derivative<<1);
pp->a2=(int32)pp->Derivative;
}
//PID参数浮点定标 此处采用Q11定标，参数最大可达16，最小精确到小数点后0.00048828125
void PIDParaF2F(IncPID *pp,float32 Prop, float32 Integ,float32 Deriv) //q15格式
{ pp->Proportion=(int16)(Prop*2048); // <<11;
pp->Integral=(int16)(Integ*2048 ); // <<11;
pp->Derivative=(int16)(Deriv *2048); // <<11;
}
void IncPIDInit (IncPID *pp)
{
memset ( pp,0,sizeof(IncPID)); //将pp内部的所有成员初始化为0 be included <string.h>
}
/*====================================================================================================
锁定工频市电频率和相位 Function
适用于锁定40-60Hz ,载波为20Khz, 即对应为500-334个点
=====================================================================================================*/
/*====================================================================================================
一、锁定采样电压正弦零点(从正到负)
=====================================================================================================*/
//要求:每个载波周期都要调用一次
//输入:单相电的采样电压
//输出:是否锁定到零点1为锁定零点
//A相
int16 LockAZero(int16 Voltage)
{
/* 由于需要初始化，所以放到全局变量中去了
int16 PlusPeriod=0; //正半周标志位0为不处于,1为处于正半周
int16 PlusNum=0;
int16 MinusNum=0; ///????????????????????????
*/
//找到正半周
if(APlusPeriod==0)
{
if(Voltage>=0)
APlusNum++;
else
APlusNum=0;
if(APlusNum==100) //一定处于正半周
{
APlusPeriod=1;
APlusNum=0;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -