#include <avr/io.h>
#include <util/delay.h>
#include <string.h>
#include <stdlib.h>
#include <avr/interrupt.h>
#include <stdint.h>
#include <stdio.h>
#include <math.h>
#include <inttypes.h>


#define MPU9250_ADDRESS 0x68 
#define TWI_SLAW 0xD0 //SLAVE ADRESS WRITE 
#define TWI_SLAR 0xD1 //SLAVE ADRESS READ

#define    MAG_ADDRESS_W	0x18
#define    MAG_ADDRESS_R	0x19

#define SELF_TEST_X_GYRO 0x00 // adresy psány v HEXA
#define SELF_TEST_Y_GYRO 0x01
#define SELF_TEST_Z_GYRO 0x02
#define SELF_TEST_X_ACCEL 0x0D
#define SELF_TEST_Y_ACCEL 0x0E
#define SELF_TEST_Z_ACCEL 0x0F
#define XG_OFFSET_H 0x13
#define XG_OFFSET_L 0x14
#define YG_OFFSET_H 0x15
#define YG_OFFSET_L 0x16
#define ZG_OFFSET_H 0x17
#define ZG_OFFSET_L 0x18
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_CONFIG_2 0x1D
#define LP_ACCEL_ODR 0x1E
#define WOM_THR 0x1F // v jiných registrech a volných knihovnách označený jako MOT_THR

#define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
#define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]
#define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms

#define FIFO_EN 0x23
#define I2C_MST_CTRL 0x24
#define I2C_SLV0_ADDR 0x25
#define I2C_SLV0_REG 0x26
#define I2C_SLV0_CTRL 0x27
#define I2C_SLV1_ADDR 0x28
#define I2C_SLV1_REG 0x29
#define I2C_SLV1_CTRL 0x2A
#define I2C_SLV2_ADDR 0x2B
#define I2C_SLV2_REG 0x2C
#define I2C_SLV2_CTRL 0x2D
#define I2C_SLV3_ADDR 0x2E
#define I2C_SLV3_REG 0x2F
#define I2C_SLV3_CTRL 0x30
#define I2C_SLV4_ADDR 0x31
#define I2C_SLV4_REG 0x32
#define I2C_SLV4_DO 0x33
#define I2C_SLV4_CTRL 0x34

#define I2C_SLV4_DI 0x35
#define I2C_MST_STATUS 0x36
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define INT_STATUS 0x3A
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define EXT_SENS_DATA_00 0x49
#define EXT_SENS_DATA_01 0x4A
#define EXT_SENS_DATA_02 0x4B
#define EXT_SENS_DATA_03 0x4C
#define EXT_SENS_DATA_04 0x4D
#define EXT_SENS_DATA_05 0x4E
#define EXT_SENS_DATA_06 0x4F
#define EXT_SENS_DATA_07 0x50
#define EXT_SENS_DATA_08 0x51
#define EXT_SENS_DATA_09 0x52
#define EXT_SENS_DATA_10 0x53
#define EXT_SENS_DATA_11 0x54
#define EXT_SENS_DATA_12 0x55
#define EXT_SENS_DATA_13 0x56
#define EXT_SENS_DATA_14 0x57
#define EXT_SENS_DATA_15 0x58
#define EXT_SENS_DATA_16 0x59
#define EXT_SENS_DATA_17 0x5A
#define EXT_SENS_DATA_18 0x5B
#define EXT_SENS_DATA_19 0x5C
#define EXT_SENS_DATA_20 0x5D
#define EXT_SENS_DATA_21 0x5E
#define EXT_SENS_DATA_22 0x5F
#define EXT_SENS_DATA_23 0x60
#define I2C_SLV0_DO 0x63
#define I2C_SLV1_DO 0x64
#define I2C_SLV2_DO 0x65
#define I2C_SLV3_DO 0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET 0x68
#define MOT_DETECT_CTRL 0x69
#define USER_CTRL 0x6A
#define PWR_MGMT_1 0x6B
#define PWR_MGMT_2 0x6C
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define WHO_AM_I 0x75
#define XA_OFFSET_H 0x77
#define XA_OFFSET_L 0x78
#define YA_OFFSET_H 0x7A
#define YA_OFFSET_L 0x7B
#define ZA_OFFSET_H 0x7D
#define ZA_OFFSET_L 0x7E


#define    GYRO_FULL_SCALE_250_DPS    0x00  
#define    GYRO_FULL_SCALE_500_DPS    0x08
#define    GYRO_FULL_SCALE_1000_DPS   0x10
#define    GYRO_FULL_SCALE_2000_DPS   0x18

