#include <hc11e9.h>
#include <pwm.h>
#include <buffalo.h>

/*Definition of Variables for Setpoint Tables and Motion Control*/
#define TABLE_SIZE 14
int point_master[TABLE_SIZE] = {0, 20,50, 90,150, 206,275, 345, 500, 655, 794, 905, 990, 1000};
int point_Position[TABLE_SIZE];
int point_time[TABLE_SIZE];
int point_start_time;
int point_index;
int ticks;         /*Variable to keep a system clock count*/
int point_current; /*A global variable used to track Position*/

/*Definition of 'Deadband Limits' that were determined experimentally*/
#define c_stick_pos_dynamic 255
#define c_stick_neg_dynamic 255
#define c_stick_pos_static 255
#define c_stick_neg_static 255

/*Initial Potentiometer Angle (to be determined upon startup of program)*/
int theta_start;

/*Definition of Motion and Sway Controller Gains*/
int Kpp = 10;   /*Initial Proportional Controller Gain*/
int Kpi = 20;   /*Initial Integral Controller Gain*/
int Ksp = 0;   	/*Proportional Sway Control Gain*/
int Ksi = 0;	/*Integral Sway Control Gain*/

/*Error sum terms for controller*/
int e_sum;	/*Error sum for encoder integral gain*/
int sway_sum;   /*Error sum for sway integral gain*/


signed int c_desired;		/*Desired commanded cart position*/
signed int Position = 0;	/*Initial Encoder Position*/
int duration;			/*Variable for changing the speed at which the setpoint*/
				/*table is generated.  Also dictates the speed of the cart*/

signed int theta_L;		/*A/D conversion value for potentiometer angle.*/
				/*Read every 4 ms and used in error calculations.*/

/*Definition of System Parameters*/
signed int Cpe;  /*The Cart Position Error*/
signed int Cp;   /*Position Count from Encoder*/
signed int Cpc;  /*Output Command for Position Control*/
signed int Cc;   /*Combined motor control output*/
signed int Clc;  /*Output Command for sway control*/

/*Limits for Motor Position Control*/
/*Voltages will be clipped at 5V*/
#define c_min 255
#define c_max 255

/*Definition of system time step (4ms, divided by 1000 later)*/
#define T 4

/*Definition of System Functions*/
void v_output(int v_adjusted);
int deadband(int Cd, int Position);
int controller(int Cd, int Cf);
void EncoderUpdate(void);
int integrate(int e);
int initialize();
int init_table();
void update_table();
void get_setpoint();
int sway_control(int theta_L);
int sway_integral(int e);

/*Beginning of Main Program*/
int main()  {

	int mode = 0;  		/*Use Polling Loop Update*/
	char key;      		/*User input*/
	int new_pos = 0;	/*How far forward or backward the motor will move*/

	duration = 1;		/*How fast the motor will move to Position*/

	initialize();		/*Initialize the real time interrupt and registers*/

	putstr("Enter a distance to move\n");
	putstr("Press 0 - 9 to move the motor up to 20 inches in 2 inch increments\n");
	putstr("Press 'G' to go once a distance has been selected\n")
	putstr("Press 'q' to quit\n");

	do  {

		key = input();
		switch(key)  {

			case 0:
				break;  /*No input*/
			case '1':
				new_pos = 10;   /*Move 2 inches*/
				break;
			case '2':
				new_pos = 20;	/*Move 4 inches...etc*/
				break;
			case '3':
				new_pos = 30;
				break;
			case '4':
				new_pos = 40;
				break;
			case '5':
				new_pos = 50;
				break;
			case '6':
				new_pos = 60;
				break;
			case '7':
				new_pos = 70;
				break;
			case '8':
				new_pos = 80;
				break;
			case '9':
				new_pos = 90;
				break;
			case '0':
				new_pos = 100;  
				break;
			case 'q':
			case 'Q':
				mode = 1;
				break;
				
			/*Generate the motion table after a distance has been selected*/ 
			/*and move the cart*/
			case 'g':
			case 'G':
				update_table(point_current, point_current + new_pos, duration);
				break;
			}/*End of Switch*/
	}while(mode == 0);  	/*Continue to poll until the user selects quit*/

	PWMDisable();  		/*Disable PWM generator*/

	return 0;  		/*Return value of main is int when dealing with the 6811*/

}/*end main()*/

