#include "derivative.h"
#include "ADC.h"
#include <HD44780.h>
#include <ltoaz.h>
#include "PWM.h"

unsigned char adcczas;
static short int offsetIt;
unsigned char flagaUAR=0;

float stalaK = 10.0;


char ADC_init(void)
{
	SIM_SCGC6 |= SIM_SCGC6_ADC0_MASK;			//sygnał zegarowy na przetwornik analogowo cyfrowy
	SIM_SCGC5 |=SIM_SCGC5_PORTB_MASK;			//zmienimy 3 wejscia ADC na piny portuB p0,p1,p2
	PORTB_PCR(0)= PORT_PCR_MUX(0);				//kanal nr 8	ADC
	PORTB_PCR(1)= PORT_PCR_MUX(0);				//kanal nr 9	ADC
	PORTB_PCR(2)= PORT_PCR_MUX(0);				//kanal nr 12	ADC
	ADC0_SC1A  = 0x1F;  						// moduł ADC0 wyłączony aktualnie

	ADC0_CFG1  |= ADC_CFG1_ADICLK(1) |           // (Bus clock)/2 = 12 MHz
	               ADC_CFG1_MODE(2) |             // Rozdzielczość = 10-bit
	               ADC_CFG1_ADLSMP_MASK |         // Długi czas próbkowania
	               ADC_CFG1_ADIV(2);              // (Input clock)/4 = 3 MHz
	ADC0_SC2  &= ~(1<<ADC_SC2_ADTRG_SHIFT );// Konwersja wyzwalana programowo	-> wstawiamy ADTRG =0

	ADC0_SC3   |= ADC_SC3_AVGE_MASK |            // Sprzętowe uśrednianie próbek
	               ADC_SC3_AVGS(3); 			//liczba próbek do usredniania = 32

	ADC0_SC3  |= ADC_SC3_CAL_MASK;              // Start sekwencji kalibracji
	while(ADC0_SC3 & ADC_SC3_CAL_MASK);         // Oczekiwanie na zakończenie kalibracji
	  if (ADC0_SC3 & ADC_SC3_CALF_MASK){          // Sprawdzenie wyniku kalibracji
	    ADC0_SC3  |= ADC_SC3_CALF_MASK;           // Zerowanie flagi CALF
	    return -1;                                // Jeśli konwersja zakończona błędem
	  }

	return 0;

}

int ADC_pomiar(unsigned char kanal)
{

	ADC0_SC1A = ADC_SC1_ADCH(kanal);		//wybranie kanalu do pomiaru

	while(!(ADC0_SC1A & ADC_SC1_COCO_MASK));// Oczekiwanie na zakończenie konwersji

	return (int) ADC0_RA;

}

void obslugaADC (void)
{
	unsigned char calk;
	short int pomiar,wynik,reszta;

	short int moment;


	char pradIt[8]="It=",napiecie[11]="Ut=",momentM[8]="M=";
	char *buff = napiecie+3;
	char *buff1 = pradIt+3;
	char *buff2 = momentM+2;
	static obiektLCD LCDnapiecie,LCDpradtwornika,LCDmoment;
	if(adcczas==200)		//5x na sekunde pomiar
	{
		/*
		 * CZĘŚĆ PROGRAMU ODPOWIEDZIALNA ZA OBLICZENIE/PRZELICZENIE NAPIECIA
		 */
		adcczas=0;


		pomiar=ADC_pomiar(8);
		wynik= ((long)(pomiar*3300*2))>>10;			//wynik w mV, vref =3.3V, blad na plytce, w rev E zostal poprawiony
													//mnozymy x2 aby skompensowac wplyw dzielnik 1/2 na pomiar -> zakres 0-6,6V
		calk=wynik/1000;
		reszta=wynik%1000;


		buff = ltoaz(calk,buff,10,0);
		*buff++='.';
		buff = ltoaz(reszta,buff,10,3);
		*buff++='V';
		*buff='\0';
		bRAM(napiecie,0,2,&LCDnapiecie);

		/*
		 * CZĘŚĆ PROGRAMU ODPOWIEDZIALNA ZA OBLICZENIE/PRZELICZENIE PRĄDU TWORNIKA
		 */
		pomiar=ADC_pomiar(9)-offsetIt;
		if(pomiar<0)pomiar=0;

		wynik= ((long)(pomiar*3300))>>10;

		wynik = wynik* (37/27);								//przemnazam przez odwrotnosc stalej enkodera
															//100mV/A sensivity czujnika dla zakresu 20A, napiecie dla 20A ->4,5V
		calk=wynik/100;										//liczba całkowita
		reszta=wynik%100;										//reszta, rozdzielczosc 0,1A tylko,szumy w tym układzie wynoszą 11mV/A

		buff1 = ltoaz(calk,buff1,10,0);
		*buff1++='.';
		buff1 = ltoaz(reszta,buff1,10,2);
		*buff1++='A';
		*buff1='\0';
		bRAM(pradIt,0,3,&LCDpradtwornika);

		/*
		 * CZĘŚĆ PROGRAMU ODPOWIEDZIALNA ZA OBLICZENIE/PRZELICZENIE MOMENTU
		 */

		moment=wynik*stalaK;			//przeliczenie momentu, wynik zawiera aktualna wartosc pomiaru ADC

		calk = moment/100;				//moment czesc calkowita
		reszta = moment%100;			//moment reszta
		buff2 = ltoaz(calk,buff2,10,0);
		*buff2++='.';
		buff2 = ltoaz(reszta,buff2,10,1);
		*buff2++='N';
		*buff2++='m';
		*buff2='\0';
		bRAM(momentM,0,4,&LCDmoment);

	}
}