#define    ACC_FULL_SCALE_2_G        0x00  
#define    ACC_FULL_SCALE_4_G        0x08
#define    ACC_FULL_SCALE_8_G        0x10
#define    ACC_FULL_SCALE_16_G       0x18


	int16_t preruseni=0;
	int8_t citac_povoleni=1;
	int8_t pocet_cyklu=0;
	int16_t  gsx;
	int16_t  gsy;
	int16_t  gsz;
	int16_t ax;
	int16_t ay;
	int16_t az;
	int16_t arx;
	int16_t ary;
	int16_t arz;
	int16_t gyroScale = 33;
	int16_t grx;
	int16_t gry;
	int16_t grz;
	int16_t gx;
	int16_t	gy;
	int16_t gz;	
	int16_t rx;	
	int16_t ry;
	int16_t rz;
	int16_t time;
	int16_t mx;
	int16_t my;
	int16_t mz;
	int16_t magneto_mereni;
	int16_t mag_uhel1;
	int16_t mag_uhel2;
	int16_t mag_uhel3;
	int16_t scan=0;
	int16_t zatacka=0;
	int16_t stav=0;
void init_i2c(void)
{
	TWSR = 0x00;
	TWBR = 32;
	//twbr 72 pro 100K - 19 pro 300K 32 pro 200K	
	//TWCR |= (1<<TWEN);
	//(((F_CPU/i2c_sck)-16)/(2*4^0/*<-TWPS*/));

}
int i2c_read_ack(void)
{
	TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWEA);
    while ((TWCR & (1<<TWINT)) == 0);
    return TWDR;
}

int i2c_read_nack(void)
{
	TWCR = (1<<TWINT)|(1<<TWEN);
	while ((TWCR & (1<<TWINT)) == 0);
    return TWDR;
}



void i2c_start(void){
	//Send start
	TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
	//čeká dokončení start
	while(!(TWCR & (1<<TWINT)));
	//if((TWSR & 0xF8) != START) i2c_error();
}

void i2c_send_address(char address)
{
	//načtení adresy 
	TWDR = address;
	//Start 
	TWCR = (1<<TWINT)|(1<<TWEN);
	//čeká ACK
	while(!(TWCR & (1<<TWINT)));
	//if((TWSR & 0xF8) != MT_SLA_ACK) i2c_error();
}

void i2c_send_data(char data)
{
	//načtení dat
	TWDR = data;
	//Start 
	TWCR = (1<<TWINT)|(1<<TWEN);
	//čeká ACK nebo NACK 
	while (!(TWCR & (1<<TWINT)));
	//if((TWSR & 0xF8) != MT_DATA_ACK) i2c_error();
}

void i2c_stop(void)
{
	TWCR = (1<<TWINT)|(1<<TWEN)|(1<<TWSTO);
}

void i2c_error(void)
{
	
}

void LDByteWriteI2C(int address, int reg_number,int data)
{

	
	i2c_start();
	i2c_send_address(address);
	// číslo registru pro zápis
	i2c_send_data(reg_number);
	//DATA
	i2c_send_data(data);
	i2c_stop();

}
int LDByteReadI2C(int write,int read, int reg_number)
{
	// START, 0xd0, REGISTER NUMBER, START, 0xd1, REGISTER DATA, STOP
	int data;

	i2c_start();	
	i2c_send_address(write);
	// číslo registru pro zápis
	i2c_send_data(reg_number);
	i2c_stop();	
	i2c_start();
	i2c_send_address(read);
	data = i2c_read_nack();
	i2c_stop();

	return data;
}
// přijme char 
void uart_vysilani( char data )
{	
	// čeka dokud není buffer prázdný 
	while ( ! ( UCSR1A & ( 1 << UDRE1 ) ) );    
	// vloží data do registru a odešle 
	UDR1 = data;
}


unsigned char uart_prijem(void)
{
	while (!( UCSR1A & ( 1 << RXC1) ))
		;
	return UDR1;
}

// init uart
void uart_init(void)
{ 
	// UBRR1H=(BAUDRATE>>8);
	// UBRR1L=BAUDRATE;    
	UBRR1H = 0;
	UBRR1L = 51;//103 pro 9600 51 pro 19200
	// povolení vysílání a příjmu
	UCSR1B |= ( 1 << RXEN1 ) | ( 1 << TXEN1 );
	// nastaví format frame, 8 bitů data a 1 stop bity 
	UCSR1C = 6;

}
// zkontroluje zda jsou nějaká data ke čtení 
int uart_dataAvailable(void)
{
	if ( UCSR1A & ( 1 << RXC1) )
		return 1;
    
		return 0;
}

