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

   BALANCER W/PID CONTROL Code
 */

#include <Mega32.h>

#define init_motor_period 300		// initial PWM signal period
#define init_motor_duty 150		// initial PWM duty-cycle period
#define init_sensor_index 128		// initial sensor index
#define max_motor_duty 95		// MAX motor duty-cycle percentage
#define min_motor_duty 10		// MIN motor duty-cycle percentage
#define safety_timer 200		// safety feature timer
#define safety_speed 90			// safety feature minimum speed
#define safety_angle 115		// saftey angle offset


/* PID Controller Constants */
#define kp 21.2				// Proportional
#define ki 0				// Integral
#define kd 0.001			// Derivative



/*
 * Global Variable Declarations:
 */
unsigned char motor_power;		// motor power bit
unsigned int motor_PWM_duty;		// PWM signal duty-cycle length
unsigned int motor_PWM_period;		// PWM signal period
unsigned char motor_break_bit;		// motor break bit
unsigned char motor_timer;		// motor timer
unsigned char sensor_index;		// sensor index (+ CWR; - CCWR)
unsigned char sensor_channel;		// sensor channel (bit 1=Ch.B; bit 0=Ch.A)
char pid_prev_error;			// previous error for derivative calc
int pid_sum_error;			// sum of errors for intergral calc
unsigned char safety_forward;		// forward direc counter for safety
unsigned char safety_backward;		// backward direc counter for safety


/*
 * Function Declarations:
 */
void initialize();			// initializes ports, timers, interrupts, and global vars
void pid_controller();			// PID controller
void safety_check(char);		// safety check feature
void motor_updateDuty();		// sets motor_PWM_duty value to OCR1AH and OCR1AL
void motor_updatePeriod();		// sets motor_PWM_period value to ICR1H and ICR1L
void motor_break(char);			// controls the motor's break signal
void motor_direction(char);		// controls the motor's direction signal
void motor_controller(int,char);	// center of motor control



/*
 * Interrupt Service Routines:
 */

// Timer 0 Interrupt triggered by Overflow
interrupt [TIM0_OVF] void timer0_ovfl(void) {
	if (motor_timer > 0) { motor_timer--; }
}

// External Interrupt triggered by Encoder's Channel A
interrupt [EXT_INT0] void ext_int0_isr(void) {
	sensor_channel ^= 0x01;
	if (sensor_channel == 1) {
		sensor_index--;
	}
}

// External Interrupt triggered by Encoder's Channel B
interrupt [EXT_INT1] void ext_int1_isr(void) {
	sensor_channel ^= 0x02;
	if (sensor_channel == 2) {
		sensor_index++;
	}
}



/*
 * FUNCTION: main()
 */
void main(void) {
	char yellow_led;
	yellow_led = 0;

	initialize();

	while(1) {
		// Yellow LED to indicates that the reset of MCU
		if (sensor_index > 120 && sensor_index < 136 && yellow_led == 0) {
			PORTA |= 0b00000100; // yellow LED
		}
		else {
			yellow_led = 1;
			PORTA &= 0b11111011; // yellow LED
		}
		pid_controller();
	}
}


/*
 * CONTROLLER FUNCTION: pid_controller()
 */
void pid_controller() {
	char error;

	float prop_term;
	float integ_term;
	float deriv_term;
	float sum_terms;

	prop_term = 0;
	integ_term = 0;
	deriv_term = 0;
	sum_terms = 0;

	int velocity;
	char direction;

	// calculate error
	error = sensor_index - init_sensor_index;

	// Propotional term calc of PID
	prop_term = kp*(float)error;
	sum_terms += prop_term;

	// Integeral term calc of PID
	if (ki != 0) {
		pid_sum_error += error;
		integ_term = ki*(float)pid_sum_error;
		sum_terms += integ_term;
	}

	// Derivative term calc of PID
	if (kd != 0) {
		deriv_term = kd*(float)(error-pid_prev_error);
		pid_prev_error = error;
		sum_terms += deriv_term;
	}

	// Take the absolute value and set direction bit
 	if (sum_terms > 0) { direction = 0; }
	else { direction = 1; sum_terms *= -1; }

	// Update Green LED's
	if (direction == 0) {
		PORTA |= 0b00010000;
		PORTA &= 0b11111110;
	}
	else {
		PORTA |= 0b00000001;
		PORTA &= 0b11101111;
	}

	// check max threshold of the duty-cycle perentage
	if (sum_terms > max_motor_duty) { velocity = max_motor_duty; }
	else { velocity = (int)sum_terms; }

	if(motor_power == 1) motor_controller(velocity,direction);

	safety_check(direction);
}


