#include <xc.h>
#include <stdio.h>
#include <stdbool.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 RX6                 PORTBbits.RB1
#define RX5                 PORTBbits.RB2
#define RX3                 PORTBbits.RB3
#define RX1                 PORTBbits.RB4
#define RX0                 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_IN2          LATCbits.LATC2
// SCL RC3
// SDA RC4
#define ENCODER             PORTCbits.RC5
//DRIVER_ENA PWM2_A         LATCbits.LATC6
//#define ENK1A               PORTCbits.RC7

#define DRIVER_IN1          LATDbits.LATD0
// BATTERY                             RD1 (AN21)
#define DRIVER_IN4          LATDbits.LATD2
#define DRIVER_IN3          LATDbits.LATD3
//SERVO PWM3_F              LATDbits.LATD4
//#define ENK1B               PORTDbits.RD5
//#define ENK2A               PORTDbits.RD6
//#define ENK2B               PORTDbits.RD7

//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 blackx 1
#define out    0

#define left     (0)
#define right    (2)
#define straight (1)
const unsigned char rx_black=0b11111111, rx_out=0b01001111, arrow_left='<', arrow_right='>', arrow_up=0b11011001;
const unsigned int power = 7000, powerslow = 5800;

bool load = false, crossBool = false;
unsigned char command, Past_Encoder, Direction = 2, lstDirction;
long Vbat, ADC_Distance, Encoder_Ticks=0;
int Distance=100, MS100 = 0, MS100str = 0, MS100lo = 0, MS100un, cross = 0, program = 10;
unsigned char loadingDirection = 0, loadingDirectionStart, unloadingDirection = 0, HomeDirection = 0, currentDirection = 0, start = true;


// 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 Delay(unsigned long b);
void Wait(long ms);

long Battery(int samples);
long SHARP(int samples);
void MOTOR(int pwr_l, int pwr_r);
void MOTOR_L(long power_L);
void MOTOR_R(long power_R);
void Turn(int dgr);
void SERVO_UP();
void SERVO_DOWN();
int IR_Distance(int IR_mereni);

int showrx();
void showtime(unsigned int tntnc);
void showprogram(int prg);
int Center();
int Center2();
int TurnCross(int msec);

int Crossing(unsigned char smer);
void Loading();
void Unloading();
void Home();

void Nastav();
void Manual();
void UnloadingM();

void interrupt ProgramInterrupt();

//************************************************************************************************************************************************************
// 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);
}

//******************************************************************************
// SHARP GP2 ADC_Distance measurement
long SHARP(int samples)   // RB0 = AN12
{
    long j2,result=0;
    ADCON0=0b00110001;      // 12bit ADC, channel AN12, 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=1; DRIVER_IN2=0; PSMC2DC = power_L*655.35;}
    else if(power_L<0) {DRIVER_IN1=0; DRIVER_IN2=1; 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=1; DRIVER_IN4=0; PSMC1DC = power_R*655.35;}
    else if(power_R<0) {DRIVER_IN3=0; DRIVER_IN4=1; PSMC1DC = (-power_R)*655.35;}
    else               {DRIVER_IN3=0; DRIVER_IN4=0; PSMC1DC = power_R*655.35;}

    PSMC1CONbits.PSMC1LD=1;
}

void SERVO_UP()
{
     PSMC3DC = 3000;       // PWM duty cycle stop
     PSMC3CONbits.PSMC3LD=1;
}

void SERVO_DOWN()
{
     PSMC3DC = 1100;       // PWM duty cycle stop
     PSMC3CONbits.PSMC3LD=1;
}