void Setup_MPU9250(void)
{

	
	LDByteWriteI2C(TWI_SLAW,ACCEL_CONFIG_2, 0x06);  

	LDByteWriteI2C(TWI_SLAW, CONFIG, 0x06);

	
	LDByteWriteI2C(TWI_SLAW,GYRO_CONFIG, GYRO_FULL_SCALE_1000_DPS);	 

	LDByteWriteI2C(TWI_SLAW, ACCEL_CONFIG, ACC_FULL_SCALE_4_G); 

	LDByteWriteI2C(TWI_SLAW,INT_PIN_CFG,0x02);

	LDByteWriteI2C(MAG_ADDRESS_W, 0x0A,0x16);
}

void Get_Values(void)
{		
	uint8_t accel[6];
	uint8_t gyrro[6];
   	//uint8_t temp[2];

	accel[0]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, ACCEL_XOUT_H);
	accel[1]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, ACCEL_XOUT_L);
		
	accel[2]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, ACCEL_YOUT_H);
	accel[3]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, ACCEL_YOUT_L);
	
	accel[4]=LDByteReadI2C(TWI_SLAW,TWI_SLAR,ACCEL_ZOUT_H);
	accel[5]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, ACCEL_ZOUT_L);

	gyrro[0]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, GYRO_XOUT_H);
	gyrro[1]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, GYRO_XOUT_L);
	
	//gyrro[2]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, GYRO_YOUT_H);
	//gyrro[3]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, GYRO_YOUT_L);
	
	//gyrro[4]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, GYRO_ZOUT_H);
	//gyrro[5]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, GYRO_ZOUT_L);	
	
	ax = -(accel[0]<<8|accel[1]);
	ay = -(accel[2]<<8|accel[3]);
	az =  (accel[4]<<8|accel[5]);
	
	gx = -(gyrro[0]<<8|gyrro[1]);
	//gy = -(gyrro[2]<<8|gyrro[3]);
	//gz =  (gyrro[4]<<8|gyrro[5]);	

	//arx = (180/3.141592) * atan(ax / sqrt(square(ay) + square(az))); 
	//ary = (180/3.141592) * atan(ay / sqrt(square(ax) + square(az)));
	arx = (180/3.141592) * atan(ax / (sqrt(square(ay) + square(az)))); 
	ary = (180/3.141592) * atan(ay / (sqrt(square(ax) + square(az))));
	arz = (180/3.141592) * atan((sqrt(square(ay) + square(ax))) / az);

	gsx = gx/gyroScale; 
	//gsz = gz/gyroScale;  
	//gsy = gy/gyroScale;   
	//gsz = gz/gyroScale;
	if (citac_povoleni == 1) 
	{
		grx = arx;
		//gry = ary;
		//grz = arz;
	}
	else
	{
		time=pocet_cyklu;	
		grx = grx + (gsx*time);  
 		//gry = gry + (gsy*time);
  		//grz = grz + (gsz*time);
  		pocet_cyklu=0;
	}  
	rx = (0.96 * arx) + (0.04 * grx);
	//ry = (0.96 * ary) + (0.04 * gry);
	//rz = (0.96 * arz) + (0.04 * grz);
	char data[15];	
	int16_t odesli= rx;
	sprintf(data,"%d",odesli);			
	int number_data = 0;
	for ( number_data = 0; number_data < strlen(data); number_data++ )
		{
			uart_vysilani(data[number_data]);			
		}
	uart_vysilani('\n');	
}

void Get_magneto(void)
{	char data[15];
	uint8_t ST1;
	uint8_t magneto[6];

	do
	{
		ST1=LDByteReadI2C(MAG_ADDRESS_W, MAG_ADDRESS_R, 0x02); 

	}
	while (!(ST1&0x01));
		
	i2c_start();
	i2c_send_address(MAG_ADDRESS_W);
	i2c_send_data(0x03);	
	i2c_stop();
	i2c_start();	
	i2c_send_address(MAG_ADDRESS_R);
	
	magneto[0] = i2c_read_ack();
	magneto[1] = i2c_read_ack();

	magneto[2] = i2c_read_ack();	
	magneto[3] = i2c_read_ack();

	magneto[4] = i2c_read_ack();
	magneto[5] = i2c_read_ack();
	i2c_read_nack();
	i2c_stop();	

	int16_t mx=-(magneto[3]<<8 | magneto[2]);
	int16_t my=-(magneto[1]<<8 | magneto[0]);
	int16_t mz=-(magneto[5]<<8 | magneto[4]);	
}