void kalibczujnik(void)
{
	/*
	 * URUCHAMIAJ TO JAKO OSTATNIE ABY NAPIECIE NA WYJSCIU CZUJNIKA MOGLO SIE USTABILIZOWAĆ
	 */
	int kalibracja[10],srednia=0;
	unsigned char i;

	for(i=0;i<10;i++)
	{
	kalibracja[i]=ADC_pomiar(9);
	srednia=srednia+kalibracja[i];
	}
	offsetIt =srednia/10;

}







void regulatorPID (Type_Def_PID *x)
{
	/*
	 * u(t) =kp*e(t) + kp*Td*[e(t)-e(t-1)]/Tp + (kp*Ts/Ti)*(e(0)+e(1)+e(2)+...+e(t))
	 */


	if(flagaPID&&flagaUAR)

	{

	flagaPID=0;
	float I;
	short int D;

	x->uchybakt = x->wartzadana-ADC_pomiar(9) ;


	D= x->uchybakt - x->uchybpop;
	I= x->uchybakt * x->Ts + x->calkapop;

	x->wartwyjsc =(short int) (x->kp * x->uchybakt + (x->kp * x->Td * D)/x->Ts + ((x->kp * x->Ts)/(x->Ti)) * I);

	if((x->wartwyjsc)>2400) x->wartwyjsc=2400;
	if((x->wartwyjsc)<0)x->wartwyjsc=0;

	x->uchybpop = x->uchybakt;
	x->calkapop = I;

	cnt = x->wartwyjsc;



	}


}

void obslugaPID (Type_Def_PID *x)
{
	static obiektLCD LCDmomentUAR;
	static char momentUAR[10]="M=";
	char *buff= momentUAR+2;
	unsigned char reszta,calk;
	int	temp;

	/*
	 * 564,75 - > wartosc dla pradu I=0 plynacego w obwodzie
	 * 572    -> przyjmiemy taka wartosc za punkt odniesienia 0
	 */




	if(flagaUAR)
	{

			if(!(GPIOD_PDIR & (1<<5))) x->wartzadana+=2;				// przycisk odpowiadajacy za zwiekszanie
			if(x->wartzadana>1023) x->wartzadana=1023;

			if(!(GPIOD_PDIR & (1<<0))) x->wartzadana-=2;
			if(x->wartzadana<572) x->wartzadana=572;



			temp = ((long)(3300*(572-x->wartzadana)))>>10;							//wartosc przeliczeniowa posrednia w mV

			calk = (temp*stalaK)/100;											//czesc calkowita momentu nastawionego
			reszta=(unsigned char)(temp*stalaK)%10;		//badz uwazny tutaj!!! mozliwa poprawka !!									//reszta z momentu nastawionego



			buff=ltoaz(calk,buff,10,0);
			*buff++='.';
			buff=ltoaz(reszta,buff,10,1);
			*buff++='N';
			*buff++='m';
			*buff='\0';
			bRAM(momentUAR,11,4,&LCDmomentUAR);												//dopisz opoznienie do obslugi klawiszy

	}

	regulatorPID(&PID);

}