You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
479 lines
16 KiB
479 lines
16 KiB
#define _PID_C
|
|
|
|
#include <GenericTypeDefs.h>
|
|
#include "PID.h"
|
|
#include "isr.h"
|
|
#include "iron.h"
|
|
#include "ExtFloat.h"
|
|
|
|
//volatile int LastOn;
|
|
//volatile unsigned char OffCnt;
|
|
//volatile unsigned char NoHeaterCnt;
|
|
|
|
void PIDInit(){
|
|
int i;
|
|
for(i = 2; i--;){
|
|
PIDVars[i].Starting = 1;
|
|
PIDVars[i].PIDDuty = 0;
|
|
PIDVars[i].PIDDutyP = 0;
|
|
PIDVars[i].PIDDutyI = 0;
|
|
PIDVars[i].PIDDutyFull = 0;
|
|
PIDVars[i].PWM = 0;
|
|
PIDVars[i].OffDelay = 1600;
|
|
}
|
|
};
|
|
|
|
#define intshr(a,b) ((a < 0) ? (-((-a) >> b)) : (a >> b))
|
|
|
|
#define assertin(a,b,c) \
|
|
if(b<c){\
|
|
if(a<b)a=b;\
|
|
if(a>c)a=c;\
|
|
}\
|
|
else{\
|
|
if(a<c)a=c;\
|
|
if(a>b)a=b;\
|
|
}
|
|
|
|
void PID(int PIDStep) {
|
|
int i, w, AVG;
|
|
INT32 dw, pdt;
|
|
t_PIDVars * PV;
|
|
t_IronConfig * IC;
|
|
int WSL;
|
|
|
|
AVG = ADCAVG;
|
|
PV =(t_PIDVars *)&PIDVars[PIDStep];
|
|
IC =(t_IronConfig *)&IronPars.Config[PIDStep];
|
|
if(IronPars.Config[1].Type){
|
|
AVG--;
|
|
}
|
|
else{
|
|
PV =(t_PIDVars *)&PIDVars[0];
|
|
IC =(t_IronConfig *)&IronPars.Config[0];
|
|
}
|
|
WSL = IC->WSLen;
|
|
if(WSL < 0) WSL = (IronPars.Config[1].Type ? 4 : 8);
|
|
|
|
if(PV->NoHeater || PV->NoSensor || PV->Starting || PV->ShortCircuit || PV->LastTTemp != CTTemp)PV->DestinationReached = 0;
|
|
PV->LastTTemp = CTTemp;
|
|
|
|
/**** GET ROOM TEMPERATURE **********************************************************/
|
|
if(PIDStep){
|
|
dw = ADCData.VRT >> 2;
|
|
dw *= 147;
|
|
dw >>= 8;
|
|
dw -= 100;
|
|
RTAvg -= intshr(RTAvg, ADCAVG);
|
|
RTAvg += dw;
|
|
CRTemp = intshr(RTAvg, ADCAVG);
|
|
}
|
|
/************************************************************************************/
|
|
|
|
/**** GET HEATER RESISTANCE *********************************************************/
|
|
if(PV->NoHeater) PV->NoHeaterCnt = (1 << (AVG + 1));
|
|
if(PV->NoHeaterCnt) PV->NoHeaterCnt--;
|
|
if(PV->NoHeater){
|
|
PV->HInitData = 1;
|
|
PV->HVAvg = 0;
|
|
PV->HIAvg = 0;
|
|
PV->HRAvg = 0x7FFF;
|
|
PV->HPAvg = 0;
|
|
PV->OffDelay = 1600;
|
|
}
|
|
else{
|
|
if(PV->HInitData) PV->HRAvg = PV->HR << AVG;
|
|
if(PV->HNewData){
|
|
if(PV->HInitData){
|
|
PV->HVAvg = PV->HV << AVG;
|
|
PV->HIAvg = PV->HI << AVG;
|
|
PV->HRAvg = PV->HR << AVG;
|
|
PV->HPAvg = PV->HP << AVG;
|
|
PV->HInitData = 0;
|
|
}
|
|
PV->HVAvg -= PV->HVAvg >> AVG;
|
|
PV->HVAvg += PV->HV;
|
|
PV->HIAvg -= PV->HIAvg >> AVG;
|
|
PV->HIAvg += PV->HI;
|
|
PV->HRAvg -= PV->HRAvg >> AVG;
|
|
PV->HRAvg += PV->HR;
|
|
PV->HPAvg -= PV->HPAvg >> AVG;
|
|
PV->HPAvg += PV->HP;
|
|
PV->HNewData=0;
|
|
}
|
|
}
|
|
/************************************************************************************/
|
|
|
|
PV->ADCTemp[1]=PV->ADCTemp[0];
|
|
PV->ADCTemp[0] = ADCData.VTEMP[1];
|
|
|
|
/**** WAVE SHAPING *****************************************************/
|
|
PV->WSCorr = 0;
|
|
if(IC->WSLen){
|
|
if(PV->Starting || PV->NoHeaterCnt || PV->NoSensor || PV->ShortCircuit){
|
|
for(i = 8; i--; ){
|
|
PV->WSDelta[i].cnt = 0;
|
|
PV->WSDelta[i].val = 0;
|
|
PV->WSMul = 32768;
|
|
PV->WSTemp = 0;
|
|
}
|
|
}
|
|
else{
|
|
//if(PV->DestinationReached == 0) for(i = 8; i--;) PV->WSDelta[i].cnt = 0;
|
|
if(ADCData.HeaterOn){
|
|
if(PV->OffCnt > 1){
|
|
i = min(PV->OffCnt, 8);
|
|
w = PV->WSDelta[0].val;
|
|
if(i > PV->WSDelta[0].cnt){
|
|
PV->WSDelta[0].cnt = i;
|
|
PV->WSDelta[0].val = (PV->ADCTemp[1] - PV->ADCTemp[0]) << 1;
|
|
}
|
|
else if(i == PV->WSDelta[0].cnt){
|
|
PV->WSDelta[0].val = intshr(PV->WSDelta[0].val, 1);
|
|
PV->WSDelta[0].val += (PV->ADCTemp[1] - PV->ADCTemp[0]);
|
|
}
|
|
w -= PV->WSDelta[0].val;
|
|
for(i = 1; i < 8; i++) PV->WSDelta[i].val -= w;
|
|
}
|
|
PV->LastOn = PV->ADCTemp[0];
|
|
PV->OffCnt = 0;
|
|
PV->WSCorr = intshr(PV->WSDelta[0].val, 1);
|
|
}
|
|
else{
|
|
if(PV->OffCnt < 8){
|
|
if(PV->OffCnt > 0){
|
|
if(PV->WSDelta[PV->OffCnt].cnt==0){
|
|
PV->WSDelta[PV->OffCnt].val = intshr(PV->WSDelta[0].val, 1) - (PV->ADCTemp[0] - PV->LastOn);
|
|
PV->WSDelta[PV->OffCnt].cnt = 1;
|
|
}
|
|
else{
|
|
PV->WSDelta[PV->OffCnt].val = intshr(PV->WSDelta[PV->OffCnt].val, 1);
|
|
}
|
|
PV->WSDelta[PV->OffCnt].val += intshr(PV->WSDelta[0].val, 1) - (PV->ADCTemp[0] - PV->LastOn);
|
|
assertin(PV->WSDelta[PV->OffCnt].val, 0, PV->WSDelta[PV->OffCnt - 1].val);
|
|
if(PV->OffCnt == 1){
|
|
//WSMul = multiplier for damping waveshaping
|
|
if(PV->WSDelta[0].val != 0){
|
|
PV->WSMul = (((INT32)PV->WSDelta[1].val) << 16) / ((INT32)PV->WSDelta[0].val);
|
|
//PV->WSMul *= 1200;
|
|
//PV->WSMul >>= 10;
|
|
if(PV->WSMul < 0) PV->WSMul = 0;
|
|
if(PV->WSMul > 32768) PV->WSMul = 32768;
|
|
}
|
|
}
|
|
}
|
|
if(IC->WSLen < 0){
|
|
//damping waveshaping
|
|
if(PV->WSDelta[0].val > 0){
|
|
dw = (PV->WSMul * 1152) >> 10;
|
|
for(i = 1; i < PV->OffCnt; i++){
|
|
dw = (dw * ((PV->WSMul * 1700) >> 10)) >> 16;
|
|
}
|
|
}
|
|
else{
|
|
dw = PV->WSMul;
|
|
for(i = 1; i < PV->OffCnt; i++){
|
|
dw = (dw * PV->WSMul) >> 16;
|
|
}
|
|
}
|
|
PV->WSCorr = intshr((((INT32)PV->WSDelta[0].val) * dw), 17);
|
|
}
|
|
else{
|
|
//sample waveshaping
|
|
if(PV->OffCnt < IC->WSLen)PV->WSCorr = intshr(PV->WSDelta[PV->OffCnt].val, 1);
|
|
}
|
|
}
|
|
}
|
|
//if(IC->WSLen > 0){
|
|
//sample waveshaping
|
|
//if(PV->OffCnt < IC->WSLen){
|
|
//PV->WSCorr = intshr(PV->WSDelta[PV->OffCnt].val, 1);
|
|
//}
|
|
//else{
|
|
//PV->WSCorr += (intshr(PV->WSDelta[IC->WSLen - 1].val, 1) * (IC->WSLen-1)) / PV->OffCnt;
|
|
//}
|
|
//}
|
|
if(PV->OffCnt < 255)PV->OffCnt++;
|
|
}
|
|
}
|
|
/************************************************************************************/
|
|
w = PV->WSCorr *= 1024;
|
|
w >>= 10;//PV->WSCorr >>= 10
|
|
/**** GET AND NORMALISE IRON TEMPRATURE *********************************************/
|
|
w += PV->ADCTemp[0];
|
|
//w = PV->ADCTemp[0] + PV->WSCorr;
|
|
if(w < 0) w = 0;
|
|
|
|
//heater resistance compensation for series TC
|
|
//compensation = (HRCompCurrent * 19.6 * Gain * HRAvg)/65536
|
|
//compensation = (((HRCompCurrent * Gain * 20070) / 32767) * HRAvg) / 2048;
|
|
w -= ((((INT32)IC->Gain * (INT32)IC->HRCompCurrent * 20070L) >> 15) * (INT32)(PV->HRAvg >> AVG)) >> 11;
|
|
|
|
|
|
/************************************************************************************/
|
|
|
|
{
|
|
dw = w + IC->Offset;
|
|
if(dw < 0)dw = 0;
|
|
if(dw > 2047)dw = 2047;
|
|
|
|
/******* INPUT MILLIVOLTS CALCULATION ***********************************************/
|
|
//ADC = Vin * 750 * (IC->Gain / 256) * (1024 / 3000mV)
|
|
//mV = (256 * 3000 * ADC) / (750 * 1024 * IC->Gain)) = ADC / IC->Gain
|
|
ExtFloat x1;
|
|
if(x1.m = ((UINT32)dw) << 20){
|
|
x1.e = 138;
|
|
while (x1.m < 0x80000000){
|
|
x1.m <<=1;
|
|
x1.e--;
|
|
}
|
|
}
|
|
ExtFloatDivByUInt(x1, IC->Gain);
|
|
|
|
/******* Resistance calculation if resistive sensor *********************************/
|
|
//R=mV/Current=mV / ((1.225V * IC->Current) / (1600 * 256))
|
|
if(IC->Type==2){
|
|
const ExtFloat rcc = {
|
|
0xA72F0539, //1.6*256/1.225 32 bit mantissa
|
|
127+8 //1.6*256/1.225 exponent
|
|
};
|
|
ExtFloatMul(x1, rcc);
|
|
UINT32 Current;
|
|
if(IC->InputInv){
|
|
Current = IC->CurrentA; //divide by current A if channel A selected as positive input
|
|
if(!IC->CBandA) x1.e -=4; //divide by 16 if higher current band on channel A
|
|
}
|
|
else{
|
|
Current = IC->CurrentB; //divide by current B if channel B selected as positive input
|
|
if(!IC->CBandB) x1.e =-4; //divide by 16 if higher current band on channel B
|
|
}
|
|
if(Current == 0) Current = 1;
|
|
ExtFloatDivByUInt(x1, Current);
|
|
}
|
|
SFLOAT CX = {
|
|
{
|
|
x1.m >> 8,
|
|
x1.e,
|
|
0
|
|
}
|
|
};
|
|
PV->CPolyX = CX.f;
|
|
|
|
// >>> At this point in x1 we have millivolts if thermocouple sensor, or ohms if PTC/NTC sensor. <<<
|
|
|
|
/******* TEMPERATURE POLYNOMIAL CALCULATION *****************************************************/
|
|
//T = C0 + C1 * X + c2 * X^2 + C3 * X^3 + ... + C9 * X^9
|
|
|
|
LATBbits.LATB7 = 0;
|
|
|
|
int n; //current polynomial power
|
|
ExtFloat xn = x1;
|
|
ExtFloat PSum;
|
|
ExtFloat NSum;
|
|
|
|
//Load positive or negative sum with the first polynomial coefficient depending on it's sign
|
|
float2ExtFloat(PSum, IC->TPoly[0]);
|
|
if(((SFLOAT)IC->TPoly[0]).s){
|
|
NSum = PSum;
|
|
PSum.m = 0;
|
|
PSum.e = 0;
|
|
}
|
|
|
|
for(n = 1; n < 10; n++){
|
|
ExtFloat CSum, cn;
|
|
|
|
float2ExtFloat(cn, IC->TPoly[n]);
|
|
|
|
//get positive or negative sum depending on current coefficient sign
|
|
if(((SFLOAT)IC->TPoly[n]).s){
|
|
CSum = NSum;
|
|
}
|
|
else{
|
|
CSum = PSum;
|
|
}
|
|
|
|
ExtFloatMul(cn, xn);
|
|
|
|
ExtFloatAdd(CSum, cn);
|
|
|
|
//store in positive or negative sum depending on current coefficient
|
|
if(((SFLOAT)IC->TPoly[n]).s){
|
|
NSum = CSum;
|
|
}
|
|
else{
|
|
PSum = CSum;
|
|
}
|
|
|
|
//don't calculate next argument power if the end is reached
|
|
if(n >= 9) break;
|
|
|
|
//calculate next polynomial argument power
|
|
ExtFloatMul(xn, x1);
|
|
}
|
|
|
|
//calculate (PSum - NSum) * 2 in order to get integer temperature * 2
|
|
dw=0;
|
|
if(PSum.e >= NSum.e){
|
|
NSum.m >>= min(PSum.e - NSum.e, 32);
|
|
if(PSum.m > NSum.m){
|
|
if(PSum.e >= 125 + 32){
|
|
dw=1023;
|
|
}
|
|
else{
|
|
PSum.m -= NSum.m;
|
|
dw = ((UINT64)PSum.m << (PSum.e - 125)) >> 32;
|
|
if(dw > 1023)dw = 1023;
|
|
}
|
|
}
|
|
}
|
|
|
|
//Add room temperature if thermocouple
|
|
if(IC->Type == 1) dw += CRTemp;
|
|
if(dw > 1023)dw = 1023;
|
|
|
|
PV->CTemp[0] = dw;
|
|
LATBbits.LATB7 = 1;
|
|
|
|
}
|
|
|
|
// >>> At this point in CTemp[0] we have temeprature in degrees Celsius, multiplied by 2. <<<
|
|
|
|
if(PV->Starting || PV->NoHeaterCnt){
|
|
PV->Starting = 0;
|
|
i = (1 << ADCAVG);
|
|
while(i--){
|
|
PV->TBuff[i] = PV->CTemp[0];
|
|
PV->SlopeBuff[i] = PV->CTemp[0];
|
|
}
|
|
PV->TAvgP[0] = PV->CTemp[0];
|
|
PV->TAvgP[1] = PV->CTemp[0];
|
|
PV->TAvg=PV->CTemp[0];
|
|
i = AVG;
|
|
while(i--)PV->TAvg += PV->TAvg;
|
|
PV->TAvgF[0] = PV->TAvg;
|
|
PV->TAvgF[1] = PV->TAvg;
|
|
PV->TSlope = 0;
|
|
PV->TBPos = 0;
|
|
PV->SBPos = 0;
|
|
}
|
|
|
|
//TAvg = averaged temperature
|
|
PV->TAvg -= PV->TBuff[PV->TBPos];
|
|
PV->TAvg += PV->CTemp[0];
|
|
|
|
//TBuff is array with most recent temperatures, used for TAvg calculation
|
|
PV->TBuff[PV->TBPos] = PV->CTemp[0];
|
|
PV->TBPos++;
|
|
PV->TBPos &= (1 << AVG) - 1;
|
|
|
|
//TAvgF = software RC filtered temperature
|
|
PV->TAvgF[1] = PV->TAvgF[0];
|
|
PV->TAvgF[0] -= PV->TAvgF[0] >> AVG;
|
|
PV->TAvgF[0] += PV->CTemp[0];
|
|
|
|
//SlopeBuff is array with most recent averaged temperatures, used for TSlope calculation
|
|
PV->SlopeBuff[PV->SBPos] = PV->TAvg;
|
|
PV->SBPos++;
|
|
PV->SBPos &= (1 << AVG) - 1;
|
|
|
|
//TSlope is current temperature slope, used with DGain to calculate current predicted temperature for PI controller
|
|
PV->TSlope -= intshr(PV->TSlope, 2);
|
|
PV->TSlope += PV->SlopeBuff[(PV->SBPos - 1) & ((1 << AVG) - 1)];
|
|
PV->TSlope -= PV->SlopeBuff[PV->SBPos];
|
|
|
|
dw = ((INT32)IC->PID_DGain) * intshr(PV->TSlope, 2);
|
|
dw = intshr(dw, AVG + 2);
|
|
|
|
//TAvgP is array with current end previous temperature for PI controller, TAvgP=TAvgF + (TSlope * DGain) / 4
|
|
PV->TAvgP[1] = PV->TAvgP[0];
|
|
PV->TAvgP[0] = (PV->TAvgF[0] >> AVG);
|
|
PV->TAvgP[0] += dw;
|
|
if(PV->TAvgP[0] < 0) PV->TAvgP[0] = 0;
|
|
if(PV->TAvgP[0] > 2047) PV->TAvgP[0] = 2047;
|
|
|
|
if(PV->DestinationReached == 0 && WSL > 0){
|
|
if(!PV->NoHeater && !PV->NoSensor && !PV->Starting && !PV->ShortCircuit){
|
|
w = CTTemp << 2;
|
|
if( ((PV->TAvgP[0] >= w) && (PV->TAvgP[1] < w)) || ((PV->CTemp[0] >= w) && (PV->CTemp[1] < w)) ){
|
|
//Shut off the power when destination is reached in order to calculate waveshaping shape.
|
|
PV->WSDelta[0].cnt = WSL - 1;
|
|
PV->KeepOff = WSL + 1; //(1 << AVG) + 1;
|
|
PV->DestinationReached = 1;
|
|
}
|
|
}
|
|
if(PV->DestinationReached == 0){
|
|
w = PV->TAvgF[0] >> AVG;
|
|
w -= PV->WSTemp;
|
|
if(w <= -99 || w > 99){
|
|
PV->WSTemp += w;//PV->TAvgF[0] >> AVG;
|
|
PV->WSDelta[0].cnt = WSL - 1;
|
|
PV->KeepOff = WSL + 1; //(1 << AVG) + 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//Delta is array with current and previous difference between set and current TAvgP, multiplied by 8
|
|
PV->Delta[1] = PV->Delta[0];
|
|
PV->Delta[0] = CTTemp << 2;
|
|
PV->Delta[0] -= PV->TAvgP[0];
|
|
if(PV->Delta[0] > 511) PV->Delta[0] = 511;
|
|
if(PV->Delta[0] < -511) PV->Delta[0] = -511;
|
|
PV->Delta[0] <<= 5;
|
|
|
|
|
|
PV->PIDDutyI += PV->Delta[0] * (int)IC->PID_KI;
|
|
if((PV->PIDDutyP = PV->Delta[0] * (int)IC->PID_KP) >= 0){
|
|
dw = 0x00FFFFFF;
|
|
if(PV->PIDDutyP > dw) PV->PIDDutyP = dw;
|
|
dw -= PV->PIDDutyP;
|
|
if(PV->PIDDutyI > dw || (PV->DestinationReached == 0 && PV->Delta[0] > (100<<6) )) PV->PIDDutyI = dw;
|
|
}
|
|
else{
|
|
if(PV->PIDDutyI < 0) PV->PIDDutyI = 0;
|
|
dw = -(PV->Delta[0]) * (int)IC->PID_OVSGain;
|
|
if(dw > 4095) dw = 4095;
|
|
dw <<= 12;
|
|
dw = 0x00FFFFFF - dw;
|
|
if(PV->PIDDutyI > dw) PV->PIDDutyI = dw;
|
|
}
|
|
pdt = PV->PIDDutyP + PV->PIDDutyI;
|
|
if(pdt < 0) pdt = 0;
|
|
if((IC->Type == 0) || (IC->Type == 255) || PV->NoSensor) pdt = 0;
|
|
|
|
PV->PIDDutyFull = pdt; //normalized duty
|
|
|
|
/**** RECALCULATE DUTY FOR REAL VS RATED POWER*********************************/
|
|
//Duty=Duty * rated power / peak power
|
|
pdt += 0x7FL;
|
|
pdt >>= 8;
|
|
if(PV->HP){
|
|
dw = PV->HPAvg >> AVG;
|
|
pdt *= (INT32)IC->PID_PMax;
|
|
pdt /= dw;
|
|
}
|
|
/******************************************************************************/
|
|
|
|
/**** RECALCULATE DUTY FOR MAX 6.0A RMS CURRENT******************************/
|
|
dw=0xFA00L;
|
|
if(PV->HI){
|
|
dw = PV->HIAvg >> AVG;
|
|
if(dw > 192){
|
|
dw = (INT32)(65535 * 192) / dw;
|
|
}
|
|
else{
|
|
dw=0xFA00;
|
|
}
|
|
}
|
|
if(pdt > dw) pdt = dw;
|
|
/******************************************************************************/
|
|
|
|
pdt <<= 8;
|
|
if(PV->NoHeater) pdt=0x28F5C; //PWM = once per 2 seconds in order to detect heater resistance on open heater
|
|
if(PV->ShortCircuit) pdt=0x10624; //PWM = once per 5 seconds in order to detect heater resistance on short circuit
|
|
|
|
PV->PIDDuty = pdt;
|
|
|
|
}
|
|
|
|
#undef _PID_C
|