/** ###################################################################
**     Filename    : ProcessorExpert.c
**     Project     : ProcessorExpert
**     Processor   : MKL25Z128VLK4
**     Version     : Driver 01.01
**     Compiler    : GNU C Compiler
**     Date/Time   : 2012-11-16, 16:20, # CodeGen: 0
**     Abstract    :
**         Main module.
**         This module contains user's application code.
**     Settings    :
**     Contents    :
**         No public methods
**
** ###################################################################*/
/* MODULE ProcessorExpert */


/* Including needed modules to compile this module/procedure */
#include "Cpu.h"
#include "Events.h"
#include "TI1.h"
#include "TU1.h"
#include "AD2.h"
#include "AdcLdd1.h"
#include "IN1A.h"
#include "BitIoLdd3.h"
#include "IN2A.h"
#include "BitIoLdd5.h"
#include "IN1B.h"
#include "BitIoLdd4.h"
#include "IN2B.h"
#include "BitIoLdd6.h"
#include "TU2.h"
#include "WAIT1.h"
#include "BT.h"
#include "CsIO1.h"
#include "IO1.h"
#include "RWExt.h"
#include "ExtIntLdd1.h"
#include "RWExt2.h"
#include "ExtIntLdd4.h"
//#include "RWExt.h"
//#include "ExtIntLdd1.h"
//#include "RWExt2.h"
//#include "ExtIntLdd4.h"
#include "LWExt.h"
#include "ExtIntLdd2.h"
#include "LWExt2.h"
#include "ExtIntLdd3.h"
#include "PWM_ENA.h"
#include "PwmLdd1.h"
#include "PWM_ENB.h"
#include "PwmLdd2.h"
#include "I2C2.h"
/* Including shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"
#include "stdio.h"
#include "string.h"


/* User includes (#include below this line is not maintained by Processor Expert) */

#include "ProcessorExpert.h"

enum TypeOfMovement
{
	Balance,Forward,Backward,Brake,Left,Right
} Movement,MovementLast;

float PosFilterConst=0.9;
bool LowPassEnabled=TRUE;
float velocityScaleMove=0.40;
float velocityScaleStop=30;
float targetOffset=0;
int PocitadloJizdy=0;
int MaxAngleWanted=30000;
bool ZoneEnabled=TRUE;
volatile float LWPosLast=0;
int KalmanAngleInt=0;
float dps2=0;
long long dps3=0;
int TI1casovac=0;
int GyroSuma=0;
int GyroADCLast=0;
byte CalibrationPocitadlo=0;
bool SensorsInitialized=FALSE;
/*******************VARIABLES PRO PREKAZKY****************/

/*******************VARIABLES PRO BT-SERIOVKU****************/
LDD_TDeviceData *testDevData;   /* LDD pointer na strukturu dat zarizeni seriovky*/
word hodnotaParametru;		    /* hodnota parametru ulozena jako cislo*/
bool zobrazeniPrubehu=FALSE;	/* povoluje vypis prubehu na seriovku*/
bool vyzadanoCteni=FALSE;		/* flag oznacujici, zda se ceka na prijeti prikazu*/
bool zadanaHodnota=FALSE;		/* flag oznacujici, zda byla uz zadana hodnota uzivatelem*/
bool zadanyParametr=FALSE;		/* flag oznacujici, zda byl uz zadan parametr uzivatelem*/				
byte messageParametr[] = "Zadejte parametr:\n\r";  /* output - zero-terminated string!*/
byte messageHodnota[]= "Zadejte hodnotu:\n\r";  /* output - zero-terminated string!*/
byte buffer[sizeof(messageParametr)];       /* input buffer */
byte err;					/*chybova hlaska pri neodeslani zpravy*/
char messageUhel[7];
char messageAccel[5];
char messagePID[8];
char errMsg[]="chyba!\n\r"; /*vysle se jako chyba pri neprijeti prikazu */
char okMsg[]="OK!\n\r"; /*vysle se pro kontrolu prijeti prikazu */
byte parametr[1];   /*pismeno parametru, 1 mistny */
byte hodnota[4];	/*hodnota parametru, 4 mistna*/
volatile bool blockReceived = FALSE; /* ISR flagy pro EVENTS preruseni */
volatile bool blockSent = FALSE; /* ISR flagy pro EVENTS preruseni*/
/**********KALMAN******************************************************************************/
byte AverageAngleArrSize=1;
byte AverageAnglePocetPrvku=0;
float AverageAngleArr[10];
float KalmanAngleArrLast=0.0;
float AverageAngleSum=0.0;
float AveragedKalmanAngle=0.0;
float AveragedKalmanAngleLast=0.0;
float KalmanAngle=0.0;
float KalmanAngle0=0.0;
float KalmanAngle1=0.0;
float KalmanAngle2=0.0;
float KalmanAngle3=0.0;
float Q_angle  =  0.001; //0.001
float Q_gyro   =  0.003;  //0.003
float R_angle  =  0.03;  //0.03

float x_angle = 0;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float dt=0.01;
float y, S;
float K_0, K_1;
/************ VARIABLES PRO MERENI NAKLONU, CYKLICKEHO PRERUSENI A I2C Akcelerometru**********/
int kalibrovanyUhel=0;
word pocitadlo=0;
word soucetMMA845=0;
word poleMMA845[10];
extern bool FlagTI1Int;  /*flag cyklickeho preruseni*/
bool FlagMereniDokonceno=TRUE; /* flag dokonceni mereni naklonu, aby mohlo nastat nove mereni*/
extern bool FlagADC1Dokonceno; /* flag dokonceni adc prevodu z akcelerometru a gyra*/
volatile int OsaXoff;		   /* hodnota prevodu ADC z akcelerometru offboard*/
volatile int AvgUhel;  		   /* hodnota zprumerovana z akcelerometru offboard a onboard*/
volatile long int MappedAngleX=0; /* zlinearizovany uhel mapovaci funkci*/
volatile int AngleXLast=0; /* zlinearizovany uhel mapovaci funkci*/
volatile float GyroAngle=0;		/* hodnota uhlu z gyroskopu */
volatile float GyroAngleLast=0;	/* hodnota minuleho uhlu z gyroskopu pro integraci */
volatile float GyroZero=0;
volatile signed long dps=0;		/* hodnota uhlove rychlosti z gyroskopu */
volatile signed int dpsLast=0;  /* hodnota minule uhlove rychlosti z gyroskopu pro integraci */
volatile signed int ComplementaryWeight=990;
volatile signed int ComplementaryAngleLast=0;
volatile signed int ComplementaryAngle=0; /* hodnota uhlu naklonu z kombinace gyroskopu a akcelerometru*/
volatile bool DataTransmittedFlg = FALSE;/*flag pro preruseni z odeslani dat na i2c */
word pole[4];					 /* pole hodnot pro ADC z akcelerometru a gyroskopu, pole[0] pro gyro, pole[1] pro akcelerometr*/ 
/****************** PWM RIZENI MOTORU*************************************************************/
volatile bool MotorsEnabled=FALSE;
volatile bool PWM_Inicializovano=FALSE;
volatile bool PWMGeneratedFlag=FALSE;
LDD_TDeviceData *PWM_ENA_Pointer;	/* LDD pointer na strukturu dat zarizeni generovani PWM */
LDD_TDeviceData *PWM_ENB_Pointer;	/* LDD pointer na strukturu dat zarizeni generovani PWM*/
volatile int PID_OUT_MAX=29500;
volatile int PID_OUT_MIN=-29500;
volatile word PID_OUT=0;		// cela PID hodnota prenasena do motoru(pouze kladna)
volatile word PWM_Value=0;
/****************** PID WHEEL POSITION*********************************************************/
volatile int ZoneA=24000;
volatile int ZoneB=48000;
volatile int ZoneC=72000;
volatile int   LWpocitadloKroku=0; 		//counter pro enkoder pro levy kolo
volatile int   LWpocitadloKrokuLast=0;	//minule mereni counteru pro enkoder
volatile float LWPosition=0;
volatile float LWPositionLast=0;
volatile int   RWpocitadloKroku=0; 		//counter pro enkoder pro pravy kolo
volatile int   RWpocitadloKrokuLast=0;	//minule mereni counteru pro enkoder
volatile float RWPosition=0;
volatile float RWPositionLast=0;

