//FILENAME:	segway.c
//AUTHOR:	Clayton Campagna, Lyndsy Smith, John Wenstrom, Matt Bloem, Tyler Hop
//DESCPRITION:	EGR 345, Fall 2004
//		This program controls the balancing robot allowing it to balance on two wheels
//		and follow a line of electrical tape along the floor.  It automatically adjusts
//		for any center of mass. 

#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 15 		/* Controller proportional gain constant */
#define Ki 1		/* Controller integral gain constant */
#define Kd 1		// derivitave gain
#define left_c_kinetic_pos 0x3a 	/* Left Motor static friction */
#define left_c_kinetic_neg 0x3a 	/* make the value positive */
#define left_c_static_pos 0x3a 		/* Left kinetic friction */
#define left_c_static_neg 0x3a		/* make the value positive */

#define right_c_kinetic_pos 0x57 	/* Right Motor static friction */
#define right_c_kinetic_neg 0x57 	/* make the value positive */
#define right_c_static_pos 0x57		/* Right kinetic friction */
#define right_c_static_neg 0x57		/* make the value positive */

#define desired_angle 0		/* desired angular difference */


#define c_max 255
#define c_min 255 
int left_count = 0; 			/* A left count value to be output on port B */
int right_count = 0; 			/* A right count value to be output on port B */
int e_sum = 0;
int left_moving= 0;			/* Assume it starts without moving */
int right_moving = 0;			/* Assume it starts without moving */
int left_Position = 0; 		/* Assume it starts at position 0 */
int right_Position = 0;		/* Assume it starts at position 0 */
int last_left_Position = 0; 		/* Assume it starts at position 0 */
int last_right_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 left_target_position = 0;		/* Position that will be incremented to define left motor start and end points */
int right_target_position = 0;	/* Position that will be incremented to define right motor start and end points */

int angle, angle_vel, last_angle = 0; //set to a reference angle (voltage)
int left_wheel_pos, left_wheel_vel, left_last_wheel_pos; // left wheel attributes
int right_wheel_pos, right_wheel_vel, right_last_wheel_pos; // right wheel attributes
int left_torque, right_torque; // wheel torque PWM to be set for tiping

//Flags to tell if tipping forward or backwards
int derivative_on = 1;
int front_angle = 0;
int rear_angle = 0;
int lastf_angle = 0;
int lastr_angle = 0;
//Prototypes
int left_deadband(int c_wanted);
int right_deadband(int c_wanted);
void PWM_init();
void left_PWM_update(int value);
void right_PWM_update(int value);
int v_output(int v_adjusted);
void IO_setup();
void IO_update();
int init();
//void left_EncoderUpdate(void);
//void right_EncoderUpdate(void);
int integrate(int e);
int controller1(int Cd, int Cf);
int controller2(int Cd, int Cf);
SIGNAL(SIG_OVERFLOW0);
void CLK_setup();
void delay(int ticks);
void AD_setup();
int AD_read();
int AD_read_front();
int AD_read_back();
void balance();


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 left_deadband(int c_wanted){	/* right motor deadband adjustment Call this routine when updating */
	int c_pos;
	int c_neg;
	int c_adjusted;

	if(c_wanted > c_max) c_wanted = c_max;
	if(c_wanted < -c_min) c_wanted = -c_min;

	if(left_moving == 1){
		c_pos = left_c_kinetic_pos;
		c_neg = left_c_kinetic_neg;
	} else {
		c_pos = left_c_static_pos;
		c_neg = left_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;
	} else { 			/* The output must be negative */
		c_adjusted = -c_neg - (unsigned)(c_min - c_neg) * -c_wanted / c_min;
	}
	
	return c_adjusted;
}
int right_deadband(int c_wanted){	/* Left motor deadband adjustment Call this routine when updating */
	int c_pos;
	int c_neg;
	int c_adjusted;

	if(c_wanted > c_max) c_wanted = c_max;
	if(c_wanted < -c_min) c_wanted = -c_min;

	if(right_moving == 1){
		c_pos = right_c_kinetic_pos;
		c_neg = right_c_kinetic_neg;
	} else {
		c_pos = right_c_static_pos;
		c_neg = right_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;
	} else { 			/* The output must be negative */
		c_adjusted = -c_neg - (unsigned)(c_min - c_neg) * -c_wanted / c_min;
	}
	
	return c_adjusted;
}


void PWM_init(){ 		/* Call this routine once when the program starts */
	DDRC |= (1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3);
				/* Set motor direction outputs on port C (PC0, PC1, PC2, PC3) */
	/* 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 right_PWM_update(int value){ 	/* To update the PWM output */
	if(value > 255) value = 255;
	if(value < 0) value = 0;
	OCR1A = value; 		/* Duty cycle of Left Motor PWM */
}
void left_PWM_update(int value){ 	/* To update the PWM output */
	if(value > 255) value = 255;
	if(value < 0) value = 0;
	OCR1B = value; 		/* Duty cycle of right motor PWM */
}

