/* (C) Eitan Sherer & Kenji Hashimoto
   ECE476 Final Project: Inverted Pendulum Balancer

   TEST CODE: Motor Driver
 */

#include <Mega32.h>


/*
 * Global Variable Declarations:
 */
unsigned int motor_PWM_duty;
unsigned int motor_PWM_period;
char motor_break_bit;
unsigned char motor_timer;

// TEST Variables:
char upstate,downstate;
char upPeriod,downPeriod;
char breakState,dirState;
char test_timer;


/*
 * Function Declarations:
 */
void initialize();
void motor_speedUP();
void motor_speedDOWN();
void motor_updateDuty();
void motor_updatePeriod();
void motor_break(char);
void motor_direction(char);
void motor_controller(char);

// TEST FUNCTIONS:
void test_speed();
void test_initialize();
void test_upPeriod();
void test_downPeriod();
void test_cmd();



/*
 * FLASH:
 */

// Speed Level
flash char motor_table_speed[8] = {
	0, 25, 33, 42, 50, 58, 67, 75
};

// Timer Length
flash char motor_table_time[16] = {
	0,	// Time=0: 0 sec
	6,	// Time=1: 0.24 sec
	30,	// Time=2: 0.49 sec
	46,	// Time=3: 0.75 sec
	61,	// Time=4: 1 sec
	76,	// Time=5: 1.25 sec
	92,	// Time=6: 1.51 sec
	107,	// Time=7: 1.75 sec
	122,	// Time=8: 2 sec
	137,	// Time=9: 2.24 sec
	153,	// Time=10: 2.5 sec
	168,	// Time=11: 2.75 sec
	183,	// Time=12: 3 sec
	203,	// Time=13: 3.3 sec
	223,	// Time=14: 3.65 sec
	244	// Time=15: 4 sec
};

//
flash char motor_table_cmd[3] = {
	0b10001001, // Dir = Back, Time = 3, Speed = 5
	0b00001000, // Dir = Back, Time = 2, Speed = 2
	0b00001001  // Dir = Back, Time = 1, Speed = 1
};


interrupt [TIM0_OVF] void timer0_ovfl(void) {
	if (test_timer > 0) test_timer--;
	if (motor_timer > 0) { motor_timer--; }
}



/*
 * TEST FUNCTION: test_intialize()
 */
void test_initialize() {
	upstate = 0;
	downstate = 0;
	upPeriod = 0;
	downPeriod = 0;
	breakState = 0;
	dirState = 0;


	DDRA = 0;
	DDRB = 0xff;
	PORTB = 0xff;
	TCCR0 = 0b00000101;
}

/*
 * TEST FUNCTION: test_cmd()
 */
void test_cmd() {
	char i;

	switch (upstate) {
		case 0:
			if (~PINA == 0x01) upstate = 1;
			else upstate = 0;
			break;
		case 1:
			if (~PINA == 0x01) {
				for (i=0; i<5; i++) {
					motor_controller(0);
					motor_controller(1);
				}
				upstate = 2;
			}
			else upstate = 0;
			break;
		case 2:
			if (~PINA == 0x01) upstate = 2;
			else upstate = 3;
			break;
		case 3:
			if (~PINA == 0x01) upstate = 2;
			else upstate = 0;
			break;
	}
}

/*
 * TEST FUNCTION: test_speed()
 */
