//#define F_CPU 8000000

#include <avr/io.h>
#include <util/delay.h>

#define LED_PIN1 PB4
#define LED_PIN2 PB1
#define PWM_PIN1 PB0
#define ADC_PIN PB3


void adc_init(void)
{
	ADMUX &= ~(1<<REFS0) | (1<<REFS1); //ADC REF = Vcc Volt internally
	ADCSRA |= (1<<ADEN)|(1<<ADPS2);
	ADCSRA &= ~(1<<ADPS0)|(1<<ADPS1); // ADC eingeschaltet, 2 Prescale
	ADMUX &= ~(1<<MUX3) | (1<<MUX2);
	ADMUX |= (1<<MUX1) | (1<<MUX0); //kanal PB3 analog read
}


void timer_init(void)
{
	DDRB   |= (1 << PWM_PIN1);              // PWM output on PB2
	TCCR0A = (1 << COM0A1) | (1 << WGM00);  // phase correct PWM mode
	OCR0A  = 0x10;                          // initial PWM pulse width
	TCCR0B = (1 << CS00);   // clock source = CLK/1, start PWM
}

void led_init(void)
{
	DDRB |= (1 << LED_PIN1);
	PORTB |= (1<<LED_PIN1);
	DDRB |= (1 << LED_PIN2);
	PORTB |= (1<<LED_PIN2);
}

int main(void)
{	
	led_init();
	
	timer_init();

	adc_init();
	
	//initialise values for the pid controller
	float speedValue=600;
	float tachometer=0;
	float E=0;
	float Ealt=0;
	float Ealt2=0;
	float Y=0;
	float Yalt=0;
	float Kp=2; //calc value 0.755
	float Ki=2; //calc calue 0.3363
	float Kd=0.2; //calc value 0.421
	float Ta=0.0156;
	float q0=Kp+Ki*Ta+Kd/Ta;
	float q1=-Kp-2*(Kd/Ta);
	float q2=Kd/Ta;
	
	while(1)
	{
	ADCSRA |= (1<<ADSC);  //ADC aktivieren
	while(ADCSRA & (1<<ADSC));  //warten bis konvertierung abgeschlosen
	
	tachometer = ADC;
	
	E = speedValue - tachometer;			//Comparison
	Y = Yalt + q0*E + q1*Ealt + q2*Ealt2;	//controller equation
	Ealt = E;
	Ealt2 = Ealt;
	
	OCR0A  = (Y/4);

		if(tachometer<600)
		{
			PORTB |= (1 << LED_PIN1);
		}
		else
		{
			PORTB &= ~(1 << LED_PIN1);
		}
		
		if(tachometer<1000)
		{
			PORTB |= (1 << LED_PIN2);
		}
		else
		{
			PORTB &= ~(1 << LED_PIN2);
		}
	}
	return 0;
}