/*Real Time Interrupt Service Routine*/
extern void __attribute__((interrupt)) RTI_ISR(void)  {

	signed int c_wanted;
	signed int c_adjusted;

	TFLG2 = RTIF;  /*Clear the interrupt flag*/
	__asm("cli");  /*Clear all other interrupts*/

	/*Update RefSignal, motion table, and the encoder to the latest user inpue*/

	EncoderUpdate();
	get_setpoint();
	theta_L = ADR2;  /*Get Current integer angle from the A/D converter*/
	

	c_wanted = controller(point_current, Position);
	Clc = sway_control(theta_L);
	Cc = c_wanted + Clc;	/*Positive Feedback for Sway Control*/
	c_adjusted = deadband(Cc, Position);
	v_output(c_adjusted);

}/*end RTI_ISR()*/

/*Function to Handle Sway Control Once it is turned on*/
int sway_control(int theta_L)  {

	int theta_adjusted;
	int theta_error;

	theta_error = theta_L - theta_start;

	theta_adjusted = Ksp * theta_error + Ksi*sway_integral(theta_error);

	return theta_adjusted;

}/*end sway_control()*/

/*Initialization Routine.  Called upon Startup*/
int initialize()  {

	theta_start = ADR2;	/*Set the initial Potentiometer Angle Integer Value*/

	/*Enable the real time interrupt*/
	*(unsigned *)0x00EC = (unsigned)RTI_ISR;
	TMSK2 |= RTII;  /*Enable the Real Time Interrupt*/
	__asm("cli");	/*Clear all interrupts*/

	DDRD &= ~(PD4 | PD5);   /*Set pins 4 and 5 as inputs for the encoder*/
	DDRD |= (PD2 | PD3);	/*Set pins 2 and 3 as outputs for CW and CCW motion*/

	PWMInit();  	/*Initialize PWM Generator*/
	PWMEnable();	/*Enable the PWM Generator*/

	/*Set up analog inputs and set for continuos scanning*/
	OPTION |= ADPU;
	ADCTL = SCAN | CA;
	
	/*Initialize the motion contol table*/
	init_table();	

	return 0;
	
}/*end initialize()*/

/*Interrupt driven encoder update*/
void EncoderUpdate(void){ 	
			
	/*Static variables for Position calculation*/

	static unsigned char state = 0xFF;
	unsigned char new_state;
	
	new_state = (PORTD & (PD5 | PD4)) >> 4; /* Read encoder state */
	
	/* Update Position value */
	if (state!=new_state) {
		switch (state) {
			case 0x00:
				if (new_state==0x01)
					Position++;
				else
					Position--;
				break;
	
			case 0x01:
				if (new_state==0x03)
					Position++;
				else
					Position--;
				break;
	
			case 0x03:
				if (new_state==0x02)
					Position++;
				else
					Position--;
				
				break;
			case 0x02:
				if (new_state==0x00)
					Position++;
				else
					Position--;
				break;
				
		}/*end switch*/

		state = new_state;
		
	}/*end if*/
	
}/*end EncoderUpdate()*/

/*Integral Sway Controller*/
int sway_integral(int e)  {

	sway_sum += e * T;
	return sway_sum;
	
}/*end sway_integrate*/

/*Integral Motion Control*/
int integrate(int e){
	
	e_sum += e * T;
	return e_sum;
	
}/* end integrate() */