int right_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) | 0x01; 	/* 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) | 0x02; 		/* Bit 0 on, 1 off */
		if(v_adjusted < -255){ 			/* Clip output below minimum */
			RefSignal = 255;
		} 
		else {
			RefSignal = -v_adjusted;	/* Flip sign */
		}
	}
	return RefSignal;
}
int left_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 & 0xF3) | 0x04; 	/* 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 & 0xF3) | 0x08; 		/* 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();	// setup PWM
	
	// Set all Vcc 5V ouput pins
	DDRD = 0xFF;
	PORTD = 0xFF;
	DDRC = DDRC | _BV(PC4);
	DDRA = 0x00;
//	PORTA = 0x00;
	PORTC = PORTC | _BV(PC4);
	DDRB = 0xFF;
	PORTB = 0xFF;
}

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

	// Get values for motor/cart position and velocity preparing PWM 
	balance();

	/******** Must be Modified to encorporate balance() and drive() ******/
	//left_PWM_update(v_output(left_deadband(controller(table_update(),left_Position)))); //uses table for smooth motion
	//right_PWM_update(v_output(right_deadband(controller(table_update(),right_Position))));

	
	/*if (angle > 130){
		left_PWM_update(left_v_output(255));
		right_PWM_update(right_v_output(255));
		delay(200);
	}
	if (angle < -130){
		left_PWM_update(left_v_output(-255));
		right_PWM_update(right_v_output(-255));
		delay(200);
	}*/
	
	//else{
		left_PWM_update(left_v_output(left_deadband(controller1(angle,desired_angle)))); //uses table for smooth motion
		right_PWM_update(right_v_output(right_deadband(controller1(angle,desired_angle))));
	//}
	
	//set new last position
	last_right_Position = right_Position;
	last_left_Position = left_Position;
	
}
int init(){ 		/* Initialize the register once */
	//DDRB = 0x00; 	/* Set port B as inputs */

	return 0;
}

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

	new_state = (PINB & (_BV(PB0) | _BV(PB1)))>>2; 	// Read encoder state on PB0 and PB1

	 Update position value 
	if (state!=new_state) {
		switch (state) {
		case 0x00:
			if (new_state==0x01)
				left_Position++;
			else
				left_Position--;
			break;
		case 0x01:
			if (new_state==0x03)
				left_Position++;// wheel torque PWM to be set for tip compensation// wheel torque PWM to be set for tip compensation
			else
				left_Position--;
			break;
		case 0x03:
			if (new_state==0x02)
				left_Position++;
			else
				left_Position--;
			break;
		case 0x02:
			if (new_state==0x00)
				left_Position++;
			else
				left_Position--;
			break;
			}
		state = new_state;
	}
}
void right_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 on PB2 and PB3

	// Update position value 
	if (state!=new_state) {
		switch (state) {
		case 0x00:
			if (new_state==0x01)
				right_Position++;
			else
				right_Position--;
			break;
		case 0x01:
			if (new_state==0x03)
				right_Position++;
			else
				right_Position--;
			break;
		case 0x03:
			if (new_state==0x02)
				right_Position++;
			else
				right_Position--;
			break;
		case 0x02:
			if (new_state==0x00)
				right_Position++;
			else
				right_Position--;
			break;
			}
		state = new_state;
	}
}*/
int integrate(int e){
	e_sum += e * T;
	return e_sum;
}

int prev_e_1;
int prev_e_2;

int derivative1(int e){
	e_sum = (e-prev_e_1) / T;
	prev_e_1 = e;
	return e_sum;
}
/*int derivative2(int e){
	e_sum = (e-prev_e_2) / T;
	prev_e_2 = e;
	return e_sum;
} */

int controller1(int Cd, int Cf){
	int Ce;
	int Cw;
	
	Ce = Cd - Cf;
	Cw = Kp * Ce + Ki * integrate(Ce) / 1000 + derivative_on*(Kd * derivative1(Ce)*1000);
	
	return Cw;
}
/*int controller2(int Cd, int Cf){
	int Ce;
	int Cw;
	
	Ce = Cd - Cf;
	Cw = Kp * Ce + Ki * integrate(Ce) / 1000 + derivative_on*(Kd * derivative2(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 right_*/
}
/*************************optical sensors************************/
void AD_setup()
{
	// Set all Vcc 5V ouput pins
	DDRD = 0xFF;
	PORTD = 0xFF;
	DDRC = DDRC | _BV(PC4);
	DDRA = 0x00;
//	PORTA = 0x00;
	PORTC = PORTC | _BV(PC4);

	//ADMUX = 0xC5;
	//ADCSRA = 0xE0;
}

