/************************************************************************************************************************
*	Program:		finally.c							*
*	Description:	This program was written to test the pneumatic		                *
*					valves and cylinders.  It will prompt the user	*
*					to select the desired function (manual or 		*
*					automatic positioning) and to select the desired      *
*					characteristics of the PWM signal for the		*
*					valves.					*
*	Programmer:		Scott Nortman					*
*	Date:			March, 2000					*
************************************************************************************************************************/

/**************************** Constants *********************************/
#define pulse_handler TOC2_isr
#define tolerance 10 
#define valve_sensor analog(1)
/**************************** End of Constants **************************/

/*************************** Interrupts *********************************/
#pragma interrupt_handler TOC2_isr, RTI_isr, TOF_isr;
/*************************** End of Interrupts **************************/

/************************** Includes ************************************/
#include <tjpbase.h>
#include <analog.h>
#include <vectors.h>
#include <mil.h>
#include <hc11.h>
/************************ End of includes *******************************/

/************************** Global Variables*****************************/
long duty_cycle;	//duty_cycle in e clocks
long period;		//period in e clocks
int error[3] = {0};
int desired = 150;
long count = 0;
	
/************************ End of Global Variables ***********************/


/************************** Function Declarations  **********************/
void init_pulse();		//Initilizes TOC for PWM interrupts
void pulse_handler();	//This is the function executed at each TOC2_isr
/******************** End of Function Declarations  *********************/


/**************************** Main **************************************/

void main(void)
{
	int Mp, Md;
	int Kp, Kd;
	/* VT100 clear screen */ 
	char clear[]= "\x1b\x5B\x32\x4A\x04"; 
	
	/* VT100 position cursor at (x,y) = (3,12) command is "\x1b[3;12H"*/
  	char place[]= "\x1b[1;1H";	/*Home*/
 
	SET_BIT(PACTL, 0x80);
	CLEAR_BIT(PACTL, 0x03); 	//Set RTI rate
	SET_BIT(TMSK2, 0x40);		//Enable RTI interrupt
	   	
 	init_pulse();
  	init_analog();
  	init_serial();

  	//printf("%s", clear);  
  	//printf("%s", place);
  	
  	printf("\tTitle:\t\tvalve.c\n"
  "\tProgrammer:\tScott Nortman\n"
  "\tDate:\t\tMarch, 2000\n"
  "\tVersion\t\t1\n\n");
  
	printf("PNEUMATIC CYLINDER DIAGNOSTIC PROGRAM\n\n");
	//printf("Enter the desired period in e clocks:\n");
	//period = (read_int() * 1000);
	printf("Enter the proportional gain\n");
	Kp = read_int();
	printf("Enter the derivative gain\n");
	Kd = read_int();
	INTR_ON();
	desired = 150;

	
	
	while(1)
	{
		
		//Proportional control
		Mp = Kp * error[0];
		//Derivative Control
		Md = Kd * (error[0] - error[1]);
		
		period = Mp + Md;
		duty_cycle = (period / 2);
	
		if (duty_cycle < 0)
		{
			duty_cycle = -1 * duty_cycle;
			CLEAR_BIT(PORTA, 0x20);
		}
		else
		{
			SET_BIT(PORTA, 0x20);
		}
		
	}/*End While*/
  	
}/*End Main*/


/**************************** End of Main *******************************/



/************************** Function Defintions *************************/

void init_pulse(void)
{
	INTR_OFF();
	SET_BIT(TCTL1, 0x80);	//Enable OC2 for falling edge on first sucessful compare (safe default)
	CLEAR_BIT(TCTL1, 0x40);
	TOC2 = 0;	//Set TOC2 to zero
	SET_BIT(TMSK1, 0x40);	//Enable OC2 mask
	//INTR_ON();
	
	printf("End of Init pulse\n\n");
}

void pulse_handler()
{	//printf("ISR\n");


	//enable toggle
	//CLEAR_BIT(TCTL1, 0x80);
	//SET_BIT(TCTL1, 0x40);
	if (duty_cycle <= 100)	
	//This handles if duty cycle is less than or equal to 100 e clocks ( almost always off)
	{
		SET_BIT(TCTL1, 0x80);	//Enable OC2 for falling edge (make sure pin goes low)
		CLEAR_BIT(TCTL1, 0X40);
		TOC2 += period;			//Add the period to TOC2
	}
	
	else if ((period - duty_cycle) <= 100)	//This handles what to do if the pin should almost always be high
	{
		CLEAR_BIT(TCTL1, 0x80);	//Set the pin to rising edge
		SET_BIT(TCTL1, 0X40);
		TOC2 += period;			//Add period
	}
	
	else		//This is for the general case
	{
		if (TCTL1 & 0x40)	//Test bit 6.  If it is a one, you are now high and must change to go low
		{
			CLEAR_BIT(TCTL1, 0x40);  //Clear bit 6 to set for falling edge on next interrupt
			SET_BIT(TCTL1, 0X80);
			TOC2 += duty_cycle;		//Add the duty cycle e clocks if you are now high
		}
		
		else //if(!(TCTL1 & 0x40))	//Test bit 6.  If it is low, you are now low and must change to go high
		{
			SET_BIT(TCTL1, 0xC0);	//Set bit 6 to go high next interrupt
			TOC2 += (period - duty_cycle);
		}
	}
	
	
	CLEAR_FLAG(TFLG1, 0x40)	//Clear the interrupt flag
}	//This is the end of the interrupt routine

void RTI_isr(void)
{
	printf("%d\n", valve_sensor);
	error[2] = error[1];
	error[1] = error[0];
	error[0] = desired - valve_sensor;
	count += 1;
	if (count >=500)
	{
		count = 0;
		if (desired == 150)
		{
			desired = 100;
		}
		
		else if (desired == 100)
		{
			desired = 150;
		}
	}
	CLEAR_FLAG(TFLG2, 0x40);
}

void TOF_isr(void)
{
	CLEAR_FLAG(TFLG1, 0x80);
}