/*
	Egyensulyozo robot
*/

#define F_CPU 16000000UL
#define BAUD_RATE 19200

#define		pi		  3.1415926

#include <stdio.h>
#include <stdint.h>
#include <ctype.h> 
#include <math.h>
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include "usart_m16.h"
#include "roboardv10.h"
#include "terminal.h"
#include "kalman3.h"
#include "pid1.h"

#define PWM_L OCR1A
#define PWM_R OCR1B

#define ms _delay_ms

volatile unsigned short cntdiv = 3906, cycle;
volatile signed short LeftWheel = 0, RightWheel = 0;
volatile unsigned char freq0, freq2, usart_rxdata, PID_Time, PID_Time_Hz = 15, PID_Time_counter, bits, Sec;
volatile unsigned char Calibration_Sec;
float	gyro_multiplier = 0.1;

#define PID_Enable 1
#define PID_enable() (bits |= PID_Enable)
#define PID_disable() (bits &= ~PID_Enable)
#define PID_Enabled() (bits & PID_Enable)

#define Calibration_Enable 2
#define Calibration_enable() (bits |= Calibration_Enable)
#define Calibration_disable() (bits &= ~Calibration_Enable)
#define Calibration_Enabled() (bits & Calibration_Enable)

double KalmanAngle,KalmanAngle_Offset,degress,acc_z,acc_y,acc_x,gyro,ACC_OFFSET_X,ACC_OFFSET_Y,ACC_OFFSET_Z, Speed = 0;
static double	GYRO_OFFSET;

unsigned char config;
char buf[20];
unsigned char xout;
unsigned char yout;
unsigned char zout;

#define GYRO 3
#define ACC_X 4
#define ACC_Y 5
#define ACC_Z 6


PID			sPID;				//	PID Control Structure
double		rOut;				//	PID Response (Output)
double		rIn;				//	PID Feedback (Input)

int printCHAR(char character, FILE *stream)
{
   while ((UCSRA & (1 << UDRE)) == 0) {};

   UDR = character;

   return 0;
} 

FILE uart_str = FDEV_SETUP_STREAM(printCHAR, NULL, _FDEV_SETUP_RW); 

void adc_init(void)
{
	// Use the internal ref:
	ADMUX=(1<<REFS1)|(1<<REFS0);
	ADCSRA=_BV(ADEN)+_BV(ADPS2)+_BV(ADPS0); // enabled
}

uint16_t ad_conv(uint8_t channel) 
{
	unsigned char adlow,adhigh;

	ADMUX=(0<<REFS1)|(1<<REFS0)|(channel & 0x07);
    ADCSRA=(1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);

	ADCSRA |= (1<<ADSC);
	while(bit_is_set(ADCSRA,ADSC)); // wait for result 

	adlow=ADCL; // read low first!
	adhigh=ADCH; 
	return((uint16_t)((adhigh<<8)|(adlow & 0xFF)));
}

void init_pwm(void)	// PWM for wheel motors
{
	OCR1A = 255;	// Left Wheel
	OCR1B = 255;	// Right Wheel

	TCCR1A |= (1<<COM1A1) | (1<<COM1B1) | (1<<WGM10) | (1<<WGM12);
	TCCR1B |= (1<<CS11);
}

void init_rpm_counter(void)	// Wheel RPMs
{
	TCCR0 |= (1<<CS02) | (1<<CS01) | (1<<CS00); // T0
	TCCR2 |= (1<<CS20);
	ASSR |= (1<<AS2);
}

void pwm_update(signed short pwm_l, signed short pwm_r)
{
	if(pwm_l & 0x0100)
	{
		PWM_L = ~(pwm_l & 0xff) + 1;
		dir_left_reverse;
	}
	else
	{
		PWM_L = pwm_l & 0xff;
		dir_left_forward;
	}

	if(pwm_r & 0x0100)
	{
		PWM_R = ~(pwm_r & 0xff) + 1;
		dir_right_reverse;
	}
	else
	{
		PWM_R = pwm_r & 0xff;
		dir_right_forward;
	}
}

void init_gyro(void)
{
	ms(1000);
	GYRO_OFFSET = ad_conv(GYRO);
	GYRO_OFFSET = ad_conv(GYRO);
	GYRO_OFFSET = ad_conv(GYRO);
}

