#include <xc.h>
#include <stdio.h>
#include <stdbool.h>
#include <math.h>
#include "SmartRGB.h"

// CONFIG1
#pragma config FOSC = INTOSC    // Oscillator Selection (INTOSC oscillator: I/O function on CLKIN pin)
#pragma config WDTE = OFF       // Watchdog Timer Enable (WDT disabled)
#pragma config PWRTE = OFF      // Power-up Timer Enable (PWRT disabled)
#pragma config MCLRE = OFF      // MCLR Pin Function Select (MCLR/VPP pin function is digital input)
#pragma config CP = OFF         // Flash Program Memory Code Protection (Program memory code protection is disabled)
#pragma config CPD = OFF        // Data Memory Code Protection (Data memory code protection is disabled)
#pragma config BOREN = ON       // Brown-out Reset Enable (Brown-out Reset enabled)
#pragma config CLKOUTEN = OFF   // Clock Out Enable (CLKOUT function is disabled. I/O or oscillator function on the CLKOUT pin)
#pragma config IESO = ON        // Internal/External Switchover (Internal/External Switchover mode is enabled)
#pragma config FCMEN = ON       // Fail-Safe Clock Monitor Enable (Fail-Safe Clock Monitor is enabled)

// CONFIG2
#pragma config WRT = OFF        // Flash Memory Self-Write Protection (Write protection off)
#pragma config VCAPEN = OFF     // Voltage Regulator Capacitor Enable bit (Vcap functionality is disabled on RA6.)
#pragma config PLLEN = OFF      // PLL Enable (4x PLL disabled)
#pragma config STVREN = ON      // Stack Overflow/Underflow Reset Enable (Stack Overflow or Underflow will cause a Reset)
#pragma config BORV = LO        // Brown-out Reset Voltage Selection (Brown-out Reset Voltage (Vbor), low trip point selected.)
#pragma config LPBOR = OFF      // Low Power Brown-Out Reset Enable Bit (Low power brown-out is disabled)
#pragma config LVP = ON         // Low-Voltage Programming Enable (Low-voltage programming enabled)

// I/O pin definitions
// RA0-RA7 = LCD

#define ENK                 PORTBbits.RB0
#define RX1                 PORTBbits.RB1
#define RX2                 PORTBbits.RB2
#define RX3                 PORTBbits.RB3
#define RX4                 PORTBbits.RB4
#define RX5                 PORTBbits.RB5
#define SW3                 PORTBbits.RB6
#define SW2                 PORTBbits.RB7

#define SW3_ON              PORTBbits.RB6 == 0
#define SW3_OFF             PORTBbits.RB6 == 1
#define SW2_ON              PORTBbits.RB7 == 0
#define SW2_OFF             PORTBbits.RB7 == 1

#define RGB                 LATCbits.LATC0
// DRIVER_ENB PWM_1B        LATCbits.LATC1
#define DRIVER_IN1          LATCbits.LATC2
// SCL RC3
// SDA RC4
#define RX_BACK             PORTCbits.RC5
//DRIVER_ENA PWM2_A         LATCbits.LATC6
//#define RES               PORTCbits.RC7

#define DRIVER_IN2          LATDbits.LATD0
// BATTERY                             RD1 (AN21)
#define DRIVER_IN4         LATDbits.LATD2
#define DRIVER_IN3         LATDbits.LATD3
//SERVO PWM3_F              LATDbits.LATD4
//#define RES               PORTDbits.RD5
#define US_ECHO             PORTDbits.RD6
#define US_TRIG             LATDbits.LATD7

//SPEAKER PWM_4B            LATEbits.LATE0
#define LCDE                LATEbits.LATE1          //   LCD
#define LCDRS               LATEbits.LATE2          //   LCD

#define SW1_ON              PORTEbits.RE3 == 0
#define SW1_OFF             PORTEbits.RE3 == 1

#define _XTAL_FREQ  16000000
#define USE_AND_MASKS

#define Wt MOTOR(0, 0); Wait(0)

#define black  1
#define out    0

#define left     (0)
#define right    (2)
#define straight (1)

#define I2C_IsBusy SSPSTATbits.R_nW || SSPCON2bits.SEN || SSPCON2bits.RSEN || SSPCON2bits.PEN || SSPCON2bits.RCEN || SSPCON2bits.ACKEN
#define I2C_WriteCollision SSPCON1bits.WCOL
#define I2C_Overflow       SSPCON1bits.SSPOV
#define I2C_BusCollision   PIR2bits.BCL1IF
#define I2C_HoldClock      SSPCON1bits.CKP=0
#define I2C_ReleaseClock   SSPCON1bits.CKP=1
#define I2C_Flag           PIR1bits.SSP1IF
#define I2C_BufferFull     SSPSTATbits.BF

#define Compass_ADR   0x1D
#define Gyro_ADR      0x6B

// The LSM303D Compass Registers
#define C_TEMP_L      0x05
#define C_TEMP_H      0x06
#define MAG_X_L       0x08
#define MAG_X_H       0x09
#define MAG_Y_L       0x0A
#define MAG_Y_H       0x0B
#define MAG_Z_L       0x0C
#define MAG_Z_H       0x0D
#define WHO_AM_I      0x0F
#define CTRL1         0x20
#define CTRL2         0x21
#define CTRL4         0x23
#define CTRL5         0x24
#define CTRL6         0x25
#define CTRL7         0x26
#define ACC_X_L       0x28
#define ACC_X_H       0x29
#define ACC_Y_L       0x2A
#define ACC_Y_H       0x2B
#define ACC_Z_L       0x2C
#define ACC_Z_H       0x2D

// The L3GD20H Gyro Registers
#define GYRO_TEMP     0x26
#define GYRO_X_L      0x28
#define GYRO_X_H      0x29
#define GYRO_Y_L      0x2A
#define GYRO_Y_H      0x2B
#define GYRO_Z_L      0x2C
#define GYRO_Z_H      0x2D
#define LOW_ODR       0x39

#define W ShowRX()

char* s1 = "s", s2 = "kola";

const unsigned char rx_black=0b11111111, rx_out=0b01001111, arrow_left='<', arrow_right='>', arrow_up=0b11011001;
unsigned int power = 7000, powerTurn = 9000, powerslow = 5800, step_time = 0, enkTiky = 0, prvENK  = 0, iForRGB = 1;