void test_speed() {
	char i;

		switch (upstate) {
			case 0:
				if (~PINA == 0x01) upstate = 1;
				else upstate = 0;
				break;
			case 1:
				if (~PINA == 0x01) {
					for (i=0; i<10; i++)
						motor_speedUP();
					upstate = 2;
				}
				else upstate = 0;
				break;
			case 2:
				if (~PINA == 0x01) upstate = 2;
				else upstate = 3;
				break;
			case 3:
				if (~PINA == 0x01) upstate = 2;
				else upstate = 0;
				break;
		}

		switch (downstate) {
			case 0:
				if (~PINA == 0x02) downstate = 1;
				else downstate = 0;
				break;
			case 1:
				if (~PINA == 0x02) {
					for (i=0; i<10; i++)
						motor_speedDOWN();
					downstate = 2;
				}
				else downstate = 0;
				break;
			case 2:
				if (~PINA == 0x02) downstate = 2;
				else downstate = 3;
				break;
			case 3:
				if (~PINA == 0x02) downstate = 2;
				else downstate = 0;
				break;
		}

		switch (upPeriod) {
			case 0:
				if (~PINA == 0x04) upPeriod = 1;
				else upPeriod = 0;
				break;
			case 1:
				if (~PINA == 0x04) {
					for (i=0; i<10; i++)
						test_upPeriod();
					upPeriod = 2;
				}
				else upPeriod = 0;
				break;
			case 2:
				if (~PINA == 0x04) upPeriod = 2;
				else upPeriod = 3;
				break;
			case 3:
				if (~PINA == 0x04) upPeriod = 2;
				else upPeriod = 0;
				break;
		}

		switch (downPeriod) {
			case 0:
				if (~PINA == 0x08) downPeriod = 1;
				else downPeriod = 0;
				break;
			case 1:
				if (~PINA == 0x08) {
					for (i=0; i<10; i++)
						test_downPeriod();
					downPeriod = 2;
				}
				else downPeriod = 0;
				break;
			case 2:
				if (~PINA == 0x08) downPeriod = 2;
				else downPeriod = 3;
				break;
			case 3:
				if (~PINA == 0x08) downPeriod = 2;
				else downPeriod = 0;
				break;
		}

		switch (breakState) {
			case 0:
				if (~PINA == 0x10) breakState = 1;
				else breakState = 0;
				break;
			case 1:
				if (~PINA == 0x10) {
					// break logic
					if (motor_break_bit == 0) motor_break_bit = 1;
					else motor_break_bit = 0;
					motor_break(motor_break_bit);
					breakState = 2;
				}
				else breakState = 0;
				break;
			case 2:
				if (~PINA == 0x10) breakState = 2;
				else breakState = 3;
				break;
			case 3:
				if (~PINA == 0x10) breakState = 2;
				else breakState = 0;
				break;
		}

		switch (dirState) {
			case 0:
				if (~PINA == 0x20) dirState = 1;
				else dirState = 0;
				break;
			case 1:
				if (~PINA == 0x20) {
					// direction logic
					motor_direction(2);
					dirState = 2;
				}
				else dirState = 0;
				break;
			case 2:
				if (~PINA == 0x20) dirState = 2;
				else dirState = 3;
				break;
			case 3:
				if (~PINA == 0x20) dirState = 2;
				else dirState = 0;
				break;
		}
}

/*
 * TEST FUNCTION: test_upPeriod()
 */
void test_upPeriod() {
	if (motor_PWM_period < 0xffff) {
		motor_PWM_period++;
	}

	motor_updatePeriod();
}

/*
 * TEST FUNCTION: test_downPeriod()
 */
void test_downPeriod() {
	if (motor_PWM_period > motor_PWM_duty) {
		motor_PWM_period--;
	}

	motor_updatePeriod();
}














/*
 * FUNCTION: main()
 */
void main(void) {
	test_initialize();
	initialize();

	while(1) {
		if (test_timer == 0) {
			test_timer = 0;
			test_cmd();
		}
	}
}

/*
 * MOTOR FUNCTION: motor_controller(char param)
 */
void motor_controller(char param) {
	char cmd;
	cmd = motor_table_cmd[param];

// decode command and set parameters
	// SPEED
	motor_PWM_duty = (int)(motor_table_speed[(cmd & 0b00000111)]*motor_PWM_period/100);
	motor_updateDuty();

	// TIME
	motor_timer = motor_table_time[((cmd >> 3) & 0b00001111)];

	// DIRECTION
	motor_direction((cmd >> 7) & 0b00000001);

// break OFF
	if (motor_PWM_duty != 0) motor_break(0);

// Wait until timer runs out
	PORTB = 0x00;
	while (motor_timer > 0) {}
	PORTB = 0xff;

// break ON
	motor_break(1);
}

/*
 * MOTOR FUNCTION: motor_break(char param)
 *		param:	0 - break OFF
 *				1 - break ON
 */