/*Encoder Driven Motion Feedback Controller*/
int controller(int Cd, int Cf){
	
	int Ce = 0;
	int Cw = 0;
	
	Ce = Cd - Cf;	
	
	Cw = Kpp * Ce + (long int)Kpi * integrate(Ce) / 1000 + Ce*Kenc;

	return Cw;

}/* end controller() */

/*Deadband compensation for which values were defined and determined experimentally*/
int deadband(int Cd, int Position) {

	int c_adjusted;
	unsigned c_stick_pos = 0;
	unsigned c_stick_neg = 0;

	/*Motor is not in motion, deadband is static*/
	if ((ADR2 - Position) == 0) {
		c_stick_pos = c_stick_pos_static;
		c_stick_neg = c_stick_neg_static;
	} 
	/*Motor is in motion, compensate for torque and friction*/
	else {
		c_stick_pos = c_stick_pos_dynamic;
		c_stick_neg = c_stick_neg_dynamic;
	}
	
	if(Cd == 0) { /* turn off the output */
		
		c_adjusted = 0;
		
	} /* end if */
	
	else if(Cd > 0) { /* a positive output */
		
		c_adjusted = c_stick_pos + (unsigned)(Cd)*(c_max - c_stick_pos)/ c_max;
		if(c_adjusted > c_max) c_adjusted = c_max;

	}/* end else if */ 
	
	else { /* the output must be negative */
		
		c_adjusted = -c_stick_neg - (unsigned)(-Cd)*(c_min - c_stick_neg)/ c_min;
		if(c_adjusted < -c_min) c_adjusted = -c_min;
		
	}/* end else */

	return c_adjusted;

}/*end deadband()*/

/*Call from the interrupt loop in order to output voltage to drive cart*/
void v_output(int v_adjusted){ 

	if(v_adjusted >= 0){  /* set the direction bits to CW on, CCW off */
		
		PORTD = 0x04; /* bit 2 on, all others off */
		
		if(v_adjusted > 255){ /* clip output over maximum */
			RefSignal = 255;
		}/* end if */ 
		
		else {
			RefSignal = v_adjusted;
		} /* end else */
		
	} /* end if */ 
	
	else {  
		/* need to reverse output sign */	
		/* set the direction bits to CW off, CCW on */
		
		PORTD = 0x08;               /* bit 3 on, all others off */
		if(v_adjusted < -255){      /* clip output below minimum */
			RefSignal = 255;
		}/* end if */ 
		
		else {
			RefSignal = -v_adjusted; /* flip sign */
			
		} /* end else */
		
	} /* end else */
	
} /* end v_output() */
	
int init_table(){ /* initialize the setpoint table */


	ticks = 0; /* set the clock to zero */

	point_current = 0; /* start the system at zero */

	point_index = TABLE_SIZE; /* mark the table as empty */

	return 0;
}/* end init_table() */


void update_table(int start, int end, int duration_sec){


	unsigned i;

	point_time[0] = ticks + 10; /* delay the start slightly */
	point_Position[0] = start;

	for(i = 1; i < TABLE_SIZE; i++){
		point_time[i] = point_time[0] +
			(unsigned long)i * duration_sec * 250 / (TABLE_SIZE - 1);
		point_Position[i] = start + (long int)(end - start) * point_master[i] / 1000;

		if(i==12) {
			
			Kpi = 0;  	/*Turn integral position gain off to avoid overshoot*/
			Kpp = 1;	/*Dial down proportional gain to assist in smooth decel*/
			/*Turn on Sway Control Gains*/
			Ksp = 1;	
			Ksi = 1;
		}
	}
	point_index = 0;

}/* end update_table() */

/*Interrupt Driven Encoder Update*/
void get_setpoint(void)  {  

	ticks++; /* update the clock */

	if(point_index < TABLE_SIZE){
		if(point_time[point_index] == ticks){
			point_current = point_Position[point_index++];
			outint16(point_current);
			putch('\n');
		}/* end if */
	}/* end if */

	
}/*end get_setpoint()*/