bool load = false, crossBool = false, kraj = 0, krajAL = 0, withoutUZ = false, debugTL = false;
unsigned char command, Past_Encoder, Direction = 2, lstDirction;
long Vbat, ADC_Distance, Encoder_Ticks=0, MS10 = 0;
int Distance=100, MS100str = 0, MS100lo = 0, MS100un, cross = 0, program = 10,napeti=10000,  I2C_error=0, prg = 0;;
int US_cm=0, cyklus=0, ServoPos=0, PosX=4, PosY=1;
unsigned char loadingDirection = 0, loadingDirectionStart, unloadingDirection = 0, HomeDirection = 0, currentDirection = 0, start = true;

unsigned char command, intflag=0,I2C_data[10];
unsigned char sync_mode=0, slew=0, add1, w, data, status, length, temp;
short ACC_X, ACC_Y, ACC_Z, MAG_X, MAG_Y, MAG_Z, GYRO_X, GYRO_Y, GYRO_Z;

float Rotace, Zvedani, Kompas,factor,AccXg,AccYg,AccZg,MagXg,MagYg,MagZg,MagXg2,MagYg2,GyroZg,AccXSquared,AccYSquared,AccZSquared,gravity_range,resolution;

unsigned int thisi = 0;

// Function declarations
void PICinit();
void LCDinit();
void LCDsend(unsigned int command);
void LCDwrite(unsigned int command);
void LCDposition(unsigned int position ,unsigned int row);
void LCDclear();
void GoS();
void TurnL();
void TurnR();
void Delay(unsigned long b);
void Wait(long ms);

long Battery(int samples);
void MOTOR(int pwr_l, int pwr_r);
void MOTOR_L(long power_L);
void MOTOR_R(long power_R);
void SERVO(int uhel_servo);
void SPEAKER(int frekvence);

int ShowRX();
void showtime(unsigned int tntnc);
void showprogram(int prg);

void Write_EEPROM();
void Read_EEPROM();
void Select_Program();

void interrupt ProgramInterrupt();
int Ultrasonic();
void IMU();
void lama();
void Pozice(int PosX1,int PosY1);

// < editor-fold >
float doba = 0.15; //second
int dataFL[] = {
32,33,32,33,32,
37,34,36,39,
63,56,51,48,44,39,37,
68,56,52,44,40,37,36,

32,33,32,33,32,
37,34,36,39,
63,56,51,48,44,39,37,
52,49,44,36,37,39
};
int klavesa[] = {
    4186,
    3951,
    3729,
    3520,
    3322,
    3135,
    2959,
    2793,
    2637,
    2489,
    2349,
    2217,
    2093,
    1975,
    1864,
    1760,
    1661,
    1567,
    1479,
    1396,
    1318,
    1244,
    1174,
    1108,
    1046,
    987,
    932,
    880,
    830,
    783,
    739,
    698,
    659,
    622,
    587,
    554,
    523,
    493,
    466,
    440,
    415,
    391,
    369,
    349,
    329,
    311,
    293,
    277,
    261,
    246,
    233,
    220,
    207,
    195,
    184,
    174,
    164,
    155,
    146,
    138,
    130,
    123,
    116,
    110,
    103,
    97,
    92,
    87,
    82,
    77,
    73,
    69,
    65,
    61,
    58,
    55,
    51,
    48,
    46,
    43,
    41,
    38,
    36,
    34,
    32,
    30,
    29,
    27
};


void tone(int cislo, float nota)
{
    SPEAKER((int)klavesa[cislo]);
    Wait(1000*doba*nota);
    SPEAKER(0);
}
// </editor-fold>


//  I2C IMU Communication **************************************************************************************************************************

void I2C_ErrorControl(int LineNo)
{
    if(I2C_WriteCollision) I2C_error+=1;
    if(I2C_Overflow)       I2C_error+=10;
    if(I2C_BusCollision)   I2C_error+=50;

    // if(I2C_error>0);

    I2C_WriteCollision=0; I2C_Overflow=0; I2C_BusCollision=0;
}

void I2C_Master_Init()
{
    SSPCON1bits.SSPEN=1; // I2C enable
    SSPCON1bits.SSPM=0b1000; // I2C master mode
    SSPSTATbits.SMP=1; // slew rate control disabled (for normal speed 100 kHz)
    SSPADD=39; //100 kHz Baud clock @ 16 MHz
    SSPCON3bits.SDAHT=1; // min. 300 ns hold time
}


void I2C_MasterWrite_IMU(unsigned char address, unsigned char reg, unsigned char data)
{
    I2C_error=0;
    while(I2C_IsBusy);
    SSPCON2bits.SEN=1;           // I2C start bit
    while(SSPCON2bits.SEN);      // waiting for Start bit to end
    I2C_data[0] = SSPBUF;          // read any previous stored content in buffer to clear buffer full status

    while(I2C_IsBusy);
    I2C_Flag=0;
    SSPBUF = (address * 2); // write the address of slave R/W bit is '0' for writing to slave
    I2C_ErrorControl(211);
    while(I2C_Flag==0);
    while(SSPCON2bits.ACKSTAT);  // waiting for slave acknowledge

    while(I2C_IsBusy);
    I2C_Flag=0;
    SSPBUF = reg;                 // write register number
    I2C_ErrorControl(215);
    while(I2C_Flag==0);
    while(SSPCON2bits.ACKSTAT);  // waiting for slave acknowledge

    while(I2C_IsBusy);
    I2C_Flag=0;
    SSPBUF = data;                 // write data to the selected register
    I2C_ErrorControl(219);
    while(I2C_Flag==0);
    while(SSPCON2bits.ACKSTAT);  // waiting for slave acknowledge

    while(I2C_IsBusy);
    SSPCON2bits.PEN=1;           // I2C stop bit
    while(SSPCON2bits.PEN);      // waiting for Stop bit to end
}

