#include <xc.h>
#include <stdio.h>
#include <math.h>
#include <plib/timers.h>
#include <plib/pwm.h>
#include <stdbool.h>
#include "Tractorbot.h"

// PIC18F46K22 configurations

#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

// Pin definitions

// RA0-RA7 = LCD
#define INFRA               PORTBbits.RB0   // IR-receiver for remote control systems SFH 5110-38
//#define LDR_N             PORTBbits.RB1
//#define LDR_R             PORTBbits.RB2
//#define LDR_L             PORTBbits.RB3
//#define LDR_D             PORTBbits.RB4
#define RX_L                PORTBbits.RB5  // reflective object sensor QRD1114 front left
#define LED_R               LATBbits.LATB6 // red LED
#define LED_B               LATBbits.LATB7 // blue LED

//RC0-RC1 = secondary oscillator
#define LCDRS               LATCbits.LATC2 // LCD RS
#define LCDE                LATCbits.LATC3 // LCD enable
#define M_step1             LATCbits.LATC4 // stepper motor phaze 1 via ULN2803A
#define M_step2             LATCbits.LATC5 // stepper motor phaze 2 via ULN2803A
#define M_step3             LATCbits.LATC6 // stepper motor phaze 3 via ULN2803A
#define M_step4             LATCbits.LATC7 // stepper motor phaze 4 via ULN2803A

// PORTDbits.RD0 battery voltage measurement
#define speaker             LATDbits.LATD1
#define M_IN4_right_motor   LATDbits.LATD2   // right DC motor input signal to L293E
#define M_IN3_right_motor   LATDbits.LATD3   // right DC motor input signal to L293E
#define M_IN2_left_motor    LATDbits.LATD4   // left  DC motor input signal to L293E
#define M_IN1_left_motor    LATDbits.LATD5   // left  DC motor input signal to L293E
//RD6 left  motor current measurement
//RD7 right motor current measurement

#define US_IN              PORTEbits.RE0     // Ultrasonic ranging module HC-SR04 input
#define US_OUT             LATEbits.LATE1    // Ultrasonic ranging module HC-SR04 output
#define RX_R               PORTEbits.RE2     // reflective object sensor QRD1114 front right

//sound bytes

#define hello   4105
#define dog     2212
#define battery 3224
#define OK      6709
#define ohno    7150

//IR remote control buton codes
#define CTR_A1 28260
#define CTR_A2 28324
#define CTR_A3 24164
#define CTR_A4 17640
#define CTR_A5 17576
#define CTR_A6 26916
#define CTR_A7 29988
#define CTR_A8 18148
#define CTR_A9 17828

#define CTR_B1 28388
#define CTR_B2 28452
#define CTR_B3 20386
#define CTR_B4 18792
#define CTR_B5 18664
#define CTR_B6 27108
#define CTR_B7 XXXXX
#define CTR_B8 18020
#define CTR_B9 19364

#define CTR_C1 28516
#define CTR_C2 28580
#define CTR_C3 20900
#define CTR_C4 18920
#define CTR_C5 18536
#define CTR_C6 26788
#define CTR_C7 26825
#define CTR_C8 18084
#define CTR_C9 17764

#define CTR_D1 17508
#define CTR_D2 17572
#define CTR_D3 19496
#define CTR_D4 18600
#define CTR_D5 18344
#define CTR_D6 27172
#define CTR_D7 26724
#define CTR_D8 26596
#define CTR_D9 17700

#define forward       1
#define reverse       2
#define Stop_drive    3
#define Turn_right    10
#define Turn_left     20
#define Stop_turning  30

#define s       1000000
#define ms      1000
#define us      1

#define not_black  0
#define black      1

#define _XTAL_FREQ  64000000
#define USE_AND_MASKS

//variable declarations

const unsigned char full_char=0b11111111, circle=0b01001111, arrow_left='<', arrow_right='>', arrow_up=0b11011001, arrow_down=0b11011010;
unsigned char command, start=1;
int OSCFrequency,IRpulsePeriod[17];

