//	lab_8_prelab.c
//	EGR 345
//	System Modeling and Simulation
//	Andrew Bronson
//	10/24/06

#include <avr/io.h>		/* ATMEGA32 I/O register defines */
#include <avr/signal.h>
#include <avr/interrupt.h>
#include "sio.c"
		
#define c_kinetic_pos 0x90	/* kinetic friction */
#define c_kinetic_neg 0x84 	/* make the value positive */
#define c_static_pos 0xa3	/* static friction */
#define c_static_neg 0x97 	/* make the value positive */
#define c_max 255
#define c_min 255 		/* make the value positive */
#define T 10			/* define as 10ms, must divide by 1000 later */
#define Kp 10
#define Ki 0
#define TABLE_SIZE 200

int moving = 0; 		/* assume it starts without moving */
int e_sum = 0;
int desired = 1;
unsigned pot_position[TABLE_SIZE];
int array_position;
int count = 0; 		// a count value to be output on port B
int db_correct = 1; 	/* deadband correction is off by default */
int Cf;
int Cf_old;
int stop_count = 1;

int deadband(int c_wanted);
void PWM_init();
void PWM_update(int value);
int v_output(int v_adjusted);
void IO_setup();
void IO_update();
void delay(int ticks);
int integrate(int e);
int controller(int Cd, int Cf);
SIGNAL(SIG_OVERFLOW0);
void CLK_setup();

void AD_setup(void)
{
	/* Setup A/D to read tach input from channel 0 */
	DDRD = 0x00;
	ADMUX = 0x40; 		/* Set the input to channel 0 */
	ADCSRA = 0xE0;		/* Turn on ADC and set to free running */
}

int AD_read()
{
	return (ADCW >> 2);
}

int deadband(int c_wanted){/* call this routine when updating */
	int c_pos;
	int c_neg;
	int c_adjusted;

	if(moving == 1){
		c_pos = c_kinetic_pos;
		c_neg = c_kinetic_neg;
	} else {
		c_pos = c_static_pos;
		c_neg = c_static_neg;
	}
	if(c_wanted == 0){ /* turn off the output */
		c_adjusted = 0;
	} else if(c_wanted > 0){ /* a positive output */
		c_adjusted = c_pos + (unsigned)(c_max - c_pos) * c_wanted / c_max;
		if(c_adjusted > c_max) c_adjusted = c_max;
	} else { /* the output must be negative */
		c_adjusted = -c_neg - (unsigned)(c_min - c_neg) * -c_wanted / c_min;
		if(c_adjusted < -c_min) c_adjusted = -c_min;
	}
	
	return c_adjusted;
}

void PWM_init(){ /* call this routine once when the program starts */
	DDRD |= (1 << PD5); /* set PWM outputs */
	DDRC |= (1 << PC0) | (1 << PC1);
		/* set motor direction outputs on port C*/
	//using OCR1
	TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
		// turn on both PWM outputs on counter 1
	TCCR1B = _BV(CS11) ; // set the internal clock to /8
}

void PWM_update(int value){ /* to update the PWM output */
	if(value > 255) value = 255;
	if(value < 0) value = 0;
	OCR1A = value; // duty cycle
}

int v_output(int v_adjusted){ /* call from the interrupt loop */
	int RefSignal; // the value to be returned
	if(v_adjusted >= 0){
		/* set the direction bits to CW on, CCW off */
		PORTC = (PINC & 0xFC) | 0x02; /* bit 1 on, 0 off */
		if(v_adjusted > 255){ /* clip output over maximum */
			RefSignal = 255;
		} else {
			RefSignal = v_adjusted;
		}
	} else { /* need to reverse output sign */
		/* set the direction bits to CW off, CCW on */
		PORTC = (PINC & 0xFC) | 0x01; /* bit 0 on, 1 off */
		if(v_adjusted < -255){ /* clip output below minimum */
			RefSignal = 255;
		} else {
			RefSignal = -v_adjusted; /* flip sign */
		}
	}
	return RefSignal;
}

void delay(int ticks){ // ticks are approximately 1ms
	volatile int i, j;
	for(i = 0; i < ticks; i++){
		for(j = 0; j < 1000; j++){}
	}
}

int integrate(int e)
{
	e_sum += e*T;
	if (e_sum > 10000)
		e_sum = 10000;		// set an upper limit
	if (e_sum < -10000)
		e_sum = -10000;		// set a lower limit
	return e_sum;
}

int controller(int Cd, int Cf)
{
	int Ce;
	int Cw;

	Ce = Cd - Cf;
	Cw = Kp * Ce;

	return Cw;
}