unsigned char I2C_MasterRead_IMU(unsigned char address, unsigned char reg)
{
    I2C_error=0;
    while(I2C_IsBusy);
    I2C_data[0] = SSPBUF;          // read any previous stored content in buffer to clear buffer full status
    SSPCON2bits.SEN=1;           // I2C start bit
    while(SSPCON2bits.SEN);      // waiting for Start bit to end


    while(I2C_IsBusy);
    I2C_Flag=0;
    SSPBUF = (address * 2); // write the address of slave R/W bit is '0' for writing to slave
    // SSPBUF = 0x3a; // write the address of slave R/W bit is '0' for writing to slave
    I2C_ErrorControl(240);
    while(I2C_Flag==0);
    while(SSPCON2bits.ACKSTAT);  // waiting for slave acknowledge

    while(I2C_IsBusy);
    I2C_Flag=0;
    SSPBUF = reg;                 // write register number
    I2C_ErrorControl(245);
    while(I2C_Flag==0);
    while(SSPCON2bits.ACKSTAT);  // waiting for slave acknowledge

    while(I2C_IsBusy);
    SSPCON2bits.RSEN=1;           // I2C Restart bit
    while(SSPCON2bits.RSEN);      // waiting for Restart bit to end
    I2C_ErrorControl(251);

    while(I2C_IsBusy);
    I2C_Flag=0;
    SSPBUF = ((address*2)+1); // write the address of slave R/W bit is '1' for reading from slave
    // SSPBUF = 0x3b; // write the address of slave R/W bit is '1' for reading from slave
    I2C_ErrorControl(254);
    while(I2C_Flag==0);
    while(SSPCON2bits.ACKSTAT);  // waiting for slave acknowledge

    while(I2C_IsBusy);
    SSPCON2bits.RCEN=1;           // I2C Master receive enable
    while(SSPCON2bits.RCEN);      // waiting for setting the enable
    while(SSPSTATbits.BF==0);      // waiting for filling the buffer
    I2C_data[0]=SSPBUF;
    I2C_ErrorControl(262);

    while(I2C_IsBusy);
    SSPCON2bits.ACKDT=1;           // Master not acknowledge
    SSPCON2bits.ACKEN=1;
    while(SSPCON2bits.ACKEN);

    while(I2C_IsBusy);
    SSPCON2bits.PEN=1;           // I2C stop bit
    while(SSPCON2bits.PEN);      // waiting for Stop bit to end

    return I2C_data[0];
}

short combineValues(unsigned char msb, unsigned char lsb)
{
   //shift the msb right by 8 bits and OR with lsb
   return ((short)msb<<8)|(short)lsb;
}


//******************************************************************************
// Battery voltage measurement
long Battery(int samples)   // RD1 = AN21
{
    long j2,result=0;
    ADCON0=0b01010101;  // 12bit ADC, channel AN21, ADC module on

    for(j2=0;j2<samples;j2++)
    {
        __delay_us(50);
        ADCON0bits.GO=1;                // turn on conversion
	while(ADCON0bits.GO==1);	// waiting for finishing conversion
        result+=(ADRESH*16)+(ADRESL/15);
    }
    return (result/samples);
}

//******************************************************************************

void MOTOR_L(long power_L)
{
    power_L = (power_L * 100) / Vbat;
    if(power_L>0)      {DRIVER_IN1=0; DRIVER_IN2=1; PSMC2DC = power_L*655.35;}
    else if(power_L<0) {DRIVER_IN1=1; DRIVER_IN2=0; PSMC2DC = (-power_L)*655.35;}
    else               {DRIVER_IN1=0; DRIVER_IN2=0; PSMC2DC = power_L*655.35;}

    PSMC2CONbits.PSMC2LD=1;
}

void MOTOR_R(long power_R)
{
    power_R = (power_R * 100) / Vbat;
    if(power_R>0)      {DRIVER_IN3=0; DRIVER_IN4=1; PSMC1DC = power_R*655.35;}
    else if(power_R<0) {DRIVER_IN3=1; DRIVER_IN4=0; PSMC1DC = (-power_R)*655.35;}
    else               {DRIVER_IN3=0; DRIVER_IN4=0; PSMC1DC = power_R*655.35;}

    PSMC1CONbits.PSMC1LD=1;
}

void MOTOR(int pwr_l, int pwr_r)
{
    MOTOR_L(pwr_l);
    MOTOR_R(pwr_r);
}

void SERVO(int uhel_servo)
{
    PSMC3DC = (long)(16.677*uhel_servo)+1500;
    PSMC3CONbits.PSMC3LD=1;
}

void SPEAKER(int frekvence)
{
 if(frekvence == 0) PSMC4PR = 0;
 else PSMC4PR = (64000000/frekvence);    // PWM period for 8 kHz at 64 MHz oscillator
 PSMC4PH = 0;                       // PWM duty cycle start
 PSMC4DC = PSMC4PR/2;               // PWM duty cycle stop
 PSMC4CONbits.PSMC4LD=1;
}


//******************************************************************************
// PIC initiation
void PICinit()
{

 // I/O pins setting
 TRISA=0;          ANSELA=0;          LATA=0;
 TRISB=0b11111111; ANSELB=0;          LATB=0;
 TRISC=0b10111000; ANSELC=0;          LATC=0;
 TRISD=0b01100010; ANSELD=0b00000010; LATD=0;
 TRISE=0b1000;     ANSELE=0;          LATE=0;

 OPTION_REGbits.nWPUEN=0; WPUB=0b11000000; WPUE=0b1000;   // buttons pull-ups setting

 OSCCON=0b01111010;  // 16MHz internal oscillator

 // A/D conversion register settings
 ADCON1=0b00100000; // sign-magn data format, conversion frequency FOSC/32, voltage reference = VDD-VSS
 ADCON2=0b00001111; // negative reference = VSS

 // PWM setup for RC1 (PSMC1B) Right Motor
 PSMC1PR = 65535;      // PWM period
 PSMC1PH = 0;          // PWM duty cycle start
 PSMC1DC = 0;          // PWM duty cycle stop
 PSMC1CLK = 0;         // FOSC as PWM clock, prescaler 1
 PSMC1OENbits.P1OEB=1; // PSMC1B pin on;
 PSMC1STR0=0b00000010; // PSMC1B pin on;
 PSMC1STR1=0b10000000; // PWM updated after the previous period ends
 PSMC1PHS = 1; PSMC1DCS = 1; PSMC1PRS = 1; // timer-based PWM
 PSMC1CON=0b11000000;  // PWM module on, update registers, no deadband, single PWM mode

// PWM setup for RC6 (PSMC2A) Left Motor
 PSMC2PR = 65535;      // PWM period
 PSMC2PH = 0;          // PWM duty cycle start
 PSMC2DC = 0;          // PWM duty cycle stop
 PSMC2CLK = 0;         // FOSC as PWM clock, prescaler 1
 PSMC2OENbits.P2OEA=1; // PSMC2A pin on;
 PSMC2STR0=0b00000001; // PSMC2A pin on;
 PSMC2STR1=0b10000000; // PWM updated after the previous period ends
 PSMC2PHS = 1; PSMC2DCS = 1; PSMC2PRS = 1; // timer-based PWM
 PSMC2CON=0b11000000;  // PWM module on, update registers, no deadband, single PWM mode

// PWM setup for RD4 (PSMC3F) Servo
 PSMC3PR = 40000;      // PWM period
 PSMC3PH = 0;          // PWM duty cycle start
 PSMC3DC = 3000;       // PWM duty cycle stop
 PSMC3CLK = 0b00110000;// FOSC as PWM clock, prescaler 8
 PSMC3OENbits.P3OEF=1; // PSMC3F pin on;
 PSMC3STR0=0b00100000; // PSMC3F pin on;
 PSMC3STR1=0b10000000; // PWM updated after the previous period ends
 PSMC3PHS = 1; PSMC3DCS = 1; PSMC3PRS = 1; // timer-based PWM
 PSMC3CON=0b11000000;  // PWM module on, update registers, no deadband, single PWM mode


// PWM setup for RE0 (PSMC4B) Speaker
 PSMC4PR = 8000;       // PWM period for 8 kHz at 64 MHz oscillator
 PSMC4PH = 0;          // PWM duty cycle start
 PSMC4DC = 0;          // PWM duty cycle stop
 PSMC4CLK = 0b00000001;// 64 MHz PWM clock, prescaler 1
 PSMC4OENbits.P4OEB=1; // PSMC4B pin on;
 PSMC4STR0=0b00000010; // PSMC4B pin on;
 PSMC4STR1=0b10000000; // PWM updated after the previous period ends
 PSMC4PHS = 1; PSMC4DCS = 1; PSMC4PRS = 1; // timer-based PWM
 PSMC4CON=0b11000000;  // PWM module on, update registers, no deadband, single PWM mode

 // Enable interrupts
 INTCONbits.GIE = 1;  // Global interrupt enable
 INTCONbits.PEIE = 1; // Peripheral interrupts enable

 // TMR1 Timer setup with interrupt
 PIR1bits.TMR1IF = 0; // TMR1 interrupt flag
 PIE1bits.TMR1IE = 1; // TMR1 interrupt enable
 T1CON = 0b00110001;  // FOSC/4 clock, 8x prescaler, timer on
 TMR1 = 60535;

 // TMR0 Timer setup (for ultrasonic sensor)
 INTCONbits.TMR0IF=0;
 OPTION_REGbits.TMR0CS=0; // FOSC/4
 OPTION_REGbits.PSA=0; // TMR0 prescaler on
 OPTION_REGbits.PS=0b111; // TMR0 prescaler 256
 }