unsigned int TMR0_LSB, TMR0_MSB, TMR0_total;
int Current_left_motor, Current_right_motor,US=0;
unsigned long step_time;

unsigned long TMR0_interrupt_number, Battery_voltage;
float LED_timer;

//procedure 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();

int LDRleft(int samples);
int LDRright(int samples);
int LDRFront(int samples);
int LDRBack(int samples);
int Get_Battery(int samples);
int Get_Current_left_motor(int samples);
int Get_Current_right_motor(int samples);

int Speaker(unsigned int sound);

unsigned long Get_US();

int MotorControl(int Motor_move, bool blink);

void Get_IR_times();
int Decode_IR_signal();

void Delay(unsigned long b);
void Wait(float time,long unit);
void Stopwatch_ON();
unsigned long Stopwatch_OFF();
void interrupt Interruption();

long PrintNumberMys(unsigned long number, int position, int row);
long PrNuM(long number);

int sumo();
int light_seeker();
int line_follower();
int object_avoider();
int doggie();

int start_menu();

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

int start_menu()
{
    while(1)
    {
        LCDposition(1,1);
        LCDwrite(0b11011001);
        printf("%i",(long)LDRFront(1000));
        printf("mV ");
        LCDwrite(0b11011010);
        printf("%i",(long)LDRBack(1000));
        printf("mV       ");

        LCDposition(14,1);
        LCDwrite(((full_char-circle)*!RX_R)+circle);
        printf(" ");
        LCDwrite(((full_char-circle)*!RX_L)+circle);

        LCDposition(1,2);
        printf("<");
        printf("%i",(long)LDRleft(1000));
        printf("mV >");
        printf("%i",(long)LDRright(1000));
        printf("mV      ");
    }
    return 0;
}

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

int LDRleft(int samples)
{
    long j2,result=0;
    for(j2=0;j2<samples;j2++)
    {     
        ADCON0=0b00100101;    // set channel RB3(AN9)(PIN 36)
	ADCON0=0b00100111;    // Conversion ON
    	while(ADCON0==0b00100111);	// waiting for conversion
        result+=ADRES;
    }
    return (( ((long)result / (long) samples)*(long)1000)/(long)1025);
}

int LDRFront(int samples)
{
    long j2,result=0;
    for(j2=0;j2<samples;j2++)
    {     //          ||
        ADCON0=0b00101001;    // set channel RB1(AN10)(PIN 34)
	ADCON0=0b00101011;    // Conversion ON
    	while(ADCON0==0b00101011);	// waiting for conversion
        result+=ADRES;
    }
    return (int) (( ((long)result / (long) samples)*(long)1000)/(long)1025);
}

int LDRright(int samples)
{
    long j2,result=0;
    for(j2=0;j2<samples;j2++)
    {     //          ||
        ADCON0=0b00100001;    // set channel RB2(AN8)(PIN 35)
	ADCON0=0b00100011;    // Conversion ON
    	while(ADCON0==0b00100011);	// waiting for conversion
        result+=ADRES;
    }
    return (int) (( ((long)result / (long) samples)*(long)1000)/(long)1025);
}

int LDRBack(int samples)
{
    long j2,result=0;
    for(j2=0;j2<samples;j2++)
    {     //          ||
        ADCON0=0b00101101;    // set channel RB4(AN11)(PIN 37)
	ADCON0=0b00101111;    // Conversion ON
        while(ADCON0==0b00101111);	// waiting for conversion
        result+=ADRES;
    }
    return (int) (( ((long)result / (long) samples)*(long)1000)/(long)1025);
}

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


int Get_Battery(int samples)
{
    long j2,result=0;

    for(j2=0;j2<samples;j2++)
    {
        ADCON0=0b01010001;    // set channel RD0(AN20)(PIN 19)
	ADCON0=0b01010011;    // Conversion ON
    	while(ADCON0==0b01010011);	// waiting for conversion
        result+=ADRES;
    }
    return (result/samples);
}

