/************************************************************************************************************************
*	Program:		PID.c							*
*	Description:	This program used a PID controller algorithm to position the cylinders
*					valves.					*
*	Programmer:		Scott Nortman					*
*	Date:			February 28, 2000					*
************************************************************************************************************************/

/**************************** Constants *********************************/
#define pulse_handler TOC2_isr
#define TOLERANCE 5 
#define valve_sensor analog(1)
#define in	0
#define out 1
/**************************** End of Constants **************************/

/*************************** Interrupts *********************************/
#pragma interrupt_handler TOC2_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*****************************/
unsigned int duty_cycle;	//duty_cycle in e clocks
unsigned int period;		//period in e clocks
int position;
/************************ End of Global Variables ***********************/


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


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

void main(void)
{
	/* 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_home[]= "\x1b[1;1H";	/*Home*/
  	
  	char place_text[] = "\x1b[10;1H";/*position for text to be entered*/
  	
  	int duty_cycle_P;
	int period_ms;
	int desired_position = 220;
	int option;
	
	SET_BIT(PACTL, 0x80);  	
 	init_pulse();
  	init_analog();
  	init_serial();

  	printf("%s", clear);  
    printf("%s", place_home);
  	
  	printf("\tTitle:\t\tvalve.c\n"
  "\tProgrammer:\tScott Nortman\n"
  "\tDate:\t\tFebruary, 2000\n"
  "\tVersion\t\t1\n\n");
  
	printf("PNEUMATIC CYLINDER DIAGNOSTIC PROGRAM\n\n");
	period = 40000;
	duty_cycle = 15000;
	INTR_OFF();
	while (1){
		INTR_ON();
		while (desired_position < valve_sensor){
			CLEAR_BIT(PORTA, 0x20);
		}
		while (desired_position > valve_sensor) {
			SET_BIT(PORTA, 0x20);
		}
		while ((desired_position <= valve_sensor + TOLERANCE) && (desired_position >= valve_sensor - TOLERANCE)){
			INTR_OFF();
		}
	}
	
}


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



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

void move_cylinder(int desired_position)
{
	int real_position;
	//while (real_position != desired_position)
	//{
		real_position = valve_sensor;
		if (real_position > desired_position)
		{
			CLEAR_BIT(PORTA, 0x20);
		}
		else
		{
			SET_BIT(PORTA, 0x20);
		}
		INTR_ON();
	//}
	
	INTR_OFF();	
	CLEAR_BIT(PORTA, 0x40);		//Set to go low when not interrupting
}

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	
	//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 	//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