void IMU_Init()
{
        I2C_MasterWrite_IMU(Compass_ADR,CTRL1,0b00010111); // 3.125 Hz Accel.,konticteni,zapni AccXYZ
        I2C_MasterWrite_IMU(Compass_ADR,CTRL2,0b11000000); // antialias 50 Hz, rozsah Acc +/- 2g
        I2C_MasterWrite_IMU(Compass_ADR,CTRL5,0b11100000); // teplomer on, magneticke rozliseni high, 3.125 Hz magnet
        I2C_MasterWrite_IMU(Compass_ADR,CTRL6,0b00000000); // rozsah mag +/- 2 gauss
        I2C_MasterWrite_IMU(Compass_ADR,CTRL7,0b00000000); // kontikonverze magnet

        I2C_MasterWrite_IMU(Gyro_ADR,CTRL1,0b00001111); // 12.5 Hz gyro,zapni XYZ
        I2C_MasterWrite_IMU(Gyro_ADR,CTRL4,0b00000000); // konticteni, rozsah 245 dps
        I2C_MasterWrite_IMU(Gyro_ADR,CTRL5,0b00000000); // FIFO off
        I2C_MasterWrite_IMU(Gyro_ADR,LOW_ODR,0b00000001); // low speed enabled

        gravity_range=4.0f; //+- 2g
        resolution = 65535.0f;
        factor = gravity_range/resolution;
}


//******************************************************************************
// TIMING FUNCTIONS

void Delay(unsigned long b)
{
  for(;b>0;b--);
}

void Wait(long ms)
{
    for(long bb=0;bb<ms;bb++) __delay_ms(1);
}

//******************************************************************************
// ULTRASONIC DISTANCE MEASUREMENT

int Ultrasonic()
{
 int vysledek;
 OPTION_REGbits.TMR0CS=0; // FOSC/4
 OPTION_REGbits.PSA=0; // TMR0 prescaler on
 OPTION_REGbits.PS=0b111; // TMR0 prescaler 256
 US_TRIG=1;
 __delay_us(20);
 US_TRIG=0;

 TMR0=0;
 INTCONbits.TMR0IF=0;
 while(US_ECHO==0 && INTCONbits.TMR0IF==0);
 TMR0=0;
 INTCONbits.TMR0IF=0;
 while(US_ECHO==1 && INTCONbits.TMR0IF==0);

 vysledek = TMR0 * 1.103;
 if(INTCONbits.TMR0IF==1) vysledek = 300;
 return vysledek;
}


/*void IMU()
{
        MAG_X = combineValues(I2C_MasterRead_IMU(Compass_ADR,MAG_X_H),I2C_MasterRead_IMU(Compass_ADR,MAG_X_L));
        MAG_Y = combineValues(I2C_MasterRead_IMU(Compass_ADR,MAG_Y_H),I2C_MasterRead_IMU(Compass_ADR,MAG_Y_L));
        MAG_Z = combineValues(I2C_MasterRead_IMU(Compass_ADR,MAG_Z_H),I2C_MasterRead_IMU(Compass_ADR,MAG_Z_L));
        ACC_X = combineValues(I2C_MasterRead_IMU(Compass_ADR,ACC_X_H),I2C_MasterRead_IMU(Compass_ADR,ACC_X_L));
        ACC_Y = combineValues(I2C_MasterRead_IMU(Compass_ADR,ACC_Y_H),I2C_MasterRead_IMU(Compass_ADR,ACC_Y_L));
        ACC_Z = combineValues(I2C_MasterRead_IMU(Compass_ADR,ACC_Z_H),I2C_MasterRead_IMU(Compass_ADR,ACC_Z_L));
        
        GYRO_Z = combineValues(I2C_MasterRead_IMU(Gyro_ADR,GYRO_Z_H),I2C_MasterRead_IMU(Gyro_ADR,GYRO_Z_L));

        AccXg = ACC_X * factor; AccYg = ACC_Y * factor; AccZg = ACC_Z * factor;
        MagXg = MAG_X * factor; MagYg = MAG_Y * factor; MagZg = MAG_Z * factor;
        GyroZg = GYRO_Z * factor * 100;
        AccXSquared = AccXg * AccXg; AccYSquared = AccYg * AccYg; AccZSquared = AccZg * AccZg ;

        Zvedani = atan(AccXg/sqrt(AccYSquared + AccZSquared));   // podle D.Molloye
        Rotace =  atan(AccYg/sqrt(AccXSquared + AccZSquared));    // podle D.Molloye
        MagXg2 = (MagXg*cos(Zvedani)) + (MagZg*sin(Zvedani));
        MagYg2 = (MagXg*sin(Rotace)*sin(Zvedani)) + (MagYg*cos(Rotace)) - (MagZg*sin(Rotace)*cos(Zvedani));

      if(MagYg2>0) Kompas = 90-(180/M_PI*atan((double)MagXg2/(double)MagYg2));
      if(MagYg2<0) Kompas = 270-(180/M_PI*atan((double)MagXg2/(double)MagYg2));
      if(MagYg2==0 && MagXg2<0) Kompas = 180;
      if(MagYg2==0 && MagXg2>0) Kompas = 0;

        Zvedani = (180 * Zvedani) /M_PI;   // podle D.Molloye
        Rotace = (180 * Rotace) /M_PI;    // podle D.Molloye
}*/