int Get_Current_left_motor(int samples)
{
    long j2,result=0;

    for(j2=0;j2<samples;j2++)
    {
        ADCON0=0b01101001;    // set channel RD6
	ADCON0=0b01101011;    // Conversion ON
    	while(ADCON0==0b01101011);	// waiting for conversion
        result+=ADRES;
    }
    return (result/samples);
}

int Get_Current_right_motor(int samples) // M1-Current_right_motor = channel RD7 = AN27
{
    long j2,result=0;

    for(j2=0;j2<samples;j2++)
    {
        ADCON0=0b01101101;    // set channel RD7
	ADCON0=0b01101111;    // Conversion ON
    	while(ADCON0==0b01101111);	// waiting for conversion
        result+=ADRES;
    }
    return (result/samples);
}

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

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

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

void PICinit()
{
 BSR=15;

 TRISA=0;  //all output=0,input=1
 ANSELA=0; //all digital, 0 = digi, 1 = analog
 PORTA=0;
 LATA=0;

 TRISB=0b00111111;
 ANSELB=0b00011110;  
 PORTB=0;
 LATB=0;

 INTCON2=0;
 WPUB=0b00000001; //pull-up on RB0

 TRISC=0;
 ANSELC=0;
 PORTC=0;
 LATC=0;

 TRISD= 0b11000001;
 ANSELD=0b11000001;
 PORTD=0;
 LATD=0;

 TRISE=0b1101;
 ANSELE=0;
 PORTE=0;
 LATE=0;

 OSCCON=0b01110000;  // 16MHz internal oscillator
 OSCCON2=0b10000000; // 4xPLL=64MHz
 OSCTUNEbits.PLLEN=1; //PLL on
 OSCFrequency=64;
 switch(OSCFrequency)
 {
     case 4:  T0CON=0b10001000;break;
     case 8:  T0CON=0b10000000;break;
     case 16: T0CON=0b10000001;break;
     case 32: T0CON=0b10000010;break;
     case 64: T0CON=0b10000011;break;
 }

 
 VREFCON0=0b10010000; //setting voltage reference, range 1.024V
 while(VREFCON0==0b10010000); //waiting for setting voltage reference

 ADCON2=0b10111110; // right adjustment, delay 20TAD, frequency conversion FOSC/64
 ADCON1=0b00001000; // internal voltage reference

 // setting interrupts
    INTCONbits.GIE=1;  // global enable all interrupts
    INTCONbits.INT0IE=1; //  INT0 interrupts enable
    INTCONbits.INT0IF=0;  // clear INT0 interrupt flag
    INTCON2bits.INTEDG0=0; // interrupts at INT0 from 1 to 0 (falling edge)
}

//******************************************************************************
// TIMING PROCEDURE

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


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

void LCDinit()
{
  Wait(17,ms);

  LCDRS=0;
  command=0b00110000;	// setting Function Set for 8bit
  LCDsend(command);
  
  Wait(6,ms);

  command=0b00110000;	// setting Function Set for 8bit
  LCDsend(command);
  
  command=0b00110000;	// setting Function Set for 8bit
  LCDsend(command);
  
  command=0b00111000;	// 2line display, 5x8 points
  LCDsend(command);

  command=0b00001100;	// display on, cursor, blink
  LCDsend(command);
  
  command=0b00000001;	// clear display
  LCDsend(command);
  Wait(6,ms);

  command=0b00000110;	// position add, shift off
  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;
  Wait(1,ms);
  LCDE=0;
  Wait(1,ms);
}

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