int AD_read()
{
	return (ADCW);
}

int AD_read_front()
{
	int voltage;

	ADMUX = 0xC1;
	ADCSRA = 0xC0;

	while ((ADCSRA & _BV(ADSC)) != 0){}

	voltage = AD_read();

	ADMUX = 0xC1;
	ADCSRA = 0xC0;

	while ((ADCSRA & _BV(ADSC)) != 0){}

	voltage = AD_read();
	
	return voltage;
}

int AD_read_back()
{
	int voltage;

	ADMUX = 0xC5;
	ADCSRA = 0xC0;

	while ((ADCSRA & _BV(ADSC)) != 0){}

	voltage = AD_read();
	
	ADMUX = 0xC5;
	ADCSRA = 0xC0;

	while ((ADCSRA & _BV(ADSC)) != 0){}

	voltage = AD_read();
	
	return voltage; 
}



#define desired_increment 5
// wheel torque PWM to be set for tip compensation
void balance(void)	// Program for segway balancing
{
   	//set reference angle voltage (zero point)
	//int front_zero_angle = 0xc0;	// ref. voltage values (TBD)
        //int rear_zero_angle = 0xe7;
	int front_zero_angle = 0x147;
	int rear_zero_angle = 0x14f;
	
	int acceptible_forward_tilt = 0;	//To be set based on experimentation
	int acceptible_reverse_tilt = 0;	//To be set based on experimentation
	//double time_interval = 0.010;
	//int balance_voltage;
	int front_change;
	int rear_change;
	int der_angle;
	angle = ((AD_read_front() - front_zero_angle) - (AD_read_back() - rear_zero_angle))/2;
	angle_vel = (angle - last_angle);	// compute angular velocity for tipping direction
	front_angle = AD_read_front();
	rear_angle = AD_read_back();
	front_change = lastf_angle - front_angle;
	rear_change = lastr_angle - rear_angle;
	der_angle = angle;
	// If within ok limits, set Cf to zero so nothing will happen
	if (angle > acceptible_reverse_tilt && angle < acceptible_forward_tilt)
		angle = 0;
	else if (der_angle < 0 && rear_change >0){
		derivative_on = 0;
	}else if (der_angle > 0 && front_change >0){
		derivative_on = 0;
	}else
		derivative_on = 1;

	//random angle check
	if (((angle - last_angle) > (200)) || ((angle - last_angle) < (-200))){
		angle = last_angle;
	}
		
	// set last angle equal to the current angle
	last_angle = angle;
	lastf_angle = front_angle;
	lastr_angle = rear_angle;
	/* if(angle > acceptible_forward_tilt){
		left_count = left_Position + desired_increment;
		right_count = right_Position  + desired_increment;
	}else if(angle < acceptible_reverse_tilt){
		left_count = left_Position - desired_increment;
		right_count = right_Position - desired_increment;
	}else{
		left_count = left_Position;
		right_count = right_Position;
	}*/		

}

int main(){
	//int target_position = 0;	/* Position that will be incremented by user to define start and end points */
	int front_position, back_position,c;
	//int duration = 0;
	
	sio_init();
	AD_setup();
	IO_setup();
	init();
	CLK_setup();
	
	for(;;){
		while((c = input()) == -1){} /* Wait for a keypress */
		if(c == 'r'){
			//right_count++;
			outln("right count incremented=");
			outint(right_count);
		}
		else if(c == 'e'){
			//right_count--;
			outln("right count decremented=");
			outint(right_count);	
		}
		else if(c == 'l'){
			//left_count++;
			outln("left count incremented=");
			outint(left_count);			
		}
		else if(c == 'k'){
			//left_count--;
			outln("left count decremented=");
			outint(left_count);	
		}else if (c == 'b') { /* Increment the output on port B */
			back_position = AD_read_back();
			outint(back_position);
			outln(" = back position;");
		} else if (c == 'f') {
			front_position = AD_read_front();
			outint(front_position);
			outln(" = front position;");
		} else if (c == 'a') {
			outint(angle);
			outln(" = angle;");
		}
		else if(c == 's'){
			left_count = left_Position;
			right_count = right_Position;
			outln("stopped");
			outint(left_count);
			outint(right_count);
		}
		else if(c == 'h'){
			outln("r:right motor forward");
			outln("e:right motor reverse");
			outln("l:left motor forward");
			outln("k:left motor reverse");
			outln("f:front opt. sensor");
			outln("b:back opt. sensor");
			}
	}
}