//******************************************************************************
// LCD DISPLAY INITIATION CODE

void LCDinit()
{
  LCDRS=0;

  Wait(50);            // wait at least 40 ms for power stabilisation
  command=0b00111000;	// 2-line display N=1, 5x8 dots F=0
  LCDsend(command);

  Wait(1);            // wait at least 39 us
  command=0b00111000;	// 2-line display N=1, 5x8 dots F=0
  LCDsend(command);

  Wait(1);            // wait at least 37 us
  command=0b00001100;	// display on, blink off, cursor off
  LCDsend(command);

  Wait(1);            // wait at least 37 us
  command=0b00000001;	// clear display
  LCDsend(command);

  Wait(3);             // wait at least 1.53 ms
  command=0b00000110;	// position added I/D=1, shift off S=0
  LCDsend(command);
}

//******************************************************************************
// LCD display control

void LCDposition(unsigned int position, unsigned int row)
{
  LCDRS = 0;
  if(position > 16 || row > 2 || position == 0 || row == 0) LCDsend(128);
  else LCDsend((position + ((row - 1) * 64)) + 127);
}

void LCDwrite(unsigned int command)
{
  LCDRS = 1;
  LCDsend(command);
}

void LCDsend(unsigned int command)
{
  LATA=command;
  LCDE = 1;
  Delay(5);
  LCDE = 0;
  Delay(5);
}

void LCDclear()
{
    LCDposition(1,1);
    printf("                 ");
    LCDposition(1,2);
    printf("                 ");
    LCDposition(1,1);
}

extern void putch(char c)   // printf enable
{
    LCDwrite(c);
    return;
}

void Pozice(int PosX1,int PosY1)
{
    LCDposition(15,2);printf("%i%i",PosX1,PosY1);
}
//******************************************************************************

void interrupt ProgramInterrupt()
{
    if(PIR1bits.TMR1IF==1)
    {
        PIR1bits.TMR1IF=0;
        T1CON = 0b00110001;  // FOSC/4 clock, 8x prescaler, timer on
        TMR1 = 55535;         // 15535 = 100 ms, 60535 = 10 ms, 45535 = 40 ms, 55535 = 20 ms
        MS10++;
        if(ENK != prvENK)
        {
            prvENK = ENK;
            enkTiky++;
        }
        /*if(MS10 % 10 == 0)
        {
            SmartRGB(iForRGB);
            iForRGB++;
            if(iForRGB > 7) iForRGB = 0;
        }*/
    }
}

int ShowRX()
{
         if(RX1==0) {LCDposition(1,1);LCDwrite(rx_out);}
         else {LCDposition(1,1);LCDwrite(rx_black);}

         if(RX2==0) {LCDposition(2,1);LCDwrite(rx_out);}
         else {LCDposition(2,1);LCDwrite(rx_black);}

         if(RX3==0) {LCDposition(3,1);LCDwrite(rx_out);}
         else {LCDposition(3,1);LCDwrite(rx_black);}

         if(RX4==0) {LCDposition(4,1);LCDwrite(rx_out);}
         else {LCDposition(4,1);LCDwrite(rx_black);}

         if(RX5==0) {LCDposition(5,1);LCDwrite(rx_out);}
         else {LCDposition(5,1);LCDwrite(rx_black);}

         if(RX_BACK==0) {LCDposition(7,1);LCDwrite(rx_out);}
         else {LCDposition(7,1);LCDwrite(rx_black);}

         LCDposition(9,1);
         printf("%i  ", enkTiky);

         LCDposition(13, 1);
         printf("%i", thisi);

         LCDposition(1, 2);
         s1 = "s";
         if (withoutUZ) s1 = "bez";
         if(prg > 1) printf("%i kola %s UZ", prg, s1);
         else printf("1 kolo %s UZ", s1);

   //    if(SW2_ON) {LCDposition(16,1);LCDwrite(rx_black);MOTOR(0,0);}
   //    else {LCDposition(16,1);LCDwrite(rx_out);}
         return 0;
}

void Wait_Show_RX(long cycles)
{
    for (long i=0;i<cycles;i++) ShowRX();
}


void showtime(unsigned int tntnc)
{
    int prstr = 0, prun = 0;
    LCDposition(1, 1);
    if(MS100str == 0) prstr = MS10;
    else prstr = MS100str;
    if(MS100str != 0) {
        if(MS100un < 0) prun = MS100un + MS10;
        else prun = MS100un;
    }
    if(tntnc > 0) printf("%i:%i:%i:%i", prstr, prun, MS10, MS10-tntnc);
    else printf("%i:%i:%i", prstr, prun, MS10);
}

void Write_EEPROM()
{
     for (int i5 = 100;i5 >= 0;i5--)
     {
        if(SW2_ON) program++;
        if(program >= 9) program = 0;
        LCDposition(1,1);showprogram(program);
        LCDposition(1,2);printf("%i", i5/10);
        Wait(100);
     }

     EEADRL = 0x00;
     EEDATL = program;
     EECON1bits.EEPGD = 0;
     EECON1bits.CFGS = 0;
     EECON1bits.WREN = 1;
     EECON2 = 0x55;
     EECON2 = 0xAA;
     EECON1bits.WR = 1;
     while(EECON1bits.WR);				//Wait till the write completion
     EECON1bits.WREN = 0;
}