//***********************************************************************************************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//***********************************************************************************************************************************************************
void main()
{
    PICinit();
    LCDinit();
    int j;

    Battery_voltage=(Get_Battery(1000)*13.383); //conversion to mV
    LCDposition(1,1);
    printf("Battery:      mV");
    PrintNumberMys(Battery_voltage, 9,1);

    LCDposition(1,2);
    printf("Capacity:");
    PrNuM(((Battery_voltage-10750)*50)/1000);
    printf("%%");

    Speaker(hello);
    Speaker(hello);
    Speaker(hello);

    Speaker(battery);
    
    if(Battery_voltage>11000) Speaker(OK);
    else Speaker(ohno);

    for(j=0;j<15;j++)
    {
        LED_R=0;
        LED_B=1;
        Wait(100,ms);
        LED_R=0;
        LED_B=1;
        Wait(100,ms);

        LED_B=0;
        LED_R=1;
        Wait(100,ms);
        LED_B=0;
        LED_R=1;
        Wait(100,ms);
    }
    LED_R=0;

    LCDclear();

    LCDposition(1,1);
    printf("line-avoid-sumo");
    LCDposition(1,2);
    printf("light-doggie");
    while(1);
}
//***********************************************************************************************************************************************************
//*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*-*
//***********************************************************************************************************************************************************

void Get_IR_times()
{
        int j;
        while(INFRA==1);
        for(j=1;j<16;j++)
        {
            while(INFRA==0);
            Stopwatch_ON();
            while(INFRA==1);
            IRpulsePeriod[j]=Stopwatch_OFF();
        }
}

int Decode_IR_signal()
{
    Get_IR_times();
    int j,TotalIRcode=0;
    char IRcode[17];

    for(j=1;j<16;j++)
    {
            if(IRpulsePeriod[j]>300 && IRpulsePeriod[j]<1000) IRcode[j]=0;
            else if(IRpulsePeriod[j]>1300 && IRpulsePeriod[j]<2000) IRcode[j]=1;
            else {IRcode[j]=9;}
    }
    for(j=1;j<16;j++) TotalIRcode=TotalIRcode+IRcode[j]*(pow(2,j));

    return TotalIRcode;
}

void Wait(float timefloat, long unit)
{
     unsigned long timelong, remainingtime;
     int overflow=0;

     timelong=(unsigned long)(timefloat*unit);
     overflow=timelong/65535;
     remainingtime=65535-(timelong-(overflow*65535));

     WRITETIMER0(remainingtime);
     INTCONbits.TMR0IF=0;

    do
    {
        while(INTCONbits.TMR0IF==0);
        INTCONbits.TMR0IF=0;
        overflow--;
    }
    while(overflow>=0);
}

void Stopwatch_ON()
{
    INTCONbits.TMR0IE=1;
    INTCONbits.TMR0IF=0;
    TMR0_interrupt_number=0;
    WRITETIMER0(0);
}

unsigned long Stopwatch_OFF()
{
    TMR0_total=READTIMER0();
    INTCONbits.TMR0IE=0;
    INTCONbits.TMR0IF=0;
  
    return ((TMR0_interrupt_number*65536)+ TMR0_total);
}

void interrupt Interruption(void)
{
    if(INTCONbits.TMR0IF==1 && INTCONbits.TMR0IE==1)
    {
        TMR0_interrupt_number++;
        INTCONbits.TMR0IF=0;
        return;
    }

    if(INTCONbits.INT0IF==1 && INTCONbits.INT0IE==1)
    {
       INTCONbits.INT0IE=0;
       switch(Decode_IR_signal())
       {
                case CTR_A1: MotorControl(forward, true); break;
                case CTR_A2: MotorControl(Stop_drive, true); break;
                case CTR_A3: MotorControl(reverse, true); break;

                case CTR_B1: MotorControl(Turn_left, true); break;
                case CTR_C1: MotorControl(Stop_drive, true); break;
                case CTR_D1: MotorControl(Turn_right, true); break;

                case CTR_A9: start_menu(); LCDclear(); break;

                case CTR_A8: if(start==1) {start=0; line_follower();} break;
                case CTR_B8: if(start==1) {start=0; object_avoider();} break;
                case CTR_C8: if(start==1) {start=0; sumo();} break;
                case CTR_D8: if(start==1) {start=0; light_seeker();} break;
                case CTR_D7: if(start==1) {start=0; doggie();} break;
       }
       INTCONbits.INT0IE=1;
       INTCONbits.INT0IF=0;
       return;
    }
}