volatile signed int PosErr=0;
volatile signed int LWPosErrLast=0;
volatile signed int RWPosErrLast=0;
volatile signed int LWPosWanted=0;
volatile signed int RWPosWanted=0;
volatile int PP_Const=0;
volatile int DP_Const=0;
volatile int IP_Const=0;
volatile int a=0,b=0,c=0;
volatile int aLast,bLast,cLast;
volatile int PID_PP=0;
volatile int PID_IP=0;
volatile int PID_IPL=0;
volatile int PID_IPR=0;
volatile int PID_DP=0;
bool LWB=FALSE;			//hodnota pinu enkoderu leveho kola kanalu B
bool LWA=FALSE;			//hodnota pinu enkoderu leveho kola kanalu A
bool RWB=FALSE;			//hodnota pinu enkoderu praveho kola kanalu B
bool RWA=FALSE;			//hodnota pinu enkoderu praveho kola kanalu A
/****************** PID WHEEL SPEED*********************************************************/
byte SpeedPocitadlo=0;
volatile float BrakeSpeed=1;
volatile float SpeedWanted=0.0;		//zadana rychlost kola
volatile float SpeedErr=0.0;		// chyba rychlosti kola
volatile float SpeedErrLast=0.0;	//minula chyba rychlosti kola

volatile float LWSpeedWanted=0;		//zadana hodnota rychlost leveho kola
volatile float LWSpeed=0;			//rychlost leveho kola
volatile float LWSpeedErr=0.0;		//chyba rychlosti leveho kola
volatile float LWSpeedErrLast=0.0;	//minula chyba rychlosti leveho kola
volatile float RWSpeedWanted=0;		//zadana hodnota rychlost leveho kola
volatile float RWSpeed=0;			//rychlost leveho kola
volatile float RWSpeedErr=0.0;		//chyba rychlosti leveho kola
volatile float RWSpeedErrLast=0.0;	//minula chyba rychlosti leveho kola
volatile float PW_Const=0.0;		//P konstanta pro PID rychlosti
volatile float DW_Const=0.0;		//D konstanta pro PID rychlosti
volatile float IW_Const=0.0;		//I konstanta pro PID rychlosti
volatile float PID_PW=0.0;
volatile float PID_DW=0.0;
volatile float PID_IW=0.0;
volatile float PID_W=0.0;

/****************** PID ANGLE control*************************************************************/
volatile float SampleTime=0.01;	//jak casto pocitame PID v desitkach milisekund
volatile int AngleOffset=30; //offset uhlu akcelerometru pro nevyvazeneho bota
volatile int AngleWanted=0;	//zadany uhel
volatile int LWAngleWanted=0;	//zadany uhel
volatile int RWAngleWanted=0;	//zadany uhel
volatile int LWAngleLast=0; //minuly namereny uhel
volatile int RWAngleLast=0; //minuly namereny uhel
volatile int AngleErr=0;		//uhlova odchylka
volatile word AngleDeltaErr=0;	//rozdil zadaneho uhlu a namereneho
volatile word AngleErrSum=0;	//suma rozdilu zadaneho a namereneho uhlu
volatile int LWAngleErrLast=0;	//rozdil v uhlech v minulem cyklu
volatile int RWAngleErrLast=0;	//rozdil v uhlech v minulem cyklu
volatile int PID_P;	// P slozka v PID
volatile int PID_I;	// I slozka v PID
volatile int PID_IL=0;	// I slozka v PID
volatile int PID_IR=0;	// I slozka v PID
volatile int PID_D;	// D slozka v PID
volatile int  PID=0;			// vypoctena PID hodnota(kladna i zaporna)
volatile int PIDSum=0;
volatile int PIDTreshold=250000; //treshold pro automatickou upravu offsetu
volatile word Pconst=0;			// P zesileni regul.
volatile word PconstMin=0;
volatile word PconstMax=9999;
volatile word Iconst=0;			// I konstanta regul.
volatile word IconstMin=0;
volatile word IconstMax=9999;
volatile word Dconst=0;			// D konstanta regul.
volatile word DconstMin=0;
volatile word DconstMax=9999;

/************ I2C variables ***************************************************************/
uint8_t OutData[2] = {0x2A, 0x1D};       //100Hz ODR, 2G range, LOW NOISE         /* Initialization of output data buffer */
uint8_t InData[MMA845X_BUFFER_SIZE];
LDD_TError Error;
LDD_TDeviceData *MyI2CPtr;
MMA845X mma845x;
int16_t mma845_tmp;
int mma845x_last;
int mma845z_last;
volatile bool DataTransmittedFlg;
volatile bool DataReceivedFlg;
static MMA845_STATE measuring = IDLE;
static MMA845_STATE measuring_last = RECEIVE;
volatile bool FlagReceive=FALSE;		 /*flag pro prijeti i2c dat pro dalsi moznost preruseni*/
volatile bool DataReceivedFlg = FALSE;   /*flag pro prijeti dat z i2c pro zpracovani mereni*/
/******************************************************************************************/
/******************************************************************************/

/****ABS FUNC******************************************************************/
int Abs(int arg) /*vezme argument a vytvori absolutni hodnotu a vrati*/
{
	if(arg<0)return (-arg);
	else return arg;
}

