/*
FILENAME:	345-Lab_6.c
AUTHOR:		Kevin LaBeau, Clayton Campagna, Sarah Rizzio
DESCPRITION:	EGR 345, Fall 2004
	This program will implement a control loop with encoder
       	feedback for position. It will use a proportional and 
	integral gain for the feedback.
*/

#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include "sio.c"

#define CLK_ms 10	/* Set the updates for every 10ms */
#define T 10 		/* Define as 4 ms, must divide by 1000 later*/
#define Kp 0		/* Controller proportional gain constant */
#define Ki 1		/* Controller integral gain constant */
#define c_kinetic_pos 0x44 	/* static friction */
#define c_kinetic_neg 0x44 	/* make the value positive */
#define c_static_pos 0x65 	/* kinetic friction */
#define c_static_neg 0x62 	/* make the value positive */
#define c_max 255
#define c_min 255 		

int count = 0; 			/* A count value to be output on port B */
int e_sum = 0;
int moving= 0;			/* Assume it starts without moving */
int Position = 0; 		/* Assume it starts at position 0 */
int db_correct = 1; 		/* Deadband correction is on by default */
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 */

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 init();
void EncoderUpdate(void);
int integrate(int e);
int controller(int Cd, int Cf);
SIGNAL(SIG_OVERFLOW0);
void CLK_setup();

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 IO_setup(){
	PWM_init();
}

void IO_update(){	/* This routine will run once per interrupt for updates */
	EncoderUpdate();
	if(db_correct == 0){
		PWM_update(v_output(controller(count,Position)));
	} else {
		PWM_update(v_output(deadband(controller(count,Position))));
	}
}

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 init(){ 		/* Initialize the register once */
	DDRB = 0x00; 	/* Set port B as inputs */

	return 0;
}

void EncoderUpdate(void){ 	/* Interrupt driven encoder update */
	/* Static variables for position calculation */
	static unsigned char state = 0xFF;
	unsigned char new_state;

	new_state = (PINB & (_BV(PB2) | _BV(PB3)))>>2; 	/* 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;
			}
		state = new_state;
	}
}

int integrate(int e){
	e_sum += e * T;
	return e_sum;
}

int controller(int Cd, int Cf){
	int Ce;
	int Cw;
	
	Ce = Cd - Cf;
	Cw = Kp * Ce + Ki * integrate(Ce) / 1000;
	

	return Cw;
}

SIGNAL(SIG_OVERFLOW0){ 			/* The interrupt calls this function */
	CLK_ticks += CLK_ms;
	IO_update();
	if(CLK_ticks >= 1000){ 		/* The number of interrupts between output changes */
		CLK_ticks = CLK_ticks - 1000;
		CLK_seconds++;
	}
	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);
	TIFR &= ~(0<<OCF0);
		/* TIFR = (0<<OCF2) | (0<<TOV2) | (0<<ICF1) | (0<<OCF1A) | (0<<OCF1B)
	 	| (0<<TOV1) | (0<<OCF0) | (1<<TOV0); // set to use overflow interrupts */
	TIMSK |= (1<<TOIE0);
	TIMSK &= ~(0<<OCIE0);
		/* TIMSK = (0<<OCIE2) | (0<<TOIE2) | (0<<TICIE1) | (0<<OCIE1A) | (0<<OCIE1B)
	 	| (0<< TOIE1 ) | (0<<OCIE0) | (1<<TOIE0); // enable TCNT1 overflow */
	sei(); 	/* Enable interrupts flag */
}

int main(){
	int c;
	sio_init();
	IO_setup();
	init();
	CLK_setup();

	for(;;){
		while((c = input()) == -1){} /* Wait for a keypress */
		if(c == '+'){ /* Increment the output on port B */
			count += 10;
			if(count > 255) count = 255;
				moving = 0;
				delay(200);
				moving = 1;
			outint(count); outln(" = count, incremented");
		} else if(c == '-'){
			count -= 10;
			if(count < -255) count = -255;
				moving = 0;
				delay(200);
				moving = 1;
			outint(count); outln(" = count, decremented");
		} else if(c == 'd'){
			if(db_correct == 0){
				db_correct = 1;
				outln("deadband correction ON");
			} else {
				db_correct = 0;
				outln("deadband correction OFF");
			}
		} else if(c == 's'){
			count = 0;
				moving = 0;
				delay(200);
				moving = 1;
			outint(count); outln(" = count, motor stopped");
		} else if(c == 'p'){
			outint(Position); outln(" = position");
		} else if(c == 'h'){
			outln(" +: increment value");
			outln(" -: decrement value");
			outln(" d: enable or disable (default) deadband comp.");
			outln(" s: stop the motor");
			outln(" p: position");
			outln(" q: quit");
		} else if(c == 'q'){
			count = 0;
			moving = 0;
			outint(count); outln(" = count, Quitting!");
			break;
		}
	}
	sio_cleanup();

	return 1;
}