long PrintNumberMys(unsigned long number, int position, int row)
{
    unsigned int j,DigitsNo;
    unsigned long exp=1,number_backup=number;
    unsigned char NUMBERS[20];
    position--;
    for(j=0;j<17;j++)
    {
        if(exp>number) break;
        exp=exp*10;
    }
    if(j==(17-position)) return 1;
    DigitsNo=j;

    if(number==0)
    {
        LCDposition(position+1,row);
        printf("0");
        return 0;
    }

    for(j=DigitsNo;j>0;j--)
    {
        NUMBERS[j]=number%10;
        number=number/10;
    }

    for(j=1;j<=DigitsNo;j++)
    {
       LCDposition(position+j,row);
       LCDwrite(NUMBERS[j]+48);
    }
    return number_backup;
  
}

long PrNuM(long number)
{
    unsigned int j,DigitsNo;
    unsigned long exp=1,number_backup=number;
    unsigned char NUMBERS[20];
    
    for(j=0;j<17;j++)
    {
        if(exp>number) break;
        exp=exp*10;
    }
    DigitsNo=j;
    
    if(number==0)
    {
        printf("0");
        return 0;
    }
    
    for(j=DigitsNo;j>0;j--)
    {
        NUMBERS[j]=number%10;
        number=number/10;
    }
    
    for(j=1;j<=DigitsNo;j++)
    {
       LCDwrite(NUMBERS[j]+48);
    }
    return number_backup;
}


unsigned long Get_US()
{
    unsigned long time_us;
    US_OUT=1;
    Wait(12,us);
    US_OUT=0;

    Stopwatch_ON();
    while(US_IN==0 && TMR0_interrupt_number==0);
    Stopwatch_ON();
    while(US_IN==1 && TMR0_interrupt_number==0);
    time_us=Stopwatch_OFF();
    if((time_us/58)<=7) time_us=141*58;
    return (time_us/58); //returns value in cm

}
//******************************************************************************
int MotorControl(int Motor_move, bool blink)
{
        switch(Motor_move)
        {
            case reverse:
                M_IN1_left_motor=0;M_IN2_left_motor=1;
                M_IN3_right_motor=0;M_IN4_right_motor=1;
                if(blink==true) {LED_B=0;LED_R=0;} break;
            case forward:
                M_IN1_left_motor=1;M_IN2_left_motor=0;
                M_IN3_right_motor=1;M_IN4_right_motor=0;
                if(blink==true) {LED_B=0;LED_R=0;}break;
                
            case Stop_turning:
            case Stop_drive:
                M_IN1_left_motor=0;  M_IN2_left_motor=0;
                M_IN4_right_motor=0; M_IN3_right_motor=0;
                if(blink==true) {LED_B=0;LED_R=0;}break;
                
            case Turn_right:
                M_IN1_left_motor=0;M_IN2_left_motor=1;
                M_IN3_right_motor=1;M_IN4_right_motor=0;
                LED_R=blink;LED_B=0; break;
            case Turn_left:
                M_IN1_left_motor=1;M_IN2_left_motor=0;
                M_IN3_right_motor=0;M_IN4_right_motor=1;
                LED_B=blink;LED_R=0; break;
        }
        return Motor_move;
}
//******************************************************************************
//------------------------------------------------------------------------------
//******************************************************************************
int line_follower()
{
    Speaker(OK);

    LCDclear();
    LCDposition(1,1);
    printf("  Line follower");

    while(1)
    {
        LCDposition(6,2);
        LCDwrite(((full_char-circle)*!RX_R)+circle);
        printf(" ");
        LCDwrite(((full_char-circle)*!RX_L)+circle);

        Wait(50,ms);
        while(RX_R==black) MotorControl(Turn_right, true);    
        Wait(50,ms);
        while(RX_L==black) MotorControl(Turn_left, true);     
        if(Battery_voltage<10750) {MotorControl(Stop_drive, true);
        Speaker(battery); Speaker(ohno); }
        MotorControl(forward, true);                       
    }
}
//******************************************************************************
int sumo()
{
    const char right=1,centre=2,left=3;
    unsigned long US[3];

    Speaker(OK);

    while(1)
    {
        step_time=1000;
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[right]=Get_US();
        
        if      (RX_R==black || RX_L==black)
        {
            MotorControl(reverse, true); Wait(1,s);
            while(RX_R==not_black && RX_L==not_black);
            MotorControl(forward, true); Wait(1,s); continue;
        }

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[centre]=Get_US();
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[left]=Get_US();
        LCDclear();
        LCDposition(1,1);
        printf("sumocleaner");

        LCDposition(1,2);
        printf("<");
        PrNuM(US[left]);
        printf(" ");
        LCDwrite(0b11011001); //arrow up
        PrNuM(US[centre]);
        printf(" >");
        PrNuM(US[right]);
        printf("                   ");

        LCDposition(15,2);
        LCDwrite(((full_char-circle)*!RX_R)+circle);
        LCDwrite(((full_char-circle)*!RX_L)+circle);

        M_step1=0; M_step2=0; M_step3=0; M_step4=0;

        if(Battery_voltage<10750) {MotorControl(Stop_drive, true); Speaker(battery); Speaker(ohno); }

        if      (RX_R==black || RX_L==black)
        {
            MotorControl(reverse, true); Wait(1,s);
            while(RX_R==not_black && RX_L==not_black);
            MotorControl(forward, true); Wait(1,s); continue;
        }
        else if((US[centre]<= US[left]) && (US[centre]<= US[right] && US[centre]< 100)) MotorControl(forward, true);
        else if((US[right]<=US[centre]) && (US[right]<=US[left]  && US[right]<100)) MotorControl(Turn_right, true);
        else if((US[left]<= US[centre]) && (US[left]<= US[right] && US[left]< 100)) MotorControl(Turn_left, true);
        else                                                                           MotorControl(Stop_drive, true);
    }

    return 0;
}

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