int AbsF(float arg) /*vezme argument a vytvori absolutni hodnotu a vrati*/
{
	if(arg<0)return (-arg);
	else return arg;
}
/******************************************************************************/
void BTResetReceive()
{
	vyzadanoCteni=FALSE;
	zadanaHodnota=FALSE;
	zadanyParametr=FALSE;
}
/****Bluetooth UART receive****************************************************/
void BluetoothReceive()
{
  if(zadanyParametr==FALSE) /*jestlize neni zadan parametr*/
  {
	if(vyzadanoCteni==FALSE) /* a nebylo jeste vyzadano cteni*/
	{
		err = BT_ReceiveBlock(testDevData, (LDD_TData*)&parametr, sizeof(parametr)); /* get new data */
		vyzadanoCteni=TRUE;
	}
	if(blockReceived) /*flag - prislo li preruseni o prijeti znaku parametru*/
	{
	  if (/*err != ERR_OK*/ 0) /*zkontroluj zda pri prijeti nedoslo k chybe*/
	  { /* error? */
		  BT_SendBlock(testDevData, (LDD_TData*)&errMsg, sizeof(errMsg)); /*pokud ano odesli chybu*/
		  if(blockSent)blockSent=FALSE; /* vynuluj preruseni pro odesilani znaku pro dalsi moznost posilani*/
	  }
	  else /*nebyla li chyba*/
	  {
		  if(parametr[0]=='P' || parametr[0]=='I' ||parametr[0]=='D' || parametr[0]=='W'|| parametr[0]=='p' || parametr[0]=='O'|| parametr[0]=='F'|| parametr[0]=='B'|| parametr[0]=='d'|| parametr[0]=='k'|| parametr[0]=='i' || parametr[0]=='a' || parametr[0]=='b' || parametr[0]=='c') /*odpovida li parametr temto moznostem*/
		  {
			  blockReceived = FALSE; /* vynuluj flag pro prijeti preruseni od zpravy*/
			  zadanyParametr=TRUE;   
			  vyzadanoCteni=FALSE;
			  BT_SendBlock(testDevData, (LDD_TData*)&messageHodnota, sizeof(messageHodnota)); /* send block of data */
			  if(blockSent)blockSent=FALSE;
			  
			  
		  }
		  else if(parametr[0]=='L')
		  {
			  if(LWpocitadloKroku<0)messageUhel[0]='-';
			  else messageUhel[0]='+';
			  messageUhel[1]=Abs(LWpocitadloKroku)/1000 +'0';
			  messageUhel[2]=(Abs(LWpocitadloKroku) % 1000)/100 +'0';
			  messageUhel[3]=(Abs(LWpocitadloKroku) % 100)/10 +'0';
			  messageUhel[4]=(Abs(LWpocitadloKroku) % 10) +'0';
			  messageUhel[5]='\n';
			  BT_SendBlock(testDevData, (LDD_TData*)&messageUhel, sizeof(messageUhel)); 
			  if(blockSent)blockSent=FALSE;
			  blockReceived = FALSE;
			  BTResetReceive();
			  
		  }
		  
		  else if(parametr[0]=='K')
		  		  {
			  	  	  int Angle=(int)(KalmanAngle*100);
		  			  if(Angle<0)messageUhel[0]='-';
		  			  else messageUhel[0]='+';
		  			  messageUhel[1]=Abs(Angle)/1000 +'0';
		  			  messageUhel[2]=(Abs(Angle) % 1000)/100 +'0';
		  			  messageUhel[3]=(Abs(Angle) % 100)/10 +'0';
		  			  messageUhel[4]=(Abs(Angle) % 10) +'0';
		  			  messageUhel[5]='\n';
		  			  BT_SendBlock(testDevData, (LDD_TData*)&messageUhel, sizeof(messageUhel)); 
		  			  if(blockSent)blockSent=FALSE;
		  			  blockReceived = FALSE;
		  			  BTResetReceive();
		  			  
		  		  }
		  else if(parametr[0]=='A')
		  		  		  {
			  	  	  	  	  int AWanted=LWAngleWanted/10;
		  		  			  if(AWanted<0)messageUhel[0]='-';
		  		  			  else messageUhel[0]='+';
		  		  			  messageUhel[1]=Abs(AWanted)/1000 +'0';
		  		  			  messageUhel[2]=(Abs(AWanted) % 1000)/100 +'0';
		  		  			  messageUhel[3]=(Abs(AWanted) % 100)/10 +'0';
		  		  			  messageUhel[4]=(Abs(AWanted) % 10) +'0';
		  		  			  messageUhel[5]='\n';
		  		  			  BT_SendBlock(testDevData, (LDD_TData*)&messageUhel, sizeof(messageUhel)); 
		  		  			  if(blockSent)blockSent=FALSE;
		  		  			  blockReceived = FALSE;
		  		  			  BTResetReceive();
		  		  			  
		  		  		  }
		  else if(parametr[0]=='s')
		  		  		  {
		  		  			  if(PIDSum<0)messageUhel[0]='-';
		  		  			  else messageUhel[0]='+';
		  		  			  messageUhel[1]=Abs(PIDSum)/1000000 +'0';
		  		  			  messageUhel[2]=(Abs(PIDSum) % 1000000)/100000 +'0';
		  		  			  messageUhel[3]=(Abs(PIDSum) % 100000)/10000 +'0';
		  		  			  messageUhel[4]=(Abs(PIDSum) % 10000)/1000 +'0';
		  		  			  messageUhel[5]='\n';
		  		  			  BT_SendBlock(testDevData, (LDD_TData*)&messageUhel, sizeof(messageUhel)); 
		  		  			  if(blockSent)blockSent=FALSE;
		  		  			  blockReceived = FALSE;
		  		  			  BTResetReceive();
		  		  			  
		  		  		  }
		  else if(parametr[0]=='o')
		  		  		  		  {
		  		  		  			  if(AngleOffset<0)messageUhel[0]='-';
		  		  		  			  else messageUhel[0]='+';
		  		  		  			  messageUhel[1]=Abs(AngleOffset)/1000 +'0';
		  		  		  			  messageUhel[2]=(Abs(AngleOffset) % 1000)/100 +'0';
		  		  		  			  messageUhel[3]=(Abs(AngleOffset) % 100)/10 +'0';
		  		  		  			  messageUhel[4]=(Abs(AngleOffset) % 10) +'0';
		  		  		  			  messageUhel[5]='\n';
		  		  		  			  BT_SendBlock(testDevData, (LDD_TData*)&messageUhel, sizeof(messageUhel)); 
		  		  		  			  if(blockSent)blockSent=FALSE;
		  		  		  			  blockReceived = FALSE;
		  		  		  			  BTResetReceive();
		  		  		  			  
		  		  		  		  }
		  /*else if(parametr[0]=='A')
		  {
			  if(pole[1]>0 && pole[1]<4095)
			  {
				  messageAccel[0]=pole[1]/1000 +'0';
				  messageAccel[1]=(pole[1] % 1000)/100 +'0';
				  messageAccel[2]=(pole[1] % 100)/10 +'0';
				  messageAccel[3]= (pole[1] % 10) +'0';
				  messageAccel[4]='\n';
			  }
			  BT_SendBlock(testDevData, (LDD_TData*)&messageAccel, sizeof(messageAccel)); 
			  if(blockSent)blockSent=FALSE;
			  blockReceived = FALSE;
			  BTResetReceive();
		  }*/
		  else if(parametr[0]=='G')
		  		{
		  		if(pole[0]>0 && pole[0]<4095)
		  			{
		  			messageAccel[0]=pole[0]/1000 +'0';
		  			messageAccel[1]=(pole[0] % 1000)/100 +'0';
		  			messageAccel[2]=(pole[0] % 100)/10 +'0';
		  			messageAccel[3]= (pole[0] % 10) +'0';
		  			messageAccel[4]='\n';
		  			}
		  		BT_SendBlock(testDevData, (LDD_TData*)&messageAccel, sizeof(messageAccel)); 
		  		if(blockSent)blockSent=FALSE;
		  		blockReceived = FALSE;
		  		BTResetReceive();
		  		}
		  else if(parametr[0]=='Z')
		  		  		{
		  		  		if(!zobrazeniPrubehu)zobrazeniPrubehu=TRUE;
		  		  		else zobrazeniPrubehu=FALSE;
		  		  		blockReceived = FALSE;
		  		  		BTResetReceive();
		  		  		parametr[0]=0;
		  		  		}
		  else if(parametr[0]=='M')
		  		  		  		{
		  		  		  		if(MotorsEnabled)MotorsEnabled=FALSE;
		  		  		  		else MotorsEnabled=TRUE;
		  		  		  		blockReceived = FALSE;
		  		  		  		BTResetReceive();
		  		  		  		parametr[0]=0;
		  		  		  		}
		  else if(parametr[0]=='R')
			  		  {
			  			  if(RWpocitadloKroku<0)messageUhel[0]='-';
			  			  else messageUhel[0]='+';
			  			  messageUhel[1]=Abs(RWpocitadloKroku)/1000 +'0';
			  			  messageUhel[2]=(Abs(RWpocitadloKroku) % 1000)/100 +'0';
			  			  messageUhel[3]=(Abs(RWpocitadloKroku) % 100)/10 +'0';
			  			  messageUhel[4]=(Abs(RWpocitadloKroku) % 10) +'0';
			  			  messageUhel[5]='\n';
			  			  BT_SendBlock(testDevData, (LDD_TData*)&messageUhel, sizeof(messageUhel)); 
			  			  if(blockSent)blockSent=FALSE;
			  			  blockReceived = FALSE;
			  			  BTResetReceive();
			  		  }
		  else if(parametr[0]=='S')
		  		  		  		  		  		{
		  			  	  	  	  	  	  	  	Movement=Brake;
		  		  		  		  		  		LWSpeedWanted=0.0;
		  		  		  		  		  		//LWSpeedErr=0;
		  		  		  		  		  		//LWSpeedErrLast=0;
		  		  		  		  		  		//PID_W=0;
		  		  		  		  		  		//AngleWanted=0;
		  		  		  		  		  		blockReceived = FALSE;
		  		  		  		  		  		BTResetReceive();
		  		  		  		  		  		parametr[0]=0;
		  		  		  		  		  		}
		  else if(parametr[0]=='T')
		  		  		  		  		  		  		{
			  	  	  	  	  	  	  	  	  	  	  	MovementLast=Balance;
		  		  			  	  	  	  	  	  	  	Movement=Left;
		  		  		  		  		  		  		blockReceived = FALSE;
		  		  		  		  		  		  		BTResetReceive();
		  		  		  		  		  		  		parametr[0]=0;
		  		  		  		  		  		  		}
	  }
	}
  }
  else if(zadanaHodnota==FALSE)
  {
	  if(vyzadanoCteni==FALSE)
	  {
		  err = BT_ReceiveBlock(testDevData, (LDD_TData*)&hodnota, sizeof(hodnota)); /* get new data */
	  		vyzadanoCteni=TRUE;
	  }
	  	  if(blockReceived)
	  	  {
	  		  if(parametr[0]=='P')
	  		  {
	  			  hodnotaParametru=(hodnota[0]-'0')*1000 + (hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  if(hodnotaParametru >PconstMin && hodnotaParametru < PconstMax) /*je li hodnota v rosahu */
	  			  {
	  				  BTResetReceive();
	  				  Pconst=hodnotaParametru*10;
	  				  parametr[0]=0;
	  				  BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */
	  				  if(blockSent)blockSent=FALSE;
	  			  }
	  			  else vyzadanoCteni=FALSE;
	  			  
	  		  }
	  		  else if(parametr[0]=='I')
	  		  {
	  			hodnotaParametru=(hodnota[0]-'0')*1000 + (hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  				  if(hodnotaParametru >=IconstMin && hodnotaParametru < IconstMax) /*je li hodnota v rosahu */
	  				  {
	  					BTResetReceive();			 
	  				  	Iconst=hodnotaParametru*10;			  
	  				  	parametr[0]=0;			  
	  				  	BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */			  
	  				  	if(blockSent)blockSent=FALSE;			  
	  				  }
	  				  else vyzadanoCteni=FALSE;			  
	  		  }
	  		  else if(parametr[0]=='D')
	  		  {
	  			hodnotaParametru=(hodnota[0]-'0')*1000 + (hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			if(hodnotaParametru >=DconstMin && hodnotaParametru < DconstMax) /*je li hodnota v rosahu */
	  			{
	  				BTResetReceive();			 
	  				Dconst=hodnotaParametru*10;  				  
	  				parametr[0]=0;  				  
	  				BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  				if(blockSent)blockSent=FALSE;  				  
	  			}
	  			else vyzadanoCteni=FALSE;		  			    			  
	  		  }
	  		  else if(parametr[0]=='p')
	  			  		  {
	  			  			hodnotaParametru=(hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			if(hodnotaParametru >=0 && hodnotaParametru < 1000) /*je li hodnota v rosahu */
	  			  			{
	  			  				BTResetReceive();			 
	  			  				PW_Const=((float)hodnotaParametru);  				  
	  			  				parametr[0]=0;  				  
	  			  				BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  				if(blockSent)blockSent=FALSE;  				  
	  			  			}
	  			  			else vyzadanoCteni=FALSE;		  			    			  
	  			  		  }

	  		else if(parametr[0]=='i')
	  			  			  		  {
	  			  			  			hodnotaParametru=(hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			if(hodnotaParametru >=0 && hodnotaParametru < 1000) /*je li hodnota v rosahu */
	  			  			  			{
	  			  			  				BTResetReceive();			 
	  			  			  				IW_Const=((float)hodnotaParametru);  				  
	  			  			  				parametr[0]=0;  				  
	  			  			  				BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  				if(blockSent)blockSent=FALSE;  				  
	  			  			  			}
	  			  			  			else vyzadanoCteni=FALSE;		  			    			  
	  			  			  		  }
	  		else if(parametr[0]=='d')
	  			  			  		  {
	  			  			  			hodnotaParametru=(hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			if(hodnotaParametru >=0 && hodnotaParametru < 1000) /*je li hodnota v rosahu */
	  			  			  			{
	  			  			  				BTResetReceive();			 
	  			  			  				DW_Const=((float)hodnotaParametru*10);  				  
	  			  			  				parametr[0]=0;  				  
	  			  			  				BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  				if(blockSent)blockSent=FALSE;  				  
	  			  			  			}
	  			  			  			else vyzadanoCteni=FALSE;
	  			  			  		  }
	  			  	
	  		else if(parametr[0]=='a')
	  			  			  			  		  {
	  			  			  			  			hodnotaParametru=(hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			  			if(hodnotaParametru >=0 && hodnotaParametru < 1000) /*je li hodnota v rosahu */
	  			  			  			  			{
	  			  			  			  				BTResetReceive();			 
	  			  			  			  				a=hodnotaParametru;  				  
	  			  			  			  				parametr[0]=0;  				  
	  			  			  			  				BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  			  				if(blockSent)blockSent=FALSE;  				  
	  			  			  			  			}
	  			  			  			  			else vyzadanoCteni=FALSE;
	  			  			  			  		  }
	  		else if(parametr[0]=='b')
	  			  			  			  		  {
	  			  			  			  			hodnotaParametru=(hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			  			if(hodnotaParametru >=0 && hodnotaParametru < 1000) /*je li hodnota v rosahu */
	  			  			  			  			{
	  			  			  			  				BTResetReceive();			 
	  			  			  			  				b=(hodnotaParametru);  				  
	  			  			  			  				parametr[0]=0;  				  
	  			  			  			  				BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  			  				if(blockSent)blockSent=FALSE;  				  
	  			  			  			  			}
	  			  			  			  			else vyzadanoCteni=FALSE;
	  			  			  			  		  }
	  		else if(parametr[0]=='c')
	  			  			  			  		  {
	  			  			  			  			hodnotaParametru=(hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			  			if(hodnotaParametru >=0 && hodnotaParametru < 1000) /*je li hodnota v rosahu */
	  			  			  			  			{
	  			  			  			  				BTResetReceive();			 
	  			  			  			  				c=(hodnotaParametru);  				  
	  			  			  			  				parametr[0]=0;  				  
	  			  			  			  				BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  			  				if(blockSent)blockSent=FALSE;  				  
	  			  			  			  			}
	  			  			  			  			else vyzadanoCteni=FALSE;
	  			  			  			  		  }
	  		  else if(parametr[0]=='W')
	  			  			  		  {
	  			  			  			hodnotaParametru=(hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			if(hodnotaParametru >=0 && hodnotaParametru < 600) /*je li hodnota v rosahu */
	  			  			  			{
	  			  			  				BTResetReceive();			 
	  			  			  				PID_OUT_MAX=hodnotaParametru*100;
	  			  			  				PID_OUT_MIN=-(hodnotaParametru*100);
	  			  			  				parametr[0]=0;  				  
	  			  			  				BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  				if(blockSent)blockSent=FALSE;  				  
	  			  			  			}
	  			  			  			else vyzadanoCteni=FALSE;		  			    			  
	  			  			  		  } 
	  		 else if(parametr[0]=='O')
	  			  	{
	  			  			  hodnotaParametru=(hodnota[1]-'0')*100+(hodnota[2]-'0')*10+(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  if(hodnotaParametru >=0 && hodnotaParametru < 400) /*je li hodnota v rosahu */
	  			  			  	{
	  			  			  		BTResetReceive();			 
	  			  			  		if(hodnota[0]=='-')AngleOffset=-hodnotaParametru;
	  			  			  		else AngleOffset=hodnotaParametru;
	  			  			  		parametr[0]=0;  				  
	  			  			  		BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  		if(blockSent)blockSent=FALSE;  				  
	  			  			  	}
	  			  			  	else vyzadanoCteni=FALSE;		  			    			  
	  			  	}  	
	  		else if(parametr[0]=='F')
	  			  			  			  	{
	  			  			  			  			  hodnotaParametru=(hodnota[2]-'0')*10 + (hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			  			  if(hodnotaParametru >=0 && hodnotaParametru < 100) /*je li hodnota v rosahu */
	  			  			  			  			  	{
	  			  			  			  			  		BTResetReceive();	
	  			  			  			  			  		Movement=Forward;
	  			  			  			  			  		MovementLast=Balance;
	  			  			  			  			  		LWSpeedWanted=(float)(-hodnotaParametru);
	  			  			  			  			  		zobrazeniPrubehu=TRUE;
	  			  			  			  			  		//targetOffset=(float)(-hodnotaParametru*100);
	  			  			  			  			  		parametr[0]=0;  				  
	  			  			  			  			  		BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  			  			  		if(blockSent)blockSent=FALSE;  				  
	  			  			  			  			  	}
	  			  			  			  			  	else vyzadanoCteni=FALSE;		  			    			  
	  			  			  			  	}  	
	  		else if(parametr[0]=='B')
	  			  			  			  			  	{
	  			  			  			  			  			  hodnotaParametru=(hodnota[2]-'0')*10 + (hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			  			  			  if(hodnotaParametru >=0 && hodnotaParametru < 100) /*je li hodnota v rosahu */
	  			  			  			  			  			  	{
	  			  			  			  			  				  Movement=Backward;
	  			  			  			  			  				  MovementLast=Balance;
	  			  			  			  			  			  		BTResetReceive();			 
	  			  			  			  			  			  		zobrazeniPrubehu=TRUE;
	  			  			  			  			  			  		LWSpeedWanted=(float)(hodnotaParametru);
	  			  			  			  			  			  		parametr[0]=0;  				  
	  			  			  			  			  			  		BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  			  			  			  		if(blockSent)blockSent=FALSE;  				  
	  			  			  			  			  			  	}
	  			  			  			  			  			  	else vyzadanoCteni=FALSE;		  			    			  
	  			  			  			  			  	}  	
	  		else if(parametr[0]=='k')
	  			  			  	{
	  			  			  			  hodnotaParametru=(hodnota[3]-'0'); //ze 4 ciselneho parametru sebere tri posledni cisla a vytvori z nich hodnotu*/
	  			  			  			  if(hodnotaParametru >=0 && hodnotaParametru < 10) /*je li hodnota v rosahu */
	  			  			  			  	{
	  			  			  			  		BTResetReceive();			 
	  			  			  			  		AverageAngleArrSize=hodnotaParametru;
	  			  			  			  		AverageAnglePocetPrvku=0;
	  			  			  			  		AverageAngleSum=0;
	  			  			  			  		parametr[0]=0;  				  
	  			  			  			  		BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */  				  
	  			  			  			  		if(blockSent)blockSent=FALSE;  				  
	  			  			  			  	}
	  			  			  			  	else vyzadanoCteni=FALSE;		  			    			  
	  			  			  	}  	
	  	  }
	  	}
  }
/*ZOBRAZENI PRUBEHU NA SERIOVKU*/
void ZobrazPrubeh(){
//		parametr[0]='0';
//		  int Angle=KalmanAngle*100;
//		  if(Angle<0)messagePID[0]='-';
//		  else messagePID[0]='+';
//		  			  messagePID[1]=Abs(Angle)/1000 +'0';
//		  			  messagePID[2]=(Abs(Angle) % 1000)/100 +'0';
//		  			  messagePID[3]=(Abs(Angle) % 100)/10 +'0';
//		  			  messagePID[4]=(Abs(Angle) % 10) +'0';
//		  			  messagePID[5]='\t';
//		  			if(MappedAngleX<0)messagePID[6]='-';
//		  						  else messagePID[6]='+';
//		  						  messagePID[7]=Abs(MappedAngleX)/1000 +'0';
//		  						  messagePID[8]=(Abs(MappedAngleX) % 1000)/100 +'0';
//		  						  messagePID[9]=(Abs(MappedAngleX) % 100)/10 +'0';
//		  						  messagePID[10]=(Abs(MappedAngleX) % 10) +'0';
//		  						messagePID[11]='\t';
//		  								  			if(dps<0)messagePID[12]='-';
//		  								  						  else messagePID[13]='+';
//		  								  						  messagePID[14]=Abs(dps)/1000 +'0';
//		  								  						  messagePID[15]=(Abs(dps) % 1000)/100 +'0';
//		  								  						  messagePID[16]=(Abs(dps) % 100)/10 +'0';
//		  								  						  messagePID[17]=(Abs(dps) % 10) +'0';
//		  								  						messagePID[18]='\t';
//		  						messagePID[19]='\n';
//		  						parametr[0]='0';
//		  						if(KalmanAngleInt<0)messagePID[0]='-';
//		  						else messagePID[0]='+';
//		  						messagePID[1]=Abs(KalmanAngleInt/10)/1000 +'0';
//		  						messagePID[2]=(Abs(KalmanAngleInt/10) % 1000)/100 +'0';
//		  						messagePID[3]=(Abs(KalmanAngleInt/10) % 100)/10 +'0';
//		  						messagePID[4]=(Abs(KalmanAngleInt/10) % 10) +'0';
//		  						messagePID[5]='\t';
//		  						
//		  						if(LWPosition<0)messagePID[0]='-';
//		  								  		else messagePID[0]='+';
//		  								  						messagePID[1]=Abs(LWPosition*10)/1000 +'0';
//		  								  					messagePID[2]=(Abs(LWPosition*10) % 1000)/100 +'0';
//		  								  					messagePID[3]=(Abs(LWPosition*10) % 100)/10 +'0';
//		  								  					messagePID[4]=(Abs(LWPosition*10) % 10) +'0';
//		  								  					messagePID[5]='\t';
									parametr[0]='0';
			  						if(LWSpeed<0)messagePID[0]='-';
			  						else messagePID[0]='+';
			  						messagePID[1]=Abs(LWSpeed*10)/1000 +'0';
			  						messagePID[2]=(Abs(LWSpeed*10) % 1000)/100 +'0';
			  						messagePID[3]=(Abs(LWSpeed*10) % 100)/10 +'0';
			  						messagePID[4]=(Abs(LWSpeed*10) % 10) +'0';
			  						messagePID[5]='\t';
		  								  						messagePID[6]='\n';
		  			  BT_SendBlock(testDevData, (LDD_TData*)&messagePID, sizeof(messagePID)); 
		  			  while(!blockSent);
		  			  blockSent=FALSE;
		  			  blockReceived = FALSE;
		  			  BTResetReceive();
		  			  //BluetoothReceive();
}
/******************************************************************************/
/******************************************************************************/

/* Prepinani stavu pri preruseni z I2C akcelerometru***************************
******************************************************************************/
void Timer_Interrupt_CB(void)
{
  if(measuring == IDLE)
  {
    if(measuring_last == TRANSMIT)
      measuring = RECEIVE; 
    else if(measuring_last == RECEIVE)
      measuring = TRANSMIT;
  }
}

/**************************************************************************//*!
* Inicializace I2C akcelerometru  MMA845x
******************************************************************************/
void MMA845X_Init(void)
{
  /* Configure I2C BUS device(e.g. RTC) - Write Operation */
  Error = I2C2_MasterSendBlock(MyI2CPtr, &OutData, 2, LDD_I2C_NO_SEND_STOP); /* Send OutData (4 bytes) on the I2C bus and generates a stop condition to end transmission */
  while (!DataTransmittedFlg); 
  DataTransmittedFlg = FALSE;
  
  /*setting high resolution sampling*/
  OutData[0] = 0x2B;
  OutData[1] =0x02;
  Error = I2C2_MasterSendBlock(MyI2CPtr, &OutData, 2, LDD_I2C_NO_SEND_STOP); /* Send OutData (4 bytes) on the I2C bus and generates a stop condition to end transmission */
  while (!DataTransmittedFlg); 
  DataTransmittedFlg = FALSE;
  
  OutData[0] = MMA845X_DEVICE_ID; 

  Error = I2C2_MasterSendBlock(MyI2CPtr, &OutData, 1, LDD_I2C_NO_SEND_STOP);
  while (!DataTransmittedFlg);  
  DataTransmittedFlg = FALSE;
  
  Error = I2C2_MasterReceiveBlock(MyI2CPtr, &mma845x.id, 1, LDD_I2C_SEND_STOP);
  while (!DataReceivedFlg);
  DataReceivedFlg = FALSE;
}

/**************************************************************************//*!
* @brief    MMA845x polling function 
******************************************************************************/
void MMA845X_Poll(void)
{
    if(measuring == TRANSMIT)
    {
      measuring = IDLE;
      measuring_last = TRANSMIT;
       
      OutData[0] = MMA845X_CFG_VALUE;
      Error = I2C2_MasterSendBlock(MyI2CPtr, &OutData, 1, LDD_I2C_NO_SEND_STOP); /* Send OutData (4 bytes) on the I2C bus and generates a stop condition to end transmission */
      while (!DataTransmittedFlg); 
      DataTransmittedFlg = FALSE;   
    }
    else if(measuring == RECEIVE)
    {
      measuring = IDLE;
      measuring_last = RECEIVE;
      Error = I2C2_MasterReceiveBlock(MyI2CPtr, &InData, MMA845X_BUFFER_SIZE, LDD_I2C_SEND_STOP);
      while (!DataReceivedFlg);
      DataReceivedFlg = FALSE;  
      
      /*
      mma845_tmp = InData[1] | (InData[0] << 8);
      //mma845x.x = (mma845_tmp / MMA845X_DIVIDER);
      mma845x.x = (mma845_tmp >> 4);
      //mma845x.x = mma845x_last*0.98 + ((mma845_tmp >> 4)+2048)*0.02;
      //mma845x_last = (mma845x.x * 1000)/1024;
       * */

      mma845_tmp = InData[3] | (InData[2] << 8);
      //mma845x.y = (mma845_tmp / MMA845X_DIVIDER);
      mma845x.y = (mma845_tmp >> 4);
      mma845x_last = (mma845x.y * 1000)/1024;

      //mma845_tmp = InData[5] | (InData[4] << 8);     
      //mma845x.z = (mma845_tmp / MMA845X_DIVIDER); 
      //mma845x.z = mma845z_last*0.5 +((mma845_tmp >> 4)+2048)*0.5;
      //mma845x.z = ((mma845_tmp >> 4));
      //mma845z_last = mma845x.z;
    }
}

/**************************************************************************//*!
***********MAPOVACI FUNKCE PRO ZLINEARIZOVANI ROZSAHU AKCELEROMETRU***********
******************************************************************************/
int map(int x)
{
	//int x,int xMin,int yMin, int yMax
	//angle=(x - xmin)/((xmax-xmin)*(ymax-ymin)) + ymin
	/*
	 * if(x<259) 		 return (((((1000*(x))/(259 - 0))*(1500 - 0)) + 0) / 1000);
	else if(x < 500) return (((((1000*(x - 259))/(500 - 259))*(3000 - 1500)) + 1500*1000) / 1000);
	else if(x < 707) return (((((1000*(x - 500))/(707 - 505))*(4500 - 3000)) + 3000*1000) / 1000);
	else if(x < 866) return (((((1000*(x - 707))/(866 - 707))*(6000 - 4500)) + 4500*1000) / 1000);
	else if(x < 966) return (((((1000*(x - 866))/(966 - 866))*(7500 - 6000)) + 6000*1000) / 1000);
	else {if(x>1000)x=1000;
					 return (((((1000*(x - 966))/(1000 - 966))*(9000 - 7500)) + 7500*1000)/1000);}
	 */
	if(x<259) 		  return  ((((1000*x)/259)*1500)/1000);
	else  if(x < 500) return (((((1000*(x - 259))/241)*1500)/1000) + 1500);
	else  if(x < 707) return (((((1000*(x - 500))/207)*1500)/1000) + 3000);
	else  if(x < 866) return (((((1000*(x - 707))/159)*1500)/1000) + 4500);
	else  if(x < 966) return (((((1000*(x - 866))/100)*1500)/1000) + 6000);
	else {if(x>1000)x=1000;
	return (((((1000*(x - 966))/34)*1500)/1000) + 7500);}
}
/******************************************************************************/
float LowPassFilter(float Actual,float Previous,float a)
{
	Actual=a*Previous+(1-a)*Actual;
	return Actual;
}

/* GYRO READING****************************************************************/
int GetGyro()
{
	//if((Abs(GyroADCLast-pole[0]))>12 && SensorsInitialized)pole[0]=(GyroADCLast+pole[0])/2;
	/*if(Abs(pole[0]-GyroADCLast)>300 && Abs(pole[0]-GyroADCLast)<700)
		{
			pole[0]=(GyroADCLast+pole[0])/2;
		}
	else if(Abs(pole[0]-GyroADCLast)>=700)
	{
		pole[0]=GyroADCLast;
	}
	*/
	//dps2=(pole[0]-GyroZero)*0.09706795934;
	//dps3=(((long long)((((int)pole[0])*10-GyroZero*10)))*6066747)/10000000;
	dps2=(pole[0]-GyroZero)*0.006066747;
	//dps2=LowPassFilter(dps2,dpsLast,0.6);
	//if(AbsF(dps2-dpsLast)<1.6)dps2=dpsLast;
	//if((Abs(GyroADCLast-pole[0]))>8)pole[0]=GyroADCLast;
	//dps=(((signed long)pole[0] - GyroZero) * 9753) / 1000;
	//GyroAngle+=((dps2+dpsLast)/2)*0.01;
	dpsLast=dps3;
	//GyroADCLast=pole[0];
	return dps3;
}
/**************************************************************************//*!
* VYPOCET UHLU KOMPLEMENTARNIM FILTREM
******************************************************************************/
int ComplemFiltCalc(int Gyro,int AccA)
{
	ComplementaryAngle=((ComplementaryWeight*(ComplementaryAngle + (dps/100)))+((1000-ComplementaryWeight)*AccA))/1000;
	//if(Abs(ComplementaryAngleLast-ComplementaryAngle)<40)ComplementaryAngle=ComplementaryAngleLast;
	dpsLast=dps;
	return ComplementaryAngle;
}

float KalmanFiltCalc(float newAngle, float newRate) 
{
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;

    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;

    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;

    return x_angle;
 }
/*
 * byte  AverageAngleArrSize=20;
float AverageAngleArr[AverageAngleArrSize];
float KalmanAngleArrLast=0;
float KalmanAngle=0.0;
 */
void AverageAngleArrInit()
{
	byte i;
	for(i=0;i<AverageAngleArrSize;i++)
		{
			AverageAngleArr[i]=0.0;
		}
}

float GetKalmanAverage()
{
	byte i;
	if(AverageAnglePocetPrvku<AverageAngleArrSize)
	{
		AverageAngleArr[AverageAnglePocetPrvku]=KalmanAngle;
		AverageAngleSum+=KalmanAngle;
		AverageAnglePocetPrvku++;
		return KalmanAngle;
	}
	else
	{
		KalmanAngleArrLast=AverageAngleArr[0];
		AverageAngleSum-=KalmanAngleArrLast;
		for(i=0;i<AverageAngleArrSize-1;i++)
		{
			AverageAngleArr[i]=AverageAngleArr[i+1];
		}
		AverageAngleArr[AverageAngleArrSize-1]=KalmanAngle;
		AverageAngleSum+=KalmanAngle;
		return AverageAngleSum/AverageAngleArrSize;
	}
	
	/*KalmanAngle3=KalmanAngle2;
	KalmanAngle2=KalmanAngle1;
	KalmanAngle1=KalmanAngle0;
	KalmanAngle0=KalmanAngle;
	return (KalmanAngle0+KalmanAngle1+KalmanAngle2+KalmanAngle3)/4;*/
}
/******************************************************************************/
/* ZJISTOVANI PREKAZEK KOLEM ROBOTA
******************************************************************************/
void ZjistiPrekazky()
{
	//if(pole[1]>3000)pole[1]=3000;
	//SharpVzdalenost=(9*pole[1]*pole[1]-50000*pole[1]+81272000)/10000;
	
}
/******************************************************************************/

void StopMotors()
{
			  IN1A_PutVal(TRUE);
			  IN2A_PutVal(TRUE);
			  IN1B_PutVal(TRUE);
			  IN2B_PutVal(TRUE);
}

void SetPWM()
{
	//PWM_ENB_SetRatio16(65535);  /*posleme PWM hodnotu motoru B*/
	//PWM_ENA_SetRatio16(65535);	/*posleme PWM hodnotu motoru A*/
	PWM_ENA_SetRatio16(PWM_Value);	/*posleme PWM hodnotu motoru A*/
	PWM_ENB_SetRatio16(PWM_Value);  /*posleme PWM hodnotu motoru B*/
}

void SetLWPWM()
{
	//PWM_ENB_SetRatio16(65535);  /*posleme PWM hodnotu motoru B*/
	//PWM_ENA_SetRatio16(65535);	/*posleme PWM hodnotu motoru A*/
	PWM_ENA_SetRatio16(PWM_Value);	/*posleme PWM hodnotu motoru A*/
}

void SetRWPWM()
{
	//PWM_ENB_SetRatio16(65535);  /*posleme PWM hodnotu motoru B*/
	//PWM_ENA_SetRatio16(65535);	/*posleme PWM hodnotu motoru A*/
	PWM_ENB_SetRatio16(PWM_Value);  /*posleme PWM hodnotu motoru B*/
}

float constrainFloat(float value,float min,float max)
{
	if(value<min)return min;
	else if(value>max)return max;
	else return value;
}

int constrainInt(int value, int min, int max)
{
	if(value<min)return min;
		else if(value>max)return max;
		else return value;
}

void PosPIDTune()
{
  if(ZoneEnabled)
  {
	if(Abs(PosErr)<10000)
	{
		PP_Const=a;
		IP_Const=b;
		DP_Const=c;
	}
	else if(Abs(PosErr)<ZoneA)
	{
		PP_Const=(12*a)/10;
		DP_Const=(13*c)/10;
		IP_Const=b;
	}
	else if(Abs(PosErr)<ZoneB)
	{
		PP_Const=(14*a)/10;
		IP_Const=b;
		DP_Const=(15*c)/10;
	}
	else if(Abs(PosErr)<ZoneC)
	{
		IP_Const=b;
		//PID_IP=0;
		PP_Const=(16*a)/10;
		DP_Const=(17*c)/10;
	}
	else
	{
		IP_Const=b;
		//PID_IP=0;
		PP_Const=(18*a)/10;
		DP_Const=(22*c)/10;
	}
  }
  else 
  {
	  PP_Const=a;
	  IP_Const=b;
	  DP_Const=c;
  }
}

void CalcLWPosPID()
{
	//LowPassFilter(LWpocitadloKroku,LWpocitadloKrokuLast,0.8);
	PosErr=(LWPosWanted-(int)(LWPosition*100));
	PosPIDTune();					//nastav parametry PID v zavislosti na zone, ve ktere se bot nachazi
	PID_PP=(PosErr*PP_Const)/1000;
	//PID_DP=((PosErr-LWPosErrLast)*DP_Const)/10;
	PID_DP=(((int)(LWPosition*100)-(int)(LWPosLast*100))*DP_Const)/10;
	PID_IPL += ((IP_Const*PosErr)/100);
	PID_IPL=constrainInt(PID_IPL,-1000*MaxAngleWanted,1000*MaxAngleWanted);
	LWAngleWanted=(constrainInt((PID_PP - PID_DP + PID_IPL/1000), -MaxAngleWanted, MaxAngleWanted));
	LWPosErrLast=PosErr;
	LWPosLast=LWPosition;
}

void CalcRWPosPID()
{
	//LowPassFilter(LWpocitadloKroku,LWpocitadloKrokuLast,0.8);
	PosErr=(RWPosWanted-(int)(RWPosition*100));
		PosPIDTune();					//nastav parametry PID v zavislosti na zone, ve ktere se bot nachazi
		PID_PP=(PosErr*PP_Const)/1000;
		PID_DP=((PosErr-RWPosErrLast)*DP_Const)/10;
		PID_IPR += ((IP_Const*PosErr)/100);
		PID_IPR=constrainInt(PID_IPR,-10000000,10000000);
		RWAngleWanted=(constrainInt((PID_PP + PID_DP + PID_IPR/1000), -MaxAngleWanted, MaxAngleWanted));
		RWPosErrLast=PosErr;
	//LWPositionLast=LWPosition;
}

void CalcSpeed()
{
	//LWPosition=LWpocitadloKroku;
	if(LowPassEnabled)
	{
		LWPosition=LowPassFilter((float)LWpocitadloKroku,LWPositionLast,PosFilterConst);
	}
	else LWPosition=LWpocitadloKroku;
	LWSpeed=LWPosition-LWPositionLast;
	LWPositionLast=LWPosition;
	RWPosition=LowPassFilter((float)RWpocitadloKroku,RWPositionLast,PosFilterConst);
	RWSpeed=RWPosition-RWPositionLast;
	RWPositionLast=RWPosition;
	/*SpeedPocitadlo++;
	if(SpeedPocitadlo==10) 	//updatuje rychlost kazdych 100ms
	{
		LWPosition=LWpocitadloKroku;
		SpeedPocitadlo=0;
		LWSpeed=LWPosition-LWPositionLast;
		
	//RWSpeed=RWpocitadloKroku-RWpocitadloKrokuLast;
	}*/
}

void CalcSpeedPID()
{
	LWSpeedErr=LWSpeedWanted-LWSpeed;
		PID_PW=LWSpeedErr*PW_Const;
		PID_DW=(LWSpeedErr-LWSpeedErrLast)*DW_Const;
		PID_IW += (IW_Const*LWSpeedErr);
		PID_IW=constrainFloat(PID_IW,-10000,10000);
		LWAngleWanted=constrainFloat((PID_PW + PID_DW + PID_IW), -10000, 10000);
		LWSpeedErrLast=LWSpeedErr;
//	LWSpeedErr=LWSpeedWanted-LWSpeed;
//	PID_PW=LWSpeedErr*PW_Const;
//	PID_DW=(LWSpeedErr-LWSpeedErrLast)*DW_Const;
//	PID_IW += (IW_Const*LWSpeedErr);
//	PID_IW=constrainFloat(PID_IW,-10,10);
//	LWAngleWanted=constrainFloat((PID_PW + PID_DW + PID_IW)*1000, -10000, 10000);
//	LWSpeedErrLast=LWSpeedErr;
}

void CalcAnglePID(int Angle)
{
	if(Movement==Right && MovementLast==Right)
	{
		AngleErr = LWAngleWanted - Angle;
		PID_D= Dconst*(Angle - LWAngleLast);
		LWAngleLast=Angle;
		LWAngleErrLast = AngleErr;
	}
	else if(Movement==Left && MovementLast==Left)
		{
			AngleErr =RWAngleWanted - Angle;
			PID_D= Dconst*(Angle - RWAngleLast);
			RWAngleLast=Angle;
			RWAngleErrLast = AngleErr;
		}
	else if(Movement==Balance || Movement==Left || Movement==Right)
	{
		AngleErr = LWAngleWanted - Angle;
		PID_D= Dconst*(Angle - LWAngleLast);
		LWAngleLast=Angle;
		LWAngleErrLast = AngleErr;
	}
	else if(Movement==Brake)
	{
		AngleErr = LWAngleWanted - Angle;
				PID_D= Dconst*(Angle - LWAngleLast);
				LWAngleLast=Angle;
				LWAngleErrLast = AngleErr;
	}
	else if(Movement==Forward)
	{
		AngleErr = LWAngleWanted- Angle;
		PID_D= Dconst*(Angle - LWAngleLast);
		LWAngleLast=Angle;
		LWAngleErrLast = AngleErr;
	}
	else if(Movement==Backward)
	 {
		AngleErr = LWAngleWanted - Angle;
				PID_D= Dconst*(Angle - LWAngleLast);
				LWAngleLast=Angle;
				LWAngleErrLast = AngleErr;
	 }
	PID_P = Pconst * AngleErr;
	PID_I += Iconst * AngleErr;
	PID_I=constrainInt(PID_I,PID_OUT_MIN*1000,PID_OUT_MAX*1000);
	//PID_D = (float)Dconst * (AngleErr - AngleErrLast);

	PID = constrainInt((-(PID_P + PID_I - PID_D)/1000),PID_OUT_MIN,PID_OUT_MAX);
	PID_OUT=Abs(PID);			/* pro motory posilame pouze kladne PID*/
	if(PID>0) /*meni smer otaceni motoru*/
	{
		if(Movement==Right)
		{
			IN1A_PutVal(TRUE);
			IN2A_PutVal(FALSE);
			IN1B_PutVal(TRUE);
			IN2B_PutVal(TRUE);
		}
		else if(Movement==Left)
				{
					IN1A_PutVal(TRUE);
					IN2A_PutVal(TRUE);
					IN1B_PutVal(TRUE);
					IN2B_PutVal(FALSE);
				}
		else if(Movement==Balance || Movement==Forward || Movement==Brake || Movement==Backward)
				{
					IN1A_PutVal(TRUE);
					IN2A_PutVal(FALSE);
					IN1B_PutVal(TRUE);
					IN2B_PutVal(FALSE);
				}
	}
	else if(PID<0) /*meni smer otaceni motoru*/
	{
		if(Movement==Right)
		{
			IN1A_PutVal(FALSE);
			IN2A_PutVal(TRUE);
			IN1B_PutVal(TRUE);
			IN2B_PutVal(TRUE);
		}
		else if(Movement==Left)
			{
				IN1A_PutVal(TRUE);
				IN2A_PutVal(TRUE);
				IN1B_PutVal(FALSE);
				IN2B_PutVal(TRUE);
			}
		else if(Movement==Balance|| Movement==Forward || Movement==Brake || Movement==Backward)
		{
			IN1A_PutVal(FALSE);
			IN2A_PutVal(TRUE);
			IN1B_PutVal(FALSE);
			IN2B_PutVal(TRUE);
		}
	}
	PWM_Value=(PID_OUT_MAX-PID_OUT); /* aby rozdil byl maximalne nula + povolene PWM kvuli vysokemu napeti baterky*/
	if(AngleErr<30000 && AngleErr>-30000)
	{
		if(Movement==Right)
		{
			SetLWPWM();
		}
		else if(Movement==Left)
		{
			SetRWPWM();
		}
		else if(Movement==Balance || Movement==Forward || Movement==Backward || Movement==Brake)
		{
			SetLWPWM();
			SetRWPWM();
		}
	}
	else 
	{
		StopMotors();
	}
 }

void CalcLWAnglePID(int Angle)
{
	AngleErr = LWAngleWanted - Angle;
		PID_P = Pconst * AngleErr;
		PID_IL += Iconst * AngleErr;
		PID_IL=constrainInt(PID_IL,PID_OUT_MIN*1000,PID_OUT_MAX*1000);
		//PID_D = (float)Dconst * (AngleErr - AngleErrLast);
		PID_D= Dconst*(Angle - LWAngleLast);
		LWAngleLast=Angle;
		LWAngleErrLast = AngleErr;
		PID = constrainInt((-(PID_P + PID_IL - PID_D)/1000),PID_OUT_MIN,PID_OUT_MAX);
		PID_OUT=Abs(PID);			/* pro motory posilame pouze kladne PID*/
		if(PID>0) /*meni smer otaceni motoru*/
		{
			IN1A_PutVal(TRUE);
			IN2A_PutVal(FALSE);
			//IN1B_PutVal(TRUE);
			//IN2B_PutVal(FALSE);
		}
		else if(PID<0) /*meni smer otaceni motoru*/
		{
			IN1A_PutVal(FALSE);
			IN2A_PutVal(TRUE);
			//IN1B_PutVal(FALSE);
			//IN2B_PutVal(TRUE);
		}
		PWM_Value=(PID_OUT_MAX-PID_OUT); /* aby rozdil byl maximalne nula + povolene PWM kvuli vysokemu napeti baterky*/
		if(AngleErr<30000 && AngleErr>-30000)
		{
			SetLWPWM();
		}
		else 
		{
			StopMotors();
		}
 }

void CalcRWAnglePID(float Angle)
{
		AngleErr = RWAngleWanted - Angle;
		PID_P = Pconst * AngleErr;
		PID_IR += Iconst * AngleErr;
		PID_IR=constrainInt(PID_IR,PID_OUT_MIN*1000,PID_OUT_MAX*1000);
		//PID_D = (float)Dconst * (AngleErr - AngleErrLast);
		PID_D= Dconst*(Angle - RWAngleLast);
		RWAngleLast=Angle;
		RWAngleErrLast = AngleErr;
		PID = constrainInt((-(PID_P + PID_IR - PID_D)/1000),PID_OUT_MIN,PID_OUT_MAX);
		PID_OUT=Abs(PID);			/* pro motory posilame pouze kladne PID*/
		if(PID>0) /*meni smer otaceni motoru*/
		{
			//IN1A_PutVal(TRUE);
			//IN2A_PutVal(FALSE);
			IN1B_PutVal(TRUE);
			IN2B_PutVal(FALSE);
		}
		else if(PID<0) /*meni smer otaceni motoru*/
		{
			//IN1A_PutVal(FALSE);
			//IN2A_PutVal(TRUE);
			IN1B_PutVal(FALSE);
			IN2B_PutVal(TRUE);
		}
		PWM_Value=(PID_OUT_MAX-PID_OUT); /* aby rozdil byl maximalne nula + povolene PWM kvuli vysokemu napeti baterky*/
		if(AngleErr<30000 && AngleErr>-30000)
		{
			SetRWPWM();
		}
		else 
		{
			StopMotors();
		}
 }

void CalibrateSensors()
	{
	CalibrationPocitadlo++;
	FlagADC1Dokonceno=FALSE;
	AD2_GetValue(&pole[0]);
	GyroSuma+=pole[0];
	if(CalibrationPocitadlo==100)
		{
			GyroZero=(float)GyroSuma/100;
			GyroADCLast=GyroZero;
			SensorsInitialized=TRUE;
		}
	FlagMereniDokonceno=TRUE;
	}

/******************************************************************************/
void CalcAngleOffset()
{
	PIDSum+=PID; //tuni uhlovej offset podle toho, v jakem smeru ma robot vic akcnich zasahu
			  if(Abs(PIDSum)>PIDTreshold)
			  {
				  if(PIDSum>PIDTreshold)AngleOffset++;
				  else AngleOffset--;
				  PIDSum=0;
			  }
}

void GetAccel()
{
	MMA845X_Poll(); 
	MappedAngleX=map(mma845x_last)+AngleOffset;
	//MappedAngleX=LowPassFilter((float)MappedAngleX,(float)AngleXLast,0.7);
	AngleXLast=MappedAngleX;
}
/*lint -save  -e970 Disable MISRA rule (6.3) checking. */
int main(void)
/*lint -restore Enable MISRA rule (6.3) checking. */
{
  /* Write your local variable definition here */
	
	
  /*** Processor Expert internal initialization. DON'T REMOVE THIS CODE!!! ***/
  PE_low_level_init();
  /* INICIALIZACE LDD DEVICES*/
  MyI2CPtr = I2C2_Init(NULL);
  //PWM_ENA_Pointer = PWM_ENA_Init(NULL);
  //PWM_ENB_Pointer = PWM_ENB_Init(NULL);
  testDevData = BT_Init(NULL);
  MMA845X_Init();
  //IRSensorMX_PutVal(senzor1);
  /****NASTAVENI PID*****/
  Pconst=2500;
  Iconst=300;
  Dconst=2000;
  a=15;
  b=6;
  c=15;
  /* inicializace moving average pole*/
  AverageAngleArrInit();
  Movement=Balance;
  /****VYZADAME SI PARAMETR PO BT*****/
  BT_SendBlock(testDevData, (LDD_TData*)&messageParametr, sizeof(messageParametr)); /* send block of data */
  if(blockSent)blockSent=FALSE;
  /***********************************/
  
  for(;;) 
  {

	  //IRSensorMX_PutVal(senzor1);
	  BluetoothReceive();			/*kontrolujeme zda nam uzivatel nepise pro zmenu parametru*/
	  if(FlagTI1Int && FlagMereniDokonceno)
	  {
		  FlagMereniDokonceno=FALSE;
		  MMA845X_Poll();
		  Timer_Interrupt_CB();
		  AD2_Measure(TRUE);
		  FlagTI1Int=FALSE;
	  }
			    
	  if(FlagADC1Dokonceno)
	  {
		if(!SensorsInitialized)CalibrateSensors();
		else{
		  FlagADC1Dokonceno=FALSE;
		  AD2_GetValue(&pole[0]);
		  GetAccel();
		  GetGyro();
		  KalmanAngle=KalmanFiltCalc(((float)MappedAngleX)/100,dps2);
		  if(AverageAngleArrSize>1)AveragedKalmanAngle=GetKalmanAverage(); else AveragedKalmanAngle=KalmanAngle;
		  KalmanAngleInt=(int)(AveragedKalmanAngle*1000);
		  if(MotorsEnabled) 
		  {
			  CalcSpeed(); //spocitej rychlost kazdych 100ms
			  if(Movement==Balance)
			  {
				  CalcLWPosPID();	//zapina PID na polohu, kdyz balancuje
				  CalcRWPosPID();	//zapina PID na polohu, kdyz balancuje
			  }
			  else if(Movement==Brake) //pokud jede nebo brzdi, pocitej PID na rychlost
			  {
					  if(Movement==Brake && LWSpeed<=BrakeSpeed)
					  {
						Movement=Balance;
						MovementLast=Balance;
						LWPosWanted=(int)(LWPosition*100);
					  }
					  else if(Movement==Brake && LWSpeed>BrakeSpeed)
					  {
						  LWSpeedWanted=0;
						  CalcSpeedPID();
					  }
			  }
			  else if(Movement==Forward)
			  {
				  if(MovementLast==Balance)
				  {
					  PocitadloJizdy=0;
					  PID_IW=0;
					  PosFilterConst=0.9;
					  //LowPassEnabled=FALSE;
					  MovementLast=Forward;
				  }
				  else if(MovementLast==Forward)
				  {
					  PocitadloJizdy++;
					  if(PocitadloJizdy==500)
					  {	
						  PocitadloJizdy=0;
						  PosFilterConst=0.9;
						  zobrazeniPrubehu=FALSE;
						  LowPassEnabled=TRUE;
						  Movement=Brake;
						  MovementLast=Forward;
					  }
				  }
				  CalcSpeedPID();
			  }
			  else if(Movement==Backward)
			  			  {
			  				  if(MovementLast==Balance)
			  				  {
			  					  PocitadloJizdy=0;
			  					  PID_IW=0;
			  					  PosFilterConst=0.9;
			  					//LowPassEnabled=FALSE;
			  					  MovementLast=Backward;
			  				  }
			  				  else if(MovementLast==Backward)
			  				  {
			  					  PocitadloJizdy++;
			  					  if(PocitadloJizdy==500)
			  					  {	
			  						  PocitadloJizdy=0;
			  						  PosFilterConst=0.9;
			  						  zobrazeniPrubehu=FALSE;
			  						LowPassEnabled=TRUE;
			  						  Movement=Brake;
			  						  MovementLast=Backward;
			  					  }
			  				  }
			  				  CalcSpeedPID();
			  			  }
			  else if(Movement==Right)
			  {
						  if(MovementLast==Balance)
						  {
									ZoneEnabled=FALSE;
									MaxAngleWanted=3000;
									LWPosWanted=(int)(LWPosition*100)-43000;
									aLast=a;
									a=10;
									bLast=b;
									b=15;
									cLast=c;
									c=0;
									Pconst*=2;
									Iconst*=4;
									Dconst*=3;
									MovementLast=Right;
						  }
						  else if(((LWPosition*100)-LWPosWanted)<0 && LWSpeed <= 10)
						  {
									  Movement=Balance;
									  LWPosWanted=LWPosition*100;
									  MaxAngleWanted=10000;
									  //PID_IPL=0;
									  a=aLast;
									  b=bLast;
									  c=cLast;
									  PID_I=0;
									  //PID_IPL=0;
									  Pconst/=2;
									  Iconst/=4;
									  Dconst/=3;
									  ZoneEnabled=TRUE;
						  }
						  else
						  {
							b=Abs(((int)(LWPosition*100)-LWPosWanted)/1400);  
							if(b<3)b=3;
							c=Abs((43000 - Abs(((int)(LWPosition*100)-LWPosWanted)))/500);
						  }
						  CalcLWPosPID();
			  }
			  else if(Movement==Left)
			  {
							if(MovementLast==Balance)
							{
								if(RWSpeed >= (-1))
								{
									ZoneEnabled=FALSE;
									MaxAngleWanted=3000;
									RWPosWanted=(int)(RWPosition*100)-43000;
									aLast=a;
									a=10;
									bLast=b;
									b=15;
									cLast=c;
									c=0;
									Pconst*=2;
									Iconst*=4;
									Dconst*=3;
									MovementLast=Left;
								}
								else CalcLWPosPID();
							}
							else if(((RWPosition*100)-RWPosWanted)<0 && RWSpeed <= 10)
							 {
									Movement=Balance;
									LWPosWanted=LWPosition*100;
									MaxAngleWanted=10000;
									//PID_IPL=0;
									a=aLast;
									b=bLast;
									c=cLast;
									PID_I=0;
									//PID_IPR=0;
									Pconst/=2;
									Iconst/=4;
									Dconst/=3;
									ZoneEnabled=TRUE;
									CalcRWPosPID();
							}
							else
							{
								b=Abs(((int)(RWPosition*100)-RWPosWanted)/1400);  
								if(b<3)b=3;
								c=Abs((43000 - Abs(((int)(RWPosition*100)-RWPosWanted)))/500);
								CalcRWPosPID();
							}
			  	}
			  	CalcAnglePID(KalmanAngleInt);

		  }
		  else StopMotors();
		  
		  //if(Movement==Balance && Abs(AveragedKalmanAngle)<3)CalcAngleOffset(); //postupne vylepsuje uhlovej offset 
		  FlagMereniDokonceno=TRUE;
		  if(zobrazeniPrubehu)ZobrazPrubeh();
		  TI1casovac=0;
		}
	  }
  }

  /*** Don't write any code pass this line, or it will be deleted during code generation. ***/
  /*** RTOS startup code. Macro PEX_RTOS_START is defined by the RTOS component. DON'T MODIFY THIS CODE!!! ***/
  #ifdef PEX_RTOS_START
    PEX_RTOS_START();                  /* Startup of the selected RTOS. Macro is defined by the RTOS component. */
  #endif
  /*** End of RTOS startup code.  ***/
  /*** Processor Expert end of main routine. DON'T MODIFY THIS CODE!!! ***/
  for(;;){}
  /*** Processor Expert end of main routine. DON'T WRITE CODE BELOW!!! ***/
} /*** End of main routine. DO NOT MODIFY THIS TEXT!!! ***/

/* END ProcessorExpert */
/*
** ###################################################################
**
**     This file was created by Processor Expert 10.0 [05.03]
**     for the Freescale Kinetis series of microcontrollers.
**
** ###################################################################
*/