/*
 * CONTROLLER FUNCTION: safety_check()
 */
void safety_check(char dir) {

	if (motor_PWM_duty > safety_speed) {
		if (dir == 0) {
			safety_forward++;
			safety_backward = 0;
		}
		else if (dir == 1) {
			safety_backward++;
			safety_forward = 0;
		}
	}
	else {
		safety_forward = 0;
		safety_backward = 0;
	}

	if (safety_forward == safety_timer ||
		safety_backward == safety_timer ||
		sensor_index < init_sensor_index-safety_angle ||
		sensor_index > init_sensor_index+safety_angle) {
		motor_power = 0;
		PORTA = 0b10000000;
	}
}

/*
 * MOTOR FUNCTION: motor_controller(char vel, char dir)
 */
void motor_controller(int vel, char dir) {

	// SPEED
	motor_PWM_duty = vel*motor_PWM_period/100;
	motor_updateDuty();

	// DIRECTION
	motor_direction(dir);

	// TIME
	motor_timer = 2; // 0.008 sec interval, 122Hz


// break OFF
	if (motor_PWM_duty > min_motor_duty) motor_break(0);

// Wait until timer runs out
	while (motor_timer > 0) {}

// 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 & 0b11101111;
			break;
		case 1:		// break ON
			PORTD = PORTD | 0b00010000;
			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_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.2 Ch A input of Encoder --> triggers INT0
	//D.3 Ch B input of Encoder --> triggers INT1
	//D.4 output for motor's break signal
	//D.5 OC1A output for motor's PWM signal
	//D.6 output for motor's direction signal
	DDRD = 0b01110000;

	// LED's
	//A.0 Green LED to indicate direction
	//A.2 Yellow LED for debugging purposes
	//A.4 Green LED to indicate direction
	//A.7 Red LED to indicate stop of motor operations
	DDRA = 0xff;
	PORTA = 0x00;

// Timer 0
	/* TCCR0:
		[7] Force Output Compare = 0
		[6,3] Waveform Generation Mode = 00 (normal)
		[5,4] Compare Match Output Mode = 00 (nornaml operation, OC0 disconnected)
		[2,1,0] Clock Select = 100 (clk/256)
	 */
	TCCR0 = 0b00000100;

// 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=ICR1)
	 */
	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=ICR1)
		[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;


// External Interrupt
	/* MCUCR - MCU Control Register:
		[3,2] Interrupt Sense Control 1 = 01 (ISR on logical change)
		[1,0] Interrupt Sense Control 0 = 01 (ISR on logical change)
	 */
	MCUCR = 0b00000101;

	/* GICR - General Interrupt Control Register:
		[7] External Interrupt Request 1 Enable = 1
		[6] External Interrupt Request 0 Enable = 1
		[5] External Interrupt Request 2 Enable = 0
	 */
	GICR = 0b11000000;


// Global Variables
	motor_PWM_period = init_motor_period;
	motor_updatePeriod();
	motor_PWM_duty = init_motor_duty;
	motor_updateDuty();
	motor_break_bit = 1;
	motor_break(motor_break_bit);
	motor_power = 1;
	motor_timer = 0;
	sensor_index = init_sensor_index;
	sensor_channel = (PIND & 0b00001100) >> 2;
	pid_prev_error = 0;
	pid_sum_error = 0;
	safety_forward = 0;
	safety_backward = 0;

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