void Read_EEPROM()
{
    EEADR = 0x00;
    EECON1bits.CFGS = 0;
    EECON1bits.EEPGD = 0;
    EECON1bits.RD = 1;
    __nop();			//Nop may be required for latency at high frequencies
    __nop();			//Nop may be required for latency at high frequencies
    program = EEDATA;           //save read byte
}

void Align2(int kam)
{
    if(krajAL){ krajAL = 0; return; }
    const int timeToTurn = 300;
    SmartRGB(red);
    //while(SW2) W;

    MOTOR(power * kam, -power * kam);

    for(int i = 0; (i < timeToTurn); i++) if(RX3 == out) Wait(1);
    MOTOR(-power * kam, power * kam);

    for(int i = 0; (i<2*timeToTurn); i++) if(RX3 == out) Wait(1);
    MOTOR(power * kam, -power * kam);

    for(int i = 0; (i < timeToTurn); i++) if(RX3 == out) Wait(1);
    MOTOR(0, 0);

    //SmartRGB(green);
    //while(SW2) W;
    SmartRGB(7);
}
void Align()
{
    SmartRGB(yellow);
    MOTOR(power, -power);
    while(RX1 + RX2 + RX3 + RX4 + RX5 == 0) W;
    MOTOR(0, 0);
    if (RX3);
    else if (RX1 || RX2) Align2(-1);
    else if (RX4 || RX5) Align2(1);
    MOTOR(0, 0);
}

void whileUZ()
{
    SmartRGB(purple);
    if(withoutUZ) return;
    MOTOR(0, 0);
    int dl = 0, dl1 = 0, dl2 = 0, dlc = 0, dc = 0, drc = 0, dr = 0;

    int s = 15, x = 100, k = 30, d = 20;

    /*while (dl < d || dl1 < d || dl2 < d || dlc < d || dc < d)//|| drc < d// || dr < d)
    {*/
        //SERVO(0);   Wait(x);  dl = Ultrasonic(); s += k; while(Ultrasonic() < d) { Wait(60); LCDposition(13, 2); printf("%icm", Ultrasonic()); Wait(60);}
        SERVO(30);  Wait(x); dl1 = Ultrasonic(); s += k; while(Ultrasonic() < d) { Wait(60); LCDposition(13, 2); printf("%icm", Ultrasonic()); Wait(60);}
        SERVO(60);  Wait(x); dl2 = Ultrasonic(); s += k; while(Ultrasonic() < d) { Wait(60); LCDposition(13, 2); printf("%icm", Ultrasonic()); Wait(60);}
        SERVO(90);  Wait(x); dlc = Ultrasonic(); s += k; while(Ultrasonic() < d) { Wait(60); LCDposition(13, 2); printf("%icm", Ultrasonic()); Wait(60);}
        SERVO(120); Wait(x);  dc = Ultrasonic(); s += k; while(Ultrasonic() < d) { Wait(60); LCDposition(13, 2); printf("%icm", Ultrasonic()); Wait(60);}
        SERVO(150); Wait(x); drc = Ultrasonic(); s += k; while(Ultrasonic() < d) { Wait(60); LCDposition(13, 2); printf("%icm", Ultrasonic()); Wait(60);}
        SERVO(180); Wait(x);  dr = Ultrasonic(); s += k; while(Ultrasonic() < d) { Wait(60); LCDposition(13, 2); printf("%icm", Ultrasonic()); Wait(60);}

        LCDclear();
        LCDposition(1, 1);
        printf("%i %i %i %i  ", dl , dl1 , dl2 , dlc);
        LCDposition(1, 2);
        printf("%i %i %i  ", dc, drc , dr);

        if (last != red) SmartRGB(red);
    //}
    SmartRGB(7);
    LCDclear();
    ShowRX();
}

void GoS()
{
    thisi++;
    SmartRGB(greenblue);
    enkTiky = 0;
    while (RX1 + RX2 + RX3 + RX4 + RX5 < (3-kraj) /*|| enkTiky < 1*/)
    {
        if(RX2 == black || RX1 == black) MOTOR(-power, power);
        else if(RX4 == black || RX5 == black) MOTOR(power, -power);
        else MOTOR(power, power);
        //ShowRX();
    }

    MOTOR(power, power);
    while(RX1 == black || RX5 == black) W;
    enkTiky = 0;
    Wait(150);
    MOTOR(0,0);

    Align();

    ShowRX();
    Wait(step_time);
    whileUZ();

    //while(SW2_OFF && debugTL) ShowRX();
    enkTiky = 0;
}

void TurnR()
{
    //if(debugTL == true) while(SW2_OFF) ShowRX();
    SmartRGB(green);
    MOTOR(powerTurn, -powerTurn);
    while(RX1 != black && kraj == 0) W;
    while(RX3 != black) W;
    //while(RX3 == black) W;

    MOTOR(0,0);
    Wait(step_time);
    whileUZ();
    Align2(-1);
    //if(debugTL == true) while(SW2_OFF) ShowRX();
    enkTiky = 0;
}

void TurnL()
{
    //if(debugTL == true) while(SW2_OFF) ShowRX();
    SmartRGB(blue);
    MOTOR(-powerTurn, powerTurn);
    while(RX5 != black && kraj == 0) W;
    while(RX3 != black) W;
    //while(RX3 == black) W;

    MOTOR(0,0);
    Wait(step_time);
    whileUZ();
    Align2(1);
    //if(debugTL == true) while(SW2_OFF) ShowRX();
    enkTiky = 0;
}

void Turn180()
{
    MOTOR(-power, -power);
    while(RX1 + RX2 + RX3 + RX4 + RX5 < 2); W;
    MOTOR(0, 0);
    MOTOR(powerTurn, -powerTurn);
    Wait(250);
    while(RX3 != black) W;
    while(RX3 == black) W;
    GoS();
}

void Turn180ToTurn()
{
    MOTOR((0-power)+1500, (0-power)+1500);
    Wait(500);
    while (RX_BACK == out) ShowRX();
    Wait(250);

    MOTOR(0, 0);

    //while(SW2_OFF) ShowRX();
    SmartRGB(blue);
    MOTOR(-powerTurn, powerTurn);
    while(RX5 != black && kraj == 0) W;
    while(RX2 != black) W;
    //while(RX3 == black) W;

    MOTOR(0,0);
    Wait(step_time);
    whileUZ();
    Align2(1);
    //while(SW2_OFF) ShowRX();
    enkTiky = 0;
}