void motor_break(char param) {
	switch (param) {
		case 0:		// break OFF
			PORTD = PORTD & 0b01111111;
			break;
		case 1:		// break ON
			PORTD = PORTD | 0b10000000;
			break;
		default:
			break;
	}
}

/*
 * MOTOR FUNCTION: motor_direction(char param)
 *		param:	0 - FORWARD
 *				1 - BACK
 *				2 - toggle
 */
void motor_direction(char param) {
	motor_break(1);

	switch (param) {
		case 0:		// FORWARD
			PORTD = PORTD & 0b10111111;
			break;
		case 1:		// BACK
			PORTD = PORTD | 0b01000000;
			break;
		case 2: 	// toggle
			PORTD = PORTD ^ 0b01000000;
			break;
		default:
			break;
	}

	motor_break(motor_break_bit);
}

/*
 * MOTOR FUNCTION: motor_speedUP()
 */
void motor_speedUP() {
	if (motor_PWM_duty < motor_PWM_period) {
		motor_PWM_duty++;
	}
	motor_updateDuty();
}

/*
 * MOTOR FUNCTION: motor_speedDOWN()
 */
void motor_speedDOWN() {
	if (motor_PWM_duty > 0) {
		motor_PWM_duty--;
	}
	motor_updateDuty();
}

/*
 * MOTOR FUNCTION: motor_updateDuty()
 */
void motor_updateDuty() {
	char lowByte;
	char highByte;

	lowByte = (char)(motor_PWM_duty);
	highByte = (char)(motor_PWM_duty >> 8);

	OCR1AH = highByte;
	OCR1AL = lowByte;
}

/*
 * MOTOR FUNCTION: motor_updatePeriod()
 */
void motor_updatePeriod() {
	char lowByte;
	char highByte;

	lowByte = (char)(motor_PWM_period);
	highByte = (char)(motor_PWM_period >> 8);

	ICR1H = highByte;
	ICR1L = lowByte;
}

/*
 * FUNCTION: initialize()
 */
void initialize() {

// Ports
	//D.5 OC1A output for motor's PWM signal
	//D.6 output for motor's direction signal
	//D.7 output for motor's break signal

	DDRD = 0b11100000;

// Timer 1

	/* TCCR1A:
		[7,6] Compare Output Mode for Channel A = 10 (clear on Comp Match, set at TOP)
		[5,4] Compare Output Mode for Channel B = 00 (disconnected)
		[3,2] Force Output Compare for Channel A/B = 00
		[1,0] Waveform Generation Mode bits 11 & 10 = 10 (Fast PWM w/ TOP=ICR1A)
	 */
	TCCR1A = 0b10000010;

	/* TCCR1B:
		[7] Input Capture Noise Canceler = 0
		[6] Input Capture Edge Select = 0
		[5] Reserved Bit = 0
		[4,3] Waveform Generation Mode bits 13 & 12 = 11 (Fast PWM w/TOP=ICR1A)
		[2,1,0] Clock Select = 100 (clk/256)
	 */
	TCCR1B = 0b00011100;

	/* TIMSK:
		[7] Timer/Counter2, Output Compare Match Interrupt Enable = 0
		[6] Timer/Counter2, Overflow Interrupt Enable = 0
		[5] Timer/Counter1, Input Capture Interrupt Enable = 0
		[4] Timer/Counter1, Output Compare A Match Interrupt Enable = 0
		[3] Timer/Counter1, Output Compare B Match Interrupt Enable = 0
		[2] Timer/Counter1, Overflow Interrupt Enable = 0
		[1] Timer/Counter0, Output Compare Match Interrupt Enable = 0
		[0] Timer/Counter0, Overflow Interrupt Enable = 1
	 */
	TIMSK = 0b00000001;

// Global Variables
	motor_PWM_period = 300;
	motor_PWM_duty = 150;
	motor_break_bit = 1;
	motor_timer = 0;
	motor_updatePeriod();
	motor_updateDuty();
	motor_break(motor_break_bit);


// Enable Interrupts
	#asm ("sei");
}