int light_seeker()
{
    int LDRFwd,LDRRev,LDRLft,LDRRgt;

    Speaker(OK);

    while(1)
    {
        LDRFwd=LDRFront(1000);
        LDRRev=LDRBack(1000);
        LDRLft=LDRleft(1000);
        LDRRgt=LDRright(1000);

        printf(" ");
        LCDposition(1,1);
        LCDwrite(0b11011001);
        printf("%i",(long)LDRFwd);
        printf("mV ");
        LCDwrite(0b11011010);
        printf("%i",(long)LDRRev);
        printf("mV       ");

        printf(" ");
        LCDposition(1,2);
        printf("<");
        printf("%i",(long)LDRLft);
        printf("mV >");
        printf("%i",(long)LDRRgt);
        printf("mV      ");

        if(Battery_voltage<10750) {MotorControl(Stop_drive, true); Speaker(battery); Speaker(ohno); }

        if((LDRFwd>=LDRRev) && (LDRFwd>=LDRLft) && (LDRFwd>=LDRRgt)) MotorControl(forward, false);
        else if((LDRRev>=LDRFwd) && (LDRRev>=LDRLft) && (LDRRev>=LDRRgt)) MotorControl(reverse, false);
        else if((LDRLft>=LDRFwd) && (LDRLft>=LDRRev) && (LDRLft>=LDRRgt)) MotorControl(Turn_left, false);
        else MotorControl(Turn_right, false);
    }

    return 0;
}

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