int IR_Distance(int IR_mereni)
{
     ADC_Distance = SHARP(IR_mereni);
     if(ADC_Distance>=2499) Distance=3;
     else if(ADC_Distance>=2294) Distance=4;
     else if(ADC_Distance>=1925) Distance=5;
     else if(ADC_Distance>=1638) Distance=6;
     else if(ADC_Distance>=1434) Distance=7;
     else if(ADC_Distance>=1270) Distance=8;
     else if(ADC_Distance>=1147) Distance=9;
     else if(ADC_Distance>=1024) Distance=10;
     else if(ADC_Distance>=942) Distance=11;
     else if(ADC_Distance>=860) Distance=12;
     else if(ADC_Distance>=819) Distance=13;
     else if(ADC_Distance>=778) Distance=14;
     else if(ADC_Distance>=696) Distance=15;
     else if(ADC_Distance>=655) Distance=16;
     else if(ADC_Distance>=639) Distance=17;
     else if(ADC_Distance>=614) Distance=18;
     else if(ADC_Distance>=573) Distance=19;
     else if(ADC_Distance>=532) Distance=20;
     else if(ADC_Distance>=508) Distance=21;
     else if(ADC_Distance>=492) Distance=22;
     else if(ADC_Distance>=459) Distance=23;
     else if(ADC_Distance>=451) Distance=24;
     else if(ADC_Distance>=434) Distance=25;
     else if(ADC_Distance>=410) Distance=26;
     else if(ADC_Distance>=401) Distance=27;
     else if(ADC_Distance>=369) Distance=28;
     else if(ADC_Distance>=344) Distance=29;
     else if(ADC_Distance>=336) Distance=30;
     else if(ADC_Distance>=328) Distance=31;
     else if(ADC_Distance>=319) Distance=32;
     else if(ADC_Distance>=311) Distance=33;
     else if(ADC_Distance>=303) Distance=34;
     else if(ADC_Distance>=279) Distance=35;
     else if(ADC_Distance>=270) Distance=36;
     else if(ADC_Distance>=262) Distance=37;
     else if(ADC_Distance>=254) Distance=38;
     else if(ADC_Distance>=246) Distance=39;
     else Distance=40;

     LCDposition(1,2);printf("%icm ",Distance);

     return Distance;
}

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

 // I/O pins setting
 TRISA=0;          ANSELA=0;          LATA=0;
 TRISB=0b11111111; ANSELB=0b00000001; LATB=0;
 TRISC=0b10111000; ANSELC=0;          LATC=0;
 TRISD=0b11100010; 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 = 1100;       // 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 = 15535;
 }

//******************************************************************************
// 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);
}


