#include "heater.h"
#include "msp430x16x.h"  //register and I/O definitions

void HeaterServoLoop(int16 setPoint,char clHeatFlag, Heater* h)
{
int32 error=0;
int16	PWM=0;

	//if we are in closed loop mode calculate PWM (for P controller)
	if(clHeatFlag==true)
	{
	h->currentTemperature=(h->tempSense1-h->tempSense2)*(h->heaterSlope_num/h->heaterSlope_den)+h->tempSense1;
		
	error=setPoint-h->currentTemperature;
//	error=setPoint-h->tempSense1;
	PWM=(h->heating.P*error/4);
//	PWM=(h->heating.P*error/2);
	//add in error to form intergral
  	h->sum = h->sum + error;
   //check it's limits
  
 	if (h->sum > H_SUM_LIMIT) h->sum = H_SUM_LIMIT;
  	else if ( h->sum < -H_SUM_LIMIT) h->sum = -H_SUM_LIMIT;
  //calc the integral portion
 PWM += h->sum / h->heating.I;
 
	
	if(PWM>90)PWM=90;
	if(PWM<0)PWM=0;
	h->heaterPWM=PWM;
	TBCCR3=h->heaterPWM;
	}
	//open loop mode, set PWM to fixed value
	else
	{
	if(h->fixedPWM>90){h->fixedPWM=90;}
	h->heaterPWM=h->fixedPWM;
	TBCCR3=h->fixedPWM;
	}
}
void calculateSlope(Heater* h,int16 setPoint)
{
	if(setPoint<2457)
	{
	h->heaterSlope_den=2;
	h->heaterSlope_num=1;
	}
	else
	{
		h->heaterSlope_num=1000000;
		h->heaterSlope_den=690000-63*setPoint;
		if((h->heaterSlope_num/h->heaterSlope_den>1)|(h->heaterSlope_num/h->heaterSlope_den<0))
		{
		h->heaterSlope_den=2;
		h->heaterSlope_num=1;
		}
	}
}
	
void initHeater(Heater* h)
{
	h->tProgram.dwell=0;
	h->tProgram.targetT=0;
	h->tProgram.errHoldoff=0;
	h->tProgram.minT=0;
	h->tProgram.maxT=0;
	//goof gains appear to be P=1 I=400
	h->heating.P=1;
	h->heating.I=400;
	//h->heating.I=200;
	h->heating.D=0;
	h->cooling.P=0;
	h->cooling.I=0;
	h->cooling.D=0;
	h->heaterSlope_den=2;
	h->heaterSlope_num=1;
	h->currentTemperature=0;	//maybe calculated temp including adjustment
	h->elapsedTime=0;
	h->heaterPWM=0;
	h->coolerPWM=0;
	h->tempSense1=0;
	h->tempSense2=0;
	h->fixedPWM=0;
	h->clHeatFlag=0;
	h->sum=0;	 		//sum of error for integral term

}