//FILENAME:	Open_Loop_Velocity_Cntrl.c
//AUTHOR:	Nate Bassett
//DESCRIPTION:	PreLab #4 --> Deadband Compensation, Fall 2005

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

#define c_kinetic_pos	0x08	//Static Friction
#define c_kinetic_neg	0x08	//Make the value positive
#define c_static_pos	0x0a	//Kinetic Friction
#define c_static_neg	0x0a	//Make the value positive
#define c_max		255	//Limit count
#define c_min		255	//Make the value positive

int moving = 0;		//Assume it starts without moving

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);

/* Call this routine when updating */
int deadband(int c_wanted)
{
	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 value 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;
}

//Call this routine once the program starts
void PWM_init()
{
	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
}

//To update the PWM output
void PWM_update(int value)
{
	if(value > 255)
		value = 255;
	if(value < 0)
		value = 0;
	outint(value); outln(" = PWM");
	OCR1A = value;	//duty cycle
}

//Call the interrupt loop
int v_output(int v_adjusted)
{
	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;
}

int count = 0;		//A count value to be output on Port B

void IO_setup()
{
	PWM_init();
}

int db_correct = 0;	//Deadband correction is off by default

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

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

/* ----------------------MAIN-----------------------------*/
int main(void)
{
	int c;
	sio_init();
	IO_setup();
	outln("");
	outln("--Deadband Compensation Program");

	for(;;)
	{
		while((c=input()) == -1) {}	//Wait for keypress

		if(c == '+')			//Increment the output on Port B
		{
			if(++count > 255) count = 255;
			if(count ==1)		//It has just started moving
			{
				moving = 0;
				IO_update();
				delay(200);
				moving = 1;
			}
			IO_update();
			outint(count); outln(" = count, INCREMENTED");
		}
		else if(c == '-')
		{
			if(--count < -255) count = -255;
			if(count == -1)		//It has just started moving
			{
				moving = 0;
				IO_update();
				delay(200);
				moving = 1;
			}
			IO_update();
			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");
			}
			IO_update();
		}
		else if(c =='s')
		{
			count = 0;
			moving = 0;
			IO_update();
			outint(count); outln(" = count, MOTOR STOPPED");
		}
		else if(c =='h')
		{
			outln("   + : Increment Value");
			outln("   - : Decrement Value");
			outln("   d : Enable or Disable (default) Deadband Comp.");
			outln("   s : Stop the Motor");
			outln("   q : Quit");
		}
		else if (c == 'q')
		{
			count = 0;
			moving = 0;
			IO_update();
			outint(count); outln(" = count, QUITTING!!");
			break;
		}
	}
	sio_cleanup();

	return 1;
}