int object_avoider()
{
    const char right=1,centre=2,left=3;
    unsigned long US[3];
    int panic=0;
    
    Speaker(OK);

    while(1)
    {
        step_time=1000;
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[right]=Get_US();

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[centre]=Get_US();
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[left]=Get_US();
        
        LCDclear();
        LCDposition(1,1);
        printf("object_avoider");

        LCDposition(1,2);
        printf("<");
        PrNuM(US[left]);
        printf(" ");
        LCDwrite(0b11011001); //arrow up
        PrNuM(US[centre]);
        printf(" >");
        PrNuM(US[right]);
        printf("                   ");


        M_step1=0; M_step2=0; M_step3=0; M_step4=0;

        if(Battery_voltage<10750) {MotorControl(Stop_drive, true); Speaker(battery); Speaker(ohno); }

        if((US[right]<30 && US[centre]<30 && US[left]<30) || panic>=6) {MotorControl(Turn_right, true);Wait(5,s); panic=0;}
        else if(US[right]<30) { MotorControl(Turn_left, true);  if(panic % 2==0) panic++;}
        else if(US[left] <30) { MotorControl(Turn_right, true); if(panic % 2==1) panic++;}
        else if(US[centre] <30)
        {
            if(US[right]>=US[left]) MotorControl(Turn_right, true);
            if(US[left]>US[right]) MotorControl(Turn_left, true);
            panic=0;
        }
        else MotorControl(forward, true);
    }
    return 0;
}

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

int doggie()
{
    const char right=1,centre=2,left=3;
    unsigned long US[3];
    
    Speaker(dog);
    Speaker(dog);
    Speaker(dog);
    Speaker(dog);
    Speaker(dog);

    while(1)
    {
        step_time=1000;
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //right

        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //right
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[right]=Get_US();

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[centre]=Get_US();
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left

        M_step1=1; M_step2=0; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=1; M_step3=0; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=1; M_step4=0; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=1; Delay(step_time);  //left
        M_step1=0; M_step2=0; M_step3=0; M_step4=0;
        US[left]=Get_US();
        
        LCDclear();
        LCDposition(1,1);
        printf("doggie");

        LCDposition(1,2);
        printf("<");
        PrNuM(US[left]);
        printf(" ");
        LCDwrite(0b11011001); //arrow up
        PrNuM(US[centre]);
        printf(" >");
        PrNuM(US[right]);
        printf("                   ");

        M_step1=0; M_step2=0; M_step3=0; M_step4=0;

        if(Battery_voltage<10750) {MotorControl(Stop_drive, true); Speaker(battery); Speaker(ohno); }

        if((US[centre]<= US[left]) && (US[centre]<= US[right] && US[centre]< 30)) MotorControl(forward, true);
        else if((US[right]<=US[centre]) && (US[right]<=US[left]  && US[right]<30)) MotorControl(Turn_right, true);
        else if((US[left]<= US[centre]) && (US[left]<= US[right] && US[left]< 30)) MotorControl(Turn_left, true);
        else {                                                                        MotorControl(Stop_drive, true); Speaker(dog);}
    }
    return 0;
}

int Speaker(unsigned int sound)
{
    unsigned int size,i;
    
    OSCCON=0b01100000; //8 MHz internal oscillator
    OSCTUNEbits.PLLEN=0;
    PMD1=0;

    // setting PWM
    TRISDbits.TRISD1=1;
    CCPTMRS1=0;

    PR2=249;

    CCP4CON=0b00001111; //PWM on
    CCPR4L=0;
    PIR1bits.TMR2IF=0;
    T2CON=0b00000100; //prescaler 1:1
    TRISDbits.TRISD1=0;
    if(sound==hello)
    {
        size=hello;
        for(i=0;i<size;i++)
        {
            CCPR4L=hello1b[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }

    if(sound==dog)
    {
        size=dog;
        for(i=0;i<size;i++)
        {
            CCPR4L=dog1[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }
    if(sound==battery)
    {
        size=battery;
        for(i=0;i<size;i++)
        {
            CCPR4L=battery1[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }
    if(sound==OK)
    {
        size=OK;
        for(i=0;i<size;i++)
        {
            CCPR4L=OK1[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }
    if(sound==ohno)
    {
        size=ohno;
        for(i=0;i<size;i++)
        {
            CCPR4L=ohno1[i];
            while(PIR1bits.TMR2IF==0);
            PIR1bits.TMR2IF=0;
        }
    }
    OSCCON=0b01110000; //16 MHz internal oscillator
    OSCTUNEbits.PLLEN=1;
    return 0;
}