//  Robot Pokud

//Import knihoven
#include <xc.h>
#include "SmartRGB.h"

// Konfigurace PIC18F26K22
#pragma config FOSC=INTIO67,PLLCFG=ON,PRICLKEN=OFF,FCMEN=OFF,IESO=OFF,PWRTEN=ON
#pragma config BOREN=OFF,BORV=190,WDTEN=OFF,WDTPS=512,PBADEN=OFF,MCLRE=INTMCLR
#pragma config STVREN=OFF,LVP=OFF,DEBUG=OFF,CP0=OFF,CP1=OFF,CP2=OFF,CP3=OFF
#pragma config CPB=OFF,CPD=OFF,WRT0=OFF,WRT1=OFF,WRT2=OFF,WRT3=OFF,WRTC=OFF
#pragma config WRTB=OFF,WRTD=OFF,EBTR0=OFF,EBTR1=OFF,EBTR2=OFF,EBTR3=OFF,EBTRB=OFF
#pragma config CCP3MX=PORTC6

// Definice I/O pinu
#define   IDC_A0           LATAbits.LATA0
#define   IDC_A1           LATAbits.LATA1
#define   IDC_A2           LATAbits.LATA2
#define   IDC_A3           LATAbits.LATA3
#define   M1_EN            LATAbits.LATA4
#define   IDC_A5           LATAbits.LATA5
#define   M1_IN2           LATAbits.LATA6
#define   M1_IN1           LATAbits.LATA7

#define   INFRA            PORTBbits.RB0
// RB1 = SCL2
// RB2 = SDA2
#define   M3_EN            LATBbits.LATB3
#define   M3_IN2           LATBbits.LATB4
#define   RGB              LATBbits.LATB5
// RB6 = PGC
// RB7 = PGD

#define   M2_IN1           LATCbits.LATC0
#define   M2_IN2           LATCbits.LATC1
#define   M2_EN            LATCbits.LATC2
// RC3 = Baterie
#define   M4_IN1           LATCbits.LATC4
#define   M4_IN2           LATCbits.LATC5
#define   M4_EN            LATCbits.LATC6
#define   M3_IN1           LATCbits.LATC7

#define   IDC_E3           PORTEbits.RE3

#define _XTAL_FREQ  32000000                           //  Frekvence oscilatoru PIC
#define USE_AND_MASKS


// Deklarace promennych
int Cas;

// Deklarace funkci
void PICinit();
void Delay(unsigned long b);
void Cekej(long ms);

//******************************************************************************
// Mereni napeti baterie
int Baterie(int vzorku)   // RC3 = AN15
{
    long j2,vysledek=0;

    for(j2=0;j2<vzorku;j2++)
    {
        ADCON0=0b00111101;               // nastaveni kanalu RC3(AN15), zapnuti ADC
	ADCON0=0b00111111;               // zapnuti konverze
    	while(ADCON0==0b00111111);	// cekani na dokonceni konverze
        vysledek+=ADRES;
    }
    return (vysledek/vzorku);
}

//******************************************************************************
// PIC initiation
void PICinit()
{
 BSR=15;

 // Nastaveni pinu PIC
 TRISA=0; ANSELA=0; LATA=0;
 TRISB=0b00000111; ANSELB=0; LATB=0;
 TRISC=0b00001000; ANSELC=0b00001000; LATC=0;

 INTCON2=0; TRISEbits.WPUE3=1;   //  pull-up odpor na RE3

 OSCCON=0b01100000;  // 8MHz interni oscilator
 OSCCON2=0b10000000; // 4xPLL=32 MHz interni oscilator
 OSCTUNEbits.PLLEN=1; //PLL zapnout
 
 ADCON2=0b10111010; // zarovnani ADC doprava, zpozdeni 20TAD, frekvence konverze FOSC/32
 ADCON1=0;  // napetova reference = VDD
}

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

//***********************************************************************************************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//***********************************************************************************************************************************************************

void Dopredu()
{
    M2_EN=1; M2_IN1=1; M2_IN2=0;
    M3_EN=1; M3_IN1=1; M3_IN2=0;
}

void Dozadu()
{
    M2_EN=1; M2_IN1=0; M2_IN2=1;
    M3_EN=1; M3_IN1=0; M3_IN2=1;
}

void Doprava()
{
    M1_EN=1; M1_IN1=1; M1_IN2=0;
    M4_EN=1; M4_IN1=1; M4_IN2=0;
}

void Doleva()
{
    M1_EN=1; M1_IN1=0; M1_IN2=1;
    M4_EN=1; M4_IN1=0; M4_IN2=1;
}

void Zastav()
{
    M1_EN=0; M1_IN1=0; M1_IN2=0;
    M2_EN=0; M2_IN1=0; M2_IN2=0;
    M3_EN=0; M3_IN1=0; M3_IN2=0;
    M4_EN=0; M4_IN1=0; M4_IN2=0;
}

void Rotace_Doleva()
{
    M1_EN=1; M1_IN1=0; M1_IN2=1;
    M2_EN=1; M2_IN1=0; M2_IN2=1;
    M3_EN=1; M3_IN1=1; M3_IN2=0;
    M4_EN=1; M4_IN1=1; M4_IN2=0;
}
void Rotace_Doprava()
{
    M1_EN=1; M1_IN1=1; M1_IN2=0;
    M2_EN=1; M2_IN1=1; M2_IN2=0;
    M3_EN=1; M3_IN1=0; M3_IN2=1;
    M4_EN=1; M4_IN1=0; M4_IN2=1;
}

void Sikmo_SV()
{
    Dopredu();
    Doprava();
}

void Sikmo_JV()
{
     Dozadu();
     Doprava();   
}

void Sikmo_SZ()
{
    Dopredu();
    Doleva();
}
void Sikmo_JZ()
{
    Dozadu();
    Doleva();
}

void main()
{
    PICinit();
    Cas = 2000;

    while(1)
    { 
        Dopredu(); SmartRGB(blue); Cekej(Cas);
        Zastav(); SmartRGB(nocolor); Cekej(Cas);
        Dozadu(); SmartRGB(yellow); Cekej(Cas);
        Zastav(); SmartRGB(nocolor);Cekej(Cas);
        Doleva(); SmartRGB(red); Cekej(Cas);
        Zastav(); SmartRGB(nocolor); Cekej(Cas);
        Doprava();SmartRGB(purple); Cekej(Cas);
        Zastav(); SmartRGB(nocolor); Cekej(Cas);
        Rotace_Doleva(); SmartRGB(greenblue); Cekej(Cas);
        Zastav();SmartRGB(nocolor); Cekej(Cas);
        Rotace_Doprava();SmartRGB(greenblue);Cekej(Cas);
        Zastav();SmartRGB(nocolor); Cekej(Cas);
        Sikmo_JV();SmartRGB(green);Cekej(Cas);
        Zastav(); SmartRGB(nocolor);Cekej(Cas);
        Sikmo_JZ();SmartRGB(green);Cekej(Cas);
        Zastav(); SmartRGB(nocolor);Cekej(Cas);
        Sikmo_SV(); SmartRGB(white); Cekej(Cas);
        Zastav(); SmartRGB(nocolor); Cekej(Cas);
        Sikmo_SZ(); SmartRGB(white);Cekej(Cas);
        Zastav(); SmartRGB(nocolor);Cekej(Cas);
  }
}