void ToLine()
{
    power -= 1000;
    while(RX_BACK == out)
    {
        if(RX2 == black) MOTOR(-power, power);
        else if(RX4 == black) MOTOR(power, -power);
        else MOTOR(power, power);
        ShowRX();
    }
    power += 1000;
    MOTOR(0, 0);
    Wait(1000);

    ShowRX(); Wait(step_time);
}
//*****************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//*****************************************************************************
void main()
{
    PICinit();
    LCDinit();
  //  I2C_Master_Init(); IMU_Init();
    
    LCDposition(3,1); printf("Ketchup House ");
    Vbat = Battery(100) * 4.387;
    LCDposition(2,2); printf("Vbat: %i mV", Vbat);

    tone(52, 1); Wait(100);
    tone(53, 1); Wait(100);
    tone(54, 1); Wait(300);
    tone(53, 1); Wait(100);
    tone(52, 1); Wait(100);

    /*tone(32, 1); Wait(100);
    tone(33, 1); Wait(100);
    tone(32, 1); Wait(100);
    tone(33, 1); Wait(100);
    tone(32, 1); Wait(100);

    tone(37, 1); Wait(100);
    tone(34, 1); Wait(100);
    tone(36, 1); Wait(100);
    tone(39, 1); Wait(100);

    tone(63, 1); Wait(100);
    tone(56, 1); Wait(100);
    tone(51, 1); Wait(100);
    tone(48, 1); Wait(100);
    tone(44, 1); Wait(100);
    tone(39, 1); Wait(100);
    tone(37, 1); Wait(100);

    tone(68, 1); Wait(100);
    tone(56, 1); Wait(100);
    tone(52, 1); Wait(100);
    tone(44, 1); Wait(100);
    tone(40, 1); Wait(100);
    tone(37, 1); Wait(100);
    tone(36, 1); Wait(100);*/


    /*32,33,32,33,32,
37,34,36,39,
63,56,51,48,44,39,37,
68,56,52,44,40,37,36,

32,33,32,33,32,
37,34,36,39,
63,56,51,48,44,39,37,
52,49,44,36,37,39*/

    if(SW2 == 0) while(1);
    Wait(500);
    LCDclear();

    /*while(1)
    {
        US_cm = Ultrasonic();
        LCDposition(1,1); printf("%i cm  ", US_cm);
        if(US_cm<20) SPEAKER(300);
        else SPEAKER(0);
        SERVO(ServoPos); ServoPos+=30; if(ServoPos>180) ServoPos=0;
        Wait(200);
    }*/

    power = 7500; powerTurn = 8000; step_time = 0;

    Pozice(PosX,PosY);
    //while(SW2_OFF) ShowRX();
    LCDposition(1, 1);
    prg = 0; printf("1");
    if (prg == 0) for (int i = 0; i < 100; i++) { Wait(10); if(SW2_ON) prg = 1;} printf("2");
    if (prg == 0) for (int i = 0; i < 100; i++) { Wait(10); if(SW2_ON) prg = 2;} printf("3");
    if (prg == 0) for (int i = 0; i < 100; i++) { Wait(10); if(SW2_ON) prg = 3;} printf("4");
    if (prg == 0) for (int i = 0; i < 100; i++) { Wait(10); if(SW2_ON) prg = 4;} printf("5");
    if (prg == 0) for (int i = 0; i < 100; i++) { Wait(10); if(SW2_ON) prg = 5;} printf("6");
    if (prg == 0) for (int i = 0; i < 100; i++) { Wait(10); if(SW2_ON) prg = 6;} printf("7");
    if (prg == 0) for (int i = 0; i < 100; i++) { Wait(10); if(SW2_ON) prg = 7;} printf("8");
    if (prg == 0) for (int i = 0; i < 100; i++) { Wait(10); if(SW2_ON) prg = 8;} printf("8");

    if(prg == 8) { withoutUZ = false; MOTOR(0, 0); while(1) { LCDclear(); LCDposition(1, 2); printf("TEST UZ"); whileUZ();} }

    if(prg == 7) lama();

    if (prg == 0) prg = 3;
    if (prg > 3) { prg -= 3; withoutUZ = true; }
    LCDclear();

    while (SW2_ON) W;
    while (SW2_OFF) W;

    Wait(30000);

    debugTL = false;
    if (prg > 0) { GoS();           GoS();  GoS(); TurnL(); GoS(); GoS();   TurnL(); GoS();    GoS();  ToLine(); }
    debugTL = false;
    if (prg > 1) { Turn180ToTurn(); GoS();  GoS(); GoS();   GoS(); TurnR(); GoS();   ToLine(); }
    debugTL = false;
    if (prg > 2) { Turn180();       GoS();  GoS(); TurnL(); GoS(); GoS();   TurnL(); GoS();    GoS();  GoS();    ToLine(); Turn180();}
    debugTL = true;

    LCDclear();
    LCDposition(1, 1);
    printf("HaHaHaRobotics.tk");
    LCDposition(1, 2);
    //printf("By Michal Hanus");

    while(1)
    {
        SmartRGB(iForRGB);
        iForRGB++;
        if(iForRGB > 7) break;
        Wait(100);
    }
    SmartRGB(green);

    /**GoS();while(SW2);
    TurnL();while(SW2);
    GoS();while(SW2);

    Wait(500);
    GoS(); PosY++;Pozice(PosX,PosY); //42
    GoS(); PosY++;Pozice(PosX,PosY); //43
    GoS(); PosY++;Pozice(PosX,PosY); //44
    GoS(); PosY++;Pozice(PosX,PosY); //45
    GoS(); PosY++;Pozice(PosX,PosY); //46
    TurnR();
    GoS(); PosX++;Pozice(PosX,PosY); //56
    GoS(); PosX++;Pozice(PosX,PosY); //66
    TurnR();
    GoS(); PosY--;Pozice(PosX,PosY); //65
    GoS(); PosY--;Pozice(PosX,PosY); //64
    GoS(); PosY--;Pozice(PosX,PosY); //63
    GoS(); PosY--;Pozice(PosX,PosY); //62
    TurnR();
    GoS(); PosX--;Pozice(PosX,PosY); //52
    GoS(); PosX--;Pozice(PosX,PosY); //42
    TurnL();
    GoS(); PosY--;Pozice(PosX,PosY); //41
    
    while(1)
    {
        ShowRX();
    }




    while(1)
    {
        US_cm = Ultrasonic();
        LCDposition(1,1); printf("%i cm  ", US_cm);
        if(US_cm<20) SPEAKER(300);
        else SPEAKER(0);
        SERVO(ServoPos); ServoPos+=30; if(ServoPos>180) ServoPos=0;
        Wait(200);
    }



    while(1)
    {
        //IMU();
        if(RX1==black) {MOTOR(0,0);SPEAKER(250);Wait(100);SPEAKER(0);MOTOR(-power,power);while(RX3!=black);LCDposition(1,2);printf("rx1");}
        else if(RX5==black) {MOTOR(0,0);SPEAKER(250);Wait(100);SPEAKER(0);MOTOR(power,-power);while(RX3!=black);LCDposition(1,2);printf("rx5");}
        else if(RX2==black) MOTOR(-power,power);
        else if(RX4==black) MOTOR(power,-power);
        else MOTOR(power,power);
        ShowRX();
       // LCDposition(1,2); printf("%i:%i:%.0f      ", (ACC_X/100), (ACC_Y/100), GyroZg);
      //  LCDposition(15,2); printf("%i%i", PosX, PosY);

    }



    while(1)
    {
        IMU();
        ShowRX();
        US_cm = Ultrasonic();
        //if(US_cm<20) SPEAKER(500); else SPEAKER(0);

        LCDposition(11,1); printf("%i cm  ", US_cm);
        //LCDposition(1,2); printf("Z:%.0f R:%.0f K:%.0f ", Zvedani, Rotace, Kompas);
        LCDposition(1,2); printf("%i:%i:%.0f      ", (ACC_X/100), (ACC_Y/100), GyroZg);
        LCDposition(15,2); printf("%i%i", PosX, PosY);
        SERVO(ServoPos); ServoPos+=45; if(ServoPos>180) ServoPos=0;
        Wait(1000);
    }*/
}
void lama()
{
    Wait(200);
    while(SW2_ON);
    while(SW2_OFF);
    GoS();
    TurnR();
    TurnR();
    GoS();
    MOTOR(power, power);
    Wait(4000);
    MOTOR(0, 0);
    while(1);
}
/*
    if(SW2_ON) { Write_EEPROM(); while(1); }
    Read_EEPROM();
    LCDclear();
    showprogram(program);


    Past_Encoder = RX_BACK;

    for(int i=0;i<50;i++)
    {
        ShowRX();
        Wait(100);
    }

    int pwr=5000;

    while(1)
    {
        SmartRGB(greenblue);
        LCDposition(1,1); printf("Rozhlizim se ...");
        SERVO(0);Wait(500);
        SERVO(180);Wait(500);
        SERVO(90);Wait(500);

       SmartRGB(green);
       LCDposition(1,1); printf("Jedu rovne !!!!!");
       MOTOR(7000,7000);
       Wait(2000);
       MOTOR(0,0);

       SmartRGB(blue);
       LCDposition(1,1); printf("Tocim doprava !!");
       SERVO(180);Wait(500);
       MOTOR(7000,-7000);
       Wait(2000);
       MOTOR(0,0);

       SmartRGB(yellow);
       LCDposition(1,1); printf  ("Tocim doleva !!!");
       SERVO(0);Wait(500);
       MOTOR(-7000,7000);
       Wait(2000);
       MOTOR(0,0);

       SmartRGB(red);
       LCDposition(1,1); printf("Couvam !!!      ");
       SERVO(90);Wait(500);
       MOTOR(-7000,-7000);
       Wait(2000);
       MOTOR(0,0);
    }

   while(1)
    {
        if(RX1==blackx || RX5==blackx) {MOTOR(0,0);Wait(5000);}
        else if(RX3==blackx) MOTOR(pwr,pwr);
        else if(RX2==blackx) MOTOR(0,pwr);
        else if(RX4==blackx) MOTOR(pwr,0);
     //   ShowRX();
    }


    while(1)
    {
        SmartRGB(greenblue);
        LCDposition(1,1); printf("Rozhlizim se ...");
        SERVO(0);Wait(500);
        SERVO(180);Wait(500);
        SERVO(90);Wait(500);

       SmartRGB(green);
       LCDposition(1,1); printf("Jedu rovne !!!!!");
       MOTOR(7000,7000);
       Wait(2000);
       MOTOR(0,0);
       
       SmartRGB(blue);
       LCDposition(1,1); printf("Tocim doprava !!");
       SERVO(180);Wait(500);
       MOTOR(7000,-7000);
       Wait(2000);
       MOTOR(0,0);
      
       SmartRGB(yellow);
       LCDposition(1,1); printf  ("Tocim doleva !!!");
       SERVO(0);Wait(500);
       MOTOR(-7000,7000);
       Wait(2000);
       MOTOR(0,0);
       
       SmartRGB(red);
       LCDposition(1,1); printf("Couvam !!!      ");
       SERVO(90);Wait(500);
       MOTOR(-7000,-7000);
       Wait(2000);
       MOTOR(0,0);
         
    }   
  
  
  

    switch(program)
    {
        case 0: loadingDirectionStart = left;
                loadingDirection = right;
                unloadingDirection = left;
                HomeDirection = straight;
                break;
        case 1: loadingDirectionStart = left;
                loadingDirection = straight;
                unloadingDirection = straight;
                HomeDirection = left;
                break;
        case 2: loadingDirectionStart = straight;
                loadingDirection = left;
                unloadingDirection = right;
                HomeDirection = right;
                break;
        case 3: loadingDirectionStart = straight;
                loadingDirection = right;
                unloadingDirection = left;
                HomeDirection = left;
                break;
        case 4: loadingDirectionStart = right;
                loadingDirection = straight;
                unloadingDirection = straight;
                HomeDirection = right;
                break;
        case 5: loadingDirectionStart = right;
                loadingDirection = left;
                unloadingDirection = right;
                HomeDirection = straight;
                break;
        case 6: unloadingDirection = left;
                HomeDirection = right;
//                Manual();
                break;
        case 7: unloadingDirection = straight;
                HomeDirection = straight;
//                Manual();
                break;
        case 8: unloadingDirection = right;
                HomeDirection = left;
//                Manual();
                break;
    }
    // </editor-fold> 

     currentDirection = loadingDirectionStart;

     while(1)
     {
         //if(MS10%10==0) {LCDposition(6,2);printf("%is", MS10/10);}
         showtime(0);

         //else {
         if(last != black) SmartRGB(black);
         if(RX2) Direction = 0;
         else if(RX4) Direction = 1;
         else if(RX3) Direction = 2;
         if(Direction == 0) lstDirction = left;
         if(Direction == 1) lstDirction = right;

         if (Direction == 0) {MOTOR(0, power);}
         if (Direction == 1) {MOTOR(power, 0);}
         if (Direction == 2) {MOTOR(powerslow, powerslow);}

        }
}
*/