//******************************************************************************
// 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 interrupt ProgramInterrupt()
{
    if(PIR1bits.TMR1IF==1)
    {
        PIR1bits.TMR1IF=0;
        T1CON = 0b00110001;  // FOSC/4 clock, 8x prescaler, timer on
        TMR1 = 15535;
        MS100++;
    }
}

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

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

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

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

         if(RX6==0) {LCDposition(9,1);LCDwrite(rx_out);}
         else {LCDposition(9,1);LCDwrite(rx_black);}

         if(ENCODER==0) {LCDposition(14,1);LCDwrite(rx_out);}
         else {LCDposition(14,1);LCDwrite(rx_black);}

         if(SW2_ON) {LCDposition(16,1);LCDwrite(rx_black);MOTOR(0,0);}
         else {LCDposition(16,1);LCDwrite(rx_out);}
         return 0;
}
void showtime(unsigned int tntnc)
{
    int prstr = 0, prun = 0;
    LCDposition(1, 1);
    if(MS100str == 0) prstr = MS100;
    else prstr = MS100str;
    if(MS100str != 0) {
        if(MS100un < 0) prun = MS100un + MS100;
        else prun = MS100un;
    }
    if(tntnc > 0) printf("%i:%i:%i:%i", prstr, prun, MS100, MS100-tntnc);
    else printf("%i:%i:%i", prstr, prun, MS100);
}
void showprogram(int prg)
{
    switch(prg)
    {
        case 0: printf("0 V< K%c", arrow_up); break;
        case 1: printf("1 V< K>"); break;
        case 2: printf("2 V%c K<", arrow_up); break;
        case 3: printf("3 V%c K>", arrow_up); break;
        case 4: printf("4 V> K<"); break;
        case 5: printf("5 V> K%c", arrow_up); break;
        case 6: printf("6 K<   "); break;
        case 7: printf("7 K%c  ", arrow_up); break;
        case 8: printf("8 K>   "); break;
    }
}
//***********************************************************************************************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//***********************************************************************************************************************************************************
void main()
{
    PICinit();LCDinit();

    if(SW2_ON) { Nastav(); while(1); }

    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
    LCDclear();
    showprogram(program);
    LCDposition(2,2); printf("RoboRave 2016");
    Wait(500);
    LCDclear();
    Vbat = Battery(100) * 4.387;
    LCDposition(1,2); printf("Vbat: %i ", Vbat);
    // <editor-fold desc="processing directions">
    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>
    Past_Encoder = ENCODER;

    Center2();

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

         //else {
         if(last != black) SmartRGB(black);
         if(RX1) Direction = 0;
         else if(RX5) 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);}

         if(MS100str == 0) if(Crossing(loadingDirectionStart)) { MS100str = MS100; }
         if(MS100str == 0) continue;

         Crossing(currentDirection);
         Loading();
         Unloading();
   }
}
int Center()
{
    if (Direction == 0) {MOTOR(0,powerslow); if(last != blue) SmartRGB(blue);}
    else if (Direction == 1) {MOTOR(powerslow,0); if(last != green) SmartRGB(green);}
    else {if(last != white) SmartRGB(white); Center2(); return 1;}
    while(!RX3);
    return 0;
}
int Center2()
{
    MOTOR(powerslow, -powerslow);
    for(int i = 0;i < 400; i++) {__delay_ms(1); if(RX3) return 1; }
    MOTOR(-powerslow, powerslow);
    for(int i = 0;i < 800; i++) {__delay_ms(1); if(RX3) return 1;}
    MOTOR(powerslow, -powerslow);
    for(int i = 0;i < 400; i++) {__delay_ms(1); if(RX3) return 1;}
    return 0;
}
int Center2_smer(int smer)
{
    int nsb = 1;
    if(smer == left) nsb = -1;
    MOTOR(powerslow * nsb, -powerslow * nsb);
    for(int i = 0;i < 400; i++) {__delay_ms(1); if(RX3) return 1; }
    MOTOR(-powerslow * nsb, powerslow * nsb);
    for(int i = 0;i < 800; i++) {__delay_ms(1); if(RX3) return 1;}
    MOTOR(powerslow * nsb, -powerslow * nsb);
    for(int i = 0;i < 400; i++) {__delay_ms(1); if(RX3) return 1;}
    return 0;
}
int Center2_smerlit(int smer)
{
    int nsb = 1;
    if(smer == left) nsb = -1;
    MOTOR(powerslow * nsb, -powerslow * nsb);
    for(int i = 0;i < 100; i++) {__delay_ms(1); if(RX3) return 1; }
    MOTOR(-powerslow * nsb, powerslow * nsb);
    for(int i = 0;i < 200; i++) {__delay_ms(1); if(RX3) return 1;}
    MOTOR(powerslow * nsb, -powerslow * nsb);
    for(int i = 0;i < 100; i++) {__delay_ms(1); if(RX3) return 1;}
    return 0;
}
void MOTOR(int pwr_l, int pwr_r)
{
    MOTOR_L(pwr_l);
    MOTOR_R(pwr_r);
}
void Turn(int dgr)
{
     Encoder_Ticks = 0;
     while(Encoder_Ticks < (dgr/5))
     {
         Wait(5);
         if(ENCODER!=Past_Encoder)
         {
             Encoder_Ticks++;
             Past_Encoder=ENCODER;
         }
     }
}
bool x0, x6, xsmer;
int TurnCross(int msec)
{
    int prv = msec;
    for(; msec > 0;msec--) {__delay_ms(1); if((RX0 && x6) || RX1 || RX3 || RX5 || (RX6 && x0)) break;}//if(((RX0 || RX1) && x6) /*|| (RX1 || RX3 || RX5)*/ || ((RX5 || RX6) && x0)) break;}
    return prv - msec;
}
int TurnCross2(int dgr)
{
    int Encoder_Dgr = 0;
    while(Encoder_Dgr < dgr)
    {
  for(int i = 5;i > 0;i--) {__delay_ms(1); if((x6 && RX0) || RX1 || RX3 || RX5 || (RX6 && x0)) return Encoder_Dgr;}
        if(ENCODER!=Past_Encoder)
        {
            Encoder_Dgr += 5;
            Past_Encoder = ENCODER;
        }
    }
    return Encoder_Dgr;
}
int Crossing(unsigned char smer)
{
    crossBool = false;
    int ts;
    if(RX0 || RX6)
    {
        if(RX0 && RX6) return 0;
         //if(last != red) SmartRGB(red);
         x0 = RX0;
         x6 = !x0;
         if(x0) xsmer = right;
         else xsmer = left;
         if(x0 && last != red) SmartRGB(red);
         if(x6 && last != purple) SmartRGB(purple);

         Wt;
         MOTOR(power, power);
         if(x0) while(RX0);
         if(x6) while(RX6);
        Wait(50);
         Wt;
         if(last != green) SmartRGB(green);
         MOTOR(power * ((x6*2)-1), power * ((x0*2)-1));
         Wait(100);
         MOTOR(power * ((x0*2)-1), power * ((x6*2)-1));
        ts = TurnCross(600);
         Wt;
         if(ts != 600)
         {
             crossBool = true;
             if(last != greenblue) SmartRGB(greenblue);
             MOTOR(powerslow * ((x6*2)-1), powerslow * ((x0*2)-1));
            Wait(ts);
         }
         else
         {
             if(last != yellow) SmartRGB(yellow);
             MOTOR(powerslow * ((x6*2)-1), powerslow * ((x0*2)-1));
            Wait(600);
             while(!RX3);
         }
         MOTOR(0,0);
         Wt;
         if(crossBool)
         {
             if(smer == straight)
             {
                 if(last != blue) SmartRGB(blue);
                 MOTOR(0,0);Wait(200);
                 Center2_smer(xsmer);
                 MOTOR(0,0);Wait(200);
             }
             else MOTOR(0,0);Wait(200);
             Wt;
             if(last != white) SmartRGB(white);
            if(smer == left)  { MOTOR(-powerslow, powerslow); Wait(300); while(!RX3); }
            if(smer == right) { MOTOR(powerslow, -powerslow); Wait(300); while(!RX3); }
            if(smer == straight) { MOTOR(power, power); Wait(100); while(RX0 || RX6); }
             Wt;
             currentDirection = straight;
             return 1;
         }
         else
         {
             if(last != yellow) SmartRGB(yellow);
         }
         
         Direction = 2;
    }
    return 0;
}
void Loading()
{
    if(SW2/* || load*/) return; //SW is not push
    MOTOR(0,0);
    Wait(2000);
    MOTOR(-power, -power);
    Wait(100);
    MOTOR(-powerslow, powerslow);
    Wait(1000);
    while(!RX1 && !RX3 && !RX5);
    MOTOR(0,0);
    load = true;
    currentDirection = unloadingDirection;
    MS100un = -MS100;
}
void Unloading()
{
    if(!load) return;
    LCDposition(1, 2); printf("%i cm", IR_Distance(1));
    for(int i = 0;i < 25;i++) if(IR_Distance(1) > 11 || Direction != 2) return;
    MOTOR(0,0);
    Wait(500);
    Center2_smerlit(lstDirction);
    MOTOR(0,0);
    SERVO_UP();
    Wait(2000);
    SERVO_DOWN();
    MOTOR(-power, -power);
    Wait(100);
    MOTOR(powerslow, -powerslow);
    Wait(1000);
    while(!RX3);
    MOTOR(0,0);
    load = false;
    currentDirection = loadingDirection;
    MS100un =  MS100 + MS100un;
    if((3.3 * MS100un) + (1.1 * MS100str) + (2.2 * MS100lo) >= 1800 - MS100) Home();
}
void Home()
{
    LCDposition(1,2);
    printf("Jedu dom");
    int MScross = 0;
    while(1)
    {
        showtime(MScross);
        if(RX1==blackx) Direction = 0;
        else if(RX5==blackx) Direction = 1;
        else if(RX3) Direction = 2;

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

        if(Crossing(HomeDirection)) if(MScross == 0) MScross = MS100;

        if((MS100 - MScross) >= (MS100str - 30) && MScross != 0)
        {
            MOTOR(0,0);
            LCDclear();
            showtime(MScross);
            LCDposition(1,2);
            printf("HaHaHa!!!");
            while(1);
        }
    }
}
void Nastav()
{
     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;
}
//******************************************************************************
//*******************************  MANUAL DRIVE  *******************************
//******************************************************************************
void Manual()
{
     LCDposition(5,2);
     printf("Manual");
     currentDirection = loadingDirection;
     while(1)
     {
         //else {
         if(last != black) SmartRGB(black);
         if(RX1==blackx) Direction = 0;
         else if(RX5==blackx) Direction = 1;
         else if(RX3) Direction = 2;

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

         Crossing(currentDirection);
         UnloadingM();
   }
}
void UnloadingM()
{
    if(IR_Distance(10) > 10) return;
    Center();
    MOTOR(0,0);
    SERVO_UP();
    Wait(2000);
    SERVO_DOWN();
    MOTOR(-power, -power);
    Wait(100);
    MOTOR(powerslow, -powerslow);
    Wait(1000);
    while(!RX3);
    MOTOR(0,0);
    currentDirection = HomeDirection;
}
//*///END