void IO_update()
{
	/* This routine will run once per interrupt for updates */

	Cf_old = Cf;
	Cf = AD_read();
	

	if(db_correct == 0){
		PWM_update(v_output(controller(count,Cf)));
	} else {
		PWM_update(v_output(deadband(controller(count,Cf))));
	}

	if(Cf == Cf_old)
	{
		moving = 0;
	}
	else 
	{
		moving = 1;
	}

	if(stop_count == 0)
	{
		pot_position[array_position] = Cf;
		array_position++;
		if(array_position == 100)
		{
			stop_count = 1;
		}
	}
	
}

#define CLK_ms 10 // set the updates for every 10 ms
unsigned int CNT_timer1; // the delay time
volatile unsigned int CLK_ticks = 0; // the current number of ms
volatile unsigned int CLK_seconds = 0; // the current number of seconds

SIGNAL(SIG_OVERFLOW0)	// The interrupt calls this function
{	
	CLK_ticks += CLK_ms;
	if(CLK_ticks >= 10){ // The number of interrupts between output changes
		CLK_ticks = CLK_ticks - 10;
		CLK_seconds++;
		IO_update();
	}
	

	TCNT0 = CNT_timer1;
}

void CLK_setup() // Start the interrupt service routine
{
	TCCR0 = (0<<FOC0) | (0<<WGM01) | (0<<WGM00) | (0<<COM00) | (0<<COM01) | (1<<CS02) | (0<<CS01) | (1<<CS00);
	// Use CLK/1024 prescale value 
	// disable PWM and compare output modes
	CNT_timer1 = 0xFFFF - CLK_ms * 8; // 8 = 1ms, 80 = 10ms
	TCNT0 = CNT_timer1; // start at the right point
	
	TIFR |= (1<<TOV0); // set to use overflow interrupts
	TIFR &= ~(0<<OCF0);// enable TCNT0 overflow
	TIMSK |= (1<<TOIE0);
	TIMSK &= ~(0<<OCIE0);
	
	sei(); // enable interrupts flag
}

void IO_setup(){
	AD_setup();
	sio_init();		//call sio_init() function
	PWM_init();		//call PWM_init() function
	CLK_setup();		//call CLK_setup() function
	
}

int main()
{
	int c, i;
	int desired_speed = 0;	/* desired motor speed (for table output) */

	IO_setup();

	outln("Tachometer Speed Data Acquistion");
	for(;;){
		while((c = input()) == -1){} 		/* Wait for a keypress */
		if(c == '0'){ 				/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 0;			/* Set PWM value */
			desired_speed = count; 		/* Store desired pwm value */
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		} else if(c == '1'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 0;
			desired_speed = count;
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		}else if(c == '2'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 32;
			desired_speed = count;
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		}else if(c == '3'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 64;
			desired_speed = count;
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		}else if(c == '4'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 96;
			desired_speed = count;
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		}else if(c == '5'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 128;			/* Set PWM */
			desired_speed = count;
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		}else if(c == '6'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 160;
			desired_speed = count;
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		}else if(c == '7'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 192;
			desired_speed = count;
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		}else if(c == '8'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 224;
			desired_speed = count;
			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		}else if(c == '9'){ 			/* Increment the output on port B */
			array_position = 0; 		/* Initialize the array position variable so
							   interrupts start taking data */
			count = 240;
			desired_speed = count;

			stop_count = 0;

			outint(count); outln(" = is the PWM.");
		} else if(c == 'c'){
			if(db_correct == 0){
				db_correct = 1;
				outln("deadband correction ON");
			} else {
				db_correct = 0;
				outln("deadband correction OFF");
			}
			IO_update();
		}else if (c == 'd'){
			outln("Desired Motor Speed (Hex)	Potentiometer Position Volts (Hex)\n");
			for(i = 0; i < 100; i++){
				outint(pot_position[i]); outln("");
			}
		}else if(c == 'r'){
			count = 0;
			moving = 0;
			array_position = 1000;		/* Set large array position to endure
							   no data being taken */
			outint(count); outln(" = count, motor stopped");
		}else if(c == 'f'){
			outint(Cf); outln(" = Pot Position");
		} else if(c == 'm'){
			outint(moving); outln(" = Moving");
		} else if(c == 'h'){
			outln(" 1 - 9 PWM value between 0 and 255");
			outln(" d: dumps data in array to screen");
			outln(" c: enable or disable (default) deadband comp.");
			outln(" r: stop the motor");
			outln(" f: display pot position");
			outln(" q: quit");
		} else if(c == 'q'){
			count = 0;
			moving = 0;
			outint(count); outln(" = count, Quitting!");
			break;
		}
	}
	sio_cleanup();

	return 1;
}