//funkce pro zapsani celeho stringu 
void uart_string( char data[] )
{
	int c = 0;
	for ( c = 0; c < strlen(data); c++ )
		uart_vysilani(data[c]);
}
//čítač časovač 3 - na prvním mám PWM, tak se nedá nic dělat
// dělička 64 -CTC - 100ms
void timer3_init(void)
{
	TCCR3B |= (1 << WGM32)|(1 << CS30)|(1 << CS32);
  // dělička 1028
	TCNT3 = 0;
  
	OCR3A = 15624;
	// 200ms 49998 -24999 pro 100ms
	TIMSK3 |= (1 << OCIE3A);  	
}
// inicializace PWM
void pwm_init(void)
{
	// inicializace pwm ve fast mode, 8 bit. 
	TCCR1A |= (1<<COM1A1)|(1<<COM1B1)|(1<<COM1C0)|(1<<WGM10);
	// CS11 a CS10 nastavené pro děličku  
	TCCR1B |= (1<<WGM12)|(1<<CS11)|(1<<CS10);
	// OCR1A a OCR1B - hodnoty pro porovnání - řízení střídy pwm (duty cycle) 
	OCR1A=60;
	OCR1B=60;  
}

// střída pro levý motor 0...255
void pwm_levy(int value)
{
	OCR1A=value;
}

// střída pro pravý motor 0...255
 void pwm_pravy(int value)
{
	OCR1B=value;
}