int main(void)
{
	init_ports();
	TIMSK |= (1<<TOIE1);
	init_pwm();
	init_rpm_counter();
	pwm_update(LeftWheel,RightWheel);
	usart_init();
	led1_off;
	led2_off;
	led3_off;
	UCSRB |= (1<<RXCIE);
	sei();

//	PID			sPID;				//	PID Control Structure
//	double		rOut;				//	PID Response (Output)
//	double		rIn;				//	PID Feedback (Input)

	PIDInit(&sPID);					//	Initialize Structure
	sPID.Proportion	= 4.62;			//	Set PID Coefficients
	sPID.Integral	= 0.29;
	sPID.Derivative	= 3.48;
	sPID.SetPoint   = (0);			//	Set PID Setpoint

	terminal_clear();
	usart_print("ADC test");

	led3_on;
	ms(3000);
	ACC_OFFSET_X = ad_conv(ACC_X);
	ACC_OFFSET_Y = ad_conv(ACC_Y);
	ACC_OFFSET_Z = ad_conv(ACC_Z);
	ms(1000);
	ACC_OFFSET_X = ad_conv(ACC_X);
	ACC_OFFSET_Y = ad_conv(ACC_Y);
	ACC_OFFSET_Z = ad_conv(ACC_Z);
	led3_off;

	double deg, gy;
	init_gyro();

	unsigned char Calibration_Sec_temp;

	led1_on;
	PID_Time = 3906/PID_Time_Hz;
	Calibration_Sec = 5;
	Calibration_Sec_temp = Sec;
	Calibration_enable();
	while(1)
	{

label:
		if(Calibration_Enabled())
		{
			if(Sec == (Calibration_Sec_temp + Calibration_Sec))
			{
				KalmanAngle_Offset = KalmanAngle;
				led1_off;
				Calibration_disable();
			}
		}
		
		// if(cycle >= 800) cycle = 800;

		acc_x = (signed short)ad_conv(ACC_X) - ACC_OFFSET_X;
		acc_y = (signed short)ad_conv(ACC_Y) - ACC_OFFSET_Y;
		acc_z = (signed short)ad_conv(ACC_Z) - ACC_OFFSET_Z;
		gyro = ad_conv(GYRO);

		gy = (signed short)ad_conv(GYRO)-GYRO_OFFSET;

		deg = atan2(acc_x,acc_y);
		KalmanAngle = kalman(gy*gyro_multiplier,deg)*57.296;
		degress = KalmanAngle - KalmanAngle_Offset;

		if(degress < -80 || degress > 80) break;

/*************************************************************************************
										PID Process
**************************************************************************************/
		if(PID_Enabled())
		{
//		if(degress > -45 && degress < 45)
//		{
			rIn = degress;					//	Read Input
			rOut = PIDCalc(&sPID,rIn );		//	Perform PID Interation
			LeftWheel = RightWheel = rOut;			//	Effect Needed Changes
			pwm_update(LeftWheel,RightWheel);
//		} else pwm_update(0,0);

//		if(degress < -45 || degress > 45) { cycle = 0; led1_on; }
			PID_disable();

		}
/*************************************************************************************
										PID Process End
**************************************************************************************/

/*
		fprintf(&uart_str,"%f,",deg*57.296);
		fprintf(&uart_str,"%f, ",degress);
		fprintf(&uart_str,"%f, ",PWM_R);
		fprintf(&uart_str,"%f\r\n",rOut);
*/
//		ms(1);

	}

	pwm_update(0,0);
	while(degress < -80 || degress > 80)
	{
		led3_on; ms(500); led3_off; ms(500);
		acc_x = (signed short)ad_conv(ACC_X) - ACC_OFFSET_X;
		acc_y = (signed short)ad_conv(ACC_Y) - ACC_OFFSET_Y;
		acc_z = (signed short)ad_conv(ACC_Z) - ACC_OFFSET_Z;
		gyro = ad_conv(GYRO);

		gy = (signed short)ad_conv(GYRO)-GYRO_OFFSET;

		deg = atan2(acc_x,acc_y);
		KalmanAngle = kalman(gy*gyro_multiplier,deg)*57.296;
		degress = KalmanAngle - KalmanAngle_Offset;
	}
	ms(1000);
	goto label;

	return 0;
}

ISR(TIMER1_OVF_vect)
{
	cntdiv--;
	if(cntdiv == 0)
	{
		freq0 = TCNT0;
		freq2 = TCNT2;
		TCNT0 = 0;
		ASSR &= ~(1<<AS2);
		TCNT2 = 0;
		ASSR |= (1<<AS2);
		cntdiv = 3906;
		led2_toggle;
		Sec++;
	}

	PID_Time_counter--;
	if(PID_Time_counter == 0)
	{
		PID_Time_counter = PID_Time;
		PID_enable();
	}
}

ISR(USART_RXC_vect)
{
	usart_rxdata = UDR;

	if(usart_rxdata == 'p') sPID.Proportion = sPID.Proportion - 0.01;
	if(usart_rxdata == 'P') sPID.Proportion = sPID.Proportion + 0.01;
	if(usart_rxdata == 'i') sPID.Integral = sPID.Integral - 0.01;
	if(usart_rxdata == 'I') sPID.Integral = sPID.Integral + 0.01;
	if(usart_rxdata == 'd') sPID.Derivative = sPID.Derivative - 0.01;
	if(usart_rxdata == 'D') sPID.Derivative = sPID.Derivative + 0.01;

	if(usart_rxdata == 't') { PID_Time_Hz--; PID_Time = 3906/PID_Time_Hz; }
	if(usart_rxdata == 'T') { PID_Time_Hz++; PID_Time = 3906/PID_Time_Hz; }

	if(usart_rxdata == 'g') gyro_multiplier = gyro_multiplier - 0.01;
	if(usart_rxdata == 'G') gyro_multiplier = gyro_multiplier + 0.01;

	if(usart_rxdata == '2') { Speed = Speed - 0.1; sPID.SetPoint   = (Speed); };
	if(usart_rxdata == '8') { Speed = Speed + 0.1; sPID.SetPoint   = (Speed); };

	terminal_clear();
	fprintf(&uart_str,"P: %2.2f\r\n",sPID.Proportion);
	fprintf(&uart_str,"I: %2.2f\r\n",sPID.Integral);
	fprintf(&uart_str,"D: %2.2f\r\n",sPID.Derivative);

	fprintf(&uart_str,"T: %d\r\n",PID_Time_Hz);
	fprintf(&uart_str,"G: %2.4f\r\n",gyro_multiplier);

	fprintf(&uart_str,"S: %2.2f\r\n",Speed);
}