// inicializace adc
void adc_init(void)
{   
	// výběr kanálu - pro první sharp je to ADMUX = 0 , pro druhý je to ADMUX=2 a ACSCRB |=(1<<MUX5) 
	ADMUX=0;
	// aden - povolení adc , nastavení děličky 
	// 16000000/128 = 125000 frekvence 
	ADCSRA |= (1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
	//ADCSRB |=(1<<MUX5); 
}
   
unsigned int adc_cteni(void)
{	ADMUX=0;
	// Start
	ADCSRA |= (1<<ADSC); 
	// čekani na dokončení převodu
	while (ADCSRA & (1<<ADSC));  
	return (ADC);
}

void AI(int cislo)
{
	int operace = cislo;
	// čísla operací 101 - povolení jízdy - 404 - total stop - 303- překažka zkusit objet 
	switch (operace)
	{
  		case 101:
			PORTF |= (1<<PF5)|(1<<PF7);
			PORTF &= ~((1 << PF4) | (1 << PF6));
			pwm_pravy(255);	
			pwm_levy(255);
		break;		
		case 202: // zpátečka
			PORTF |= (1<<PF4)|(1<<PF6);
			PORTF &= ~((1 << PF5) | (1 << PF7));
			pwm_pravy(255);	
			pwm_levy(255);
		break;	
		case 303:
			// otáčí se do leva 
			PORTF &= ~((1 << PF5)| (1 << PF6));
			PORTF |= (1<<PF4)| (1 << PF7);		
			// první se provede co nejrychlejší reakce na zpomalení, poté se přehodí a začne otáčet
			pwm_pravy(255);	
			pwm_levy(255);
		break;

		case 404: 
			// zkouška - zastaví
			PORTF &= ~((1 << PF4)|(1<< PF5)|(1 << PF6)|(1 << PF7));			
		break;
 		
		case 505: 
			// otáčí se do prava			
			PORTF &= ~((1 << PF4)| (1 << PF7));
			PORTF |= (1<<PF5)| (1 << PF6);
						
			pwm_pravy(255);	
			pwm_levy(255);			
		break;
		default:
		break;

	}
// pokud nastane situace, že je robot 1 cm od překážky, tak se prostě na základě série pokusů zkusí otočit etc. to vše ve funkci AI 
}

// určí jaká akce nastane podle přijatých dat
int commander( unsigned char string)
{   
	unsigned char operace = string;	
	int povoleni;
	// příkazy sloužící pro 1, - povolení jízdy 2 - zastavení jízdy 		
	switch (operace)
	{
  		case 49: // dopředu, povolit AI, zakazat scan
			povoleni=1; //pravy je PF 6 a PF7, S3 
			PORTF |= (1<<PF5)|(1<<PF7);
			PORTF &= ~((1 << PF4) | (1 << PF6));
			scan=0;			
			pwm_pravy(255);	
			pwm_levy(255);
		break;
		case 54: // dopředu, povolit AI, povolit scan
			povoleni=1; //pravy je PF 6 a PF7, S3 
			PORTF |= (1<<PF5)|(1<<PF7);
			PORTF &= ~((1 << PF4) | (1 << PF6));
			scan=1;			
			pwm_pravy(255);	
			pwm_levy(255);
		break;
		case 53: // dopředu bez AI
			povoleni=0; //pravy je PF 6 a PF7, S3 
			PORTF |= (1<<PF5)|(1<<PF7);
			PORTF &= ~((1 << PF4) | (1 << PF6));
			pwm_pravy(255);	
			pwm_levy(255);	
		break;		
			
		case 48: //zastavit, bez AI
			povoleni=0;
			// nastavení 0 nebo 1 do všech PF4-PF7 se aktivuje brzda. 
			PORTF &= ~((1 << PF4)|(1<< PF5)|(1 << PF6)|(1 << PF7));
			pwm_pravy(0);	
			pwm_levy(0);
		break;
		
		case 50: //otačení bez AI
			
			// nastavení 0 nebo 1 do všech PF4-PF7 se aktivuje brzda. 
	        povoleni=0; //pravy je PF 6 a PF7, S3 
			PORTF |= (1<<PF5)|(1<<PF7);
			PORTF &= ~((1 << PF4) | (1 << PF6));
			pwm_pravy(255);	
			pwm_levy(60);	
		break;

		case 51: // otačení bez AI
			povoleni=0;
			// nastavení 0 nebo 1 do všech PF4-PF7 se aktivuje brzda. 
			PORTF &= ~((1 << PF4)| (1 << PF7));
			PORTF |= (1<<PF5)| (1 << PF6);		
			pwm_pravy(255);	
			pwm_levy(255);
		break;
		
		case 57: // zpátečka bez AI 
			povoleni=0;
			// nastavení 0 nebo 1 do všech PF4-PF7 se aktivuje brzda. 
			PORTF |= (1<<PF4)|(1<<PF6);
			PORTF &= ~((1 << PF5) | (1 << PF7));	
			pwm_pravy(255);	
			pwm_levy(255);
		break;
		
		default:
			povoleni=0;		
		break;
	}	     
return povoleni;
}
int stav_baterie(void)
{	
 	ADMUX=0;
 	// Start
	ADCSRA |= (1<<ADSC); 
	// čekani na dokončení převodu
	while (ADCSRA & (1<<ADSC));  
	return (ADC);		
	if(ADC<718)
	{
	return 0;
	}
	else
	{
	return 1;
	}
}
// zpracování přerušení od mikrospínače
ISR(INT6_vect)
{	
	preruseni=1;
	AI(404);
	AI(202);
	pwm_pravy(250);
	pwm_levy(250);
	_delay_ms(500);
}
// zpracování přerušení od čítače časovače
ISR(TIMER3_COMPA_vect)
{	// zjišťování stavu baterie stačí jednou za sec.    
	stav=stav_baterie();
	pocet_cyklu++;
}
// otočení o 90° s pomocí gyroskopu
int16_t Get_rorate(int16_t smer)
{
	uint8_t gyrro[6];
	int16_t cyklus = 0;
	int16_t cas=50;
	int operace = smer;

	// čísla operací 101 - povolení jízdy - 404 - total stop - 303- překažka zkusit objet 
	switch (operace)
	{	
		case 303:
			// otáčí se do leva 
			PORTF &= ~((1 << PF5)| (1 << PF6));
			PORTF |= (1<<PF4)| (1 << PF7);		
			// první se provede co nejrychlejší reakce na zpomalení, poté se přehodí a začne otáčet
			pwm_pravy(255);	
			pwm_levy(255);
		break;
 		
		case 505: 
			// odáčí se do prava			
			PORTF &= ~((1 << PF4)| (1 << PF7));
			PORTF |= (1<<PF5)| (1 << PF6);
						
			pwm_pravy(255);	
			pwm_levy(255);			
		break;
		default:
		break;
	}
do
{
	gyrro[4]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, GYRO_ZOUT_H);
	gyrro[5]=LDByteReadI2C(TWI_SLAW,TWI_SLAR, GYRO_ZOUT_L);
	gz =  (gyrro[4]<<8|gyrro[5]);	
	gsz = gz/gyroScale;

    grz +=gsz/cas;
	
	char data[15];	
	int16_t odesli= grz;
	sprintf(data,"%d",odesli);			
	int number_data = 0;
	for ( number_data = 0; number_data < strlen(data); number_data++ )
		{
			uart_vysilani(data[number_data]);			
		}
	uart_vysilani('\n');	
} 
while (grz<90||grz<-90);
return 1;

}

int main(void)
{
	int adc_hodnota;
	int povoleni_jizdy=0;
	char receivedChar =0;  
	// piny PB5 a PB6 jako výstupní 
	DDRB |= (1<<PB5)|(1<<PB6);
	// PŘERUŠENÍ	
	DDRE &= ~((1 << PE6));	
		
	// pull up	
	PORTE |= (1 << PE6);
	//PORT F - OVLÁDÁNÍ H MŮSTKU.	
	DDRF |= (1<<PF4)|(1<<PF5)|(1<<PF6)|(1<<PF7);
	//PORTF |= (1<<PF5)|(1<<PF7);

	//přerušeni PE6 enable
	EIMSK|= (1<<INT6);
	// globální povolení přerušení 
	sei();


	init_i2c();
	

	timer3_init();


	Setup_MPU9250();

	
	// inicializace timer1 ( PWM FAST mode)
	pwm_init();
    
	
	// inicializace ADC (10bit) 
	adc_init();


	// inicializace uart (8bit 1 stop)
	uart_init();
	// první měření náklonu - akcelerometr určí výchozí hodnotu, robot musí ještě v této době stát
	Get_Values();
	while(1)
	{	
				
		Get_Values();
		adc_hodnota = adc_cteni();
		
		if ( uart_dataAvailable() )
		{ 
			receivedChar = uart_prijem();
			povoleni_jizdy= commander(receivedChar);                 
		}	
		// scan mode
		if((povoleni_jizdy==1)&&(scan==1)&&(stav==1))
		{	
						
			citac_povoleni=2;
			if((pocet_cyklu!=0)&&(preruseni!=1))
			{		
			
				switch (adc_hodnota)
				{
  					case 1 ... 520:
						// zkouška maxima 255   
						//pravy je PF 6 a PF7, S3
						// levy je PF 4 a PF 5, S2				
						pwm_pravy(255);
						pwm_levy(255);			
						
						AI(101);
					break;		
			
					case 521 ... 1024:
						if(zatacka==0)
						{		
							zatacka=!zatacka;							
							AI(303);
							//Get_rorate(303);						
							while(adc_hodnota>30) 
							{
							adc_hodnota = adc_cteni();	
							}
							_delay_ms(3000);					
							AI(101);
							_delay_ms(3000);						
							AI(303);
							_delay_ms(1000);										
							//Get_rorate(303);														
							_delay_ms(6800);
						}					
						else
						{
							zatacka=!zatacka;							
							AI(505);						
							//Get_rorate(505);
							while(adc_hodnota>30) 
							{
							adc_hodnota = adc_cteni();	
							}
							_delay_ms(3000);					
							AI(101);
							_delay_ms(3000);						
							//Get_rorate(505);							
							AI(505);
							_delay_ms(6800);	
						}


					break;
 
					default:
	    		    break;
				}
			}	
			else if(preruseni==1)
			{
				AI(303);
				while(adc_hodnota>70) 
					{
					adc_hodnota = adc_cteni();	
					}
				citac_povoleni=1;
				preruseni=0; 

			}
		}

		// normal mode
		if((povoleni_jizdy==1)&&(scan==0)&&(stav==1))
		{	
			uart_vysilani('A');
			uart_vysilani('\n');			
			citac_povoleni=2;
			if((pocet_cyklu!=0)&&(preruseni!=1))
			{		
			
				switch (adc_hodnota)
				{
  					case 1 ... 520:
						// zkouška maxima 255   
						//pravy je PF 6 a PF7, S3
						// levy je PF 4 a PF 5, S2				
						pwm_pravy(255);
						pwm_levy(255);			
						
						AI(101);
					break;		
			
					case 521 ... 1024:
						AI(303);						
						while(adc_hodnota>30) 
						{
							adc_hodnota = adc_cteni();	
						}
						_delay_ms(2000);						
					break;
 
					default:
	    		    break;
				}
			}	
			else if(preruseni==1)
			{
				AI(303);
				while(adc_hodnota>70) 
					{
					adc_hodnota = adc_cteni();	
					}
				citac_povoleni=1;
				preruseni=0; 

			}
		}

		

	}
return 0;
}