//Programming : Manual Robot
//Author:

#include <pic.h>

#define SPEEDL	CCPR1L  //PWM1
#define SPEEDR	CCPR2L  //PWM2

//__CONFIG(0x3F32);

void forward();
void reverse();
void left();
void right();
void stop();
void up();
void down();
void besi_tengah_turun();
void check_signal();


void delay(unsigned long data);
unsigned char temp=50;


//==============MAIN SETTING========================================//
void main()
{
	ADCON1= 0x00;    //diffuse analog port for digital use except RA4
	TRISB = 0B11111111;  //set Port B as INPUT
	TRISA = 0B010000;    //set RA4 untuk input dari limit switch
	PORTA = 0B000000;    //clear port A
	TRISC = 0B00011000;	
	TRISD = 0B00000000;
	PORTD = 0B00000000;
			
	PR2 		= 	255;		// set period register 
	T2CON     	=  	0b00000100;	
	CCP1CON 	=	0b00001100;	// config for RC1 to generate PWM( for more detail refer datasheet section 'capture/compare/pwm')
	CCP2CON 	=	0b00001100;	// config for RC2 to generate PWM
	check_signal();	
}

void check_signal()
{
RD4 = 0;
RD5 = 0;

while(1)
  {

	if (RB2==0)if(RB3==1)if(RB4==1)if(RB5==1)if (RC3==1)if(RC4==1)
		
		forward();
		
	if (RB2==1)if(RB3==0)if(RB4==1)if(RB5==1)if (RC3==1)if(RC4==1)
		
		left();
		
	if (RB2==1)if(RB3==1)if(RB4==0)if(RB5==1)if (RC3==1)if(RC4==1)
		
		right();
		
	if (RB2==1)if(RB3==1)if(RB4==1)if(RB5==0)if (RC3==1)if(RC4==1)
		
		reverse();

	if (RB2==1)if(RB3==1)if(RB4==1)if(RB5==1)if (RC3==1)if(RC4==0)
		
		down();

	if (RB2==1)if(RB3==1)if(RB4==1)if(RB5==1)if (RC3==0)if(RC4==1)
		
		up();

	if (RB2==1)if(RB3==1)if(RB4==1)if(RB5==1)if (RC3==0)if(RC4==0)
		
		besi_tengah_turun();

		
	if (RB2==1)if(RB3==1)if(RB4==1)if(RB5==1)if (RC3==1)if(RC4==1)
		
		stop();
	
	
        
	}
}
//======================DELAY FUNCTIONS=============================//
void delay(unsigned long data)
{
	for( ;data>0;data-=1);
}


void reverse()
{
	SPEEDR=30;
	SPEEDL=50;
	PORTA=0B000110;
	delay(20000);
	PORTA=0B000000;
	SPEEDR=70;
	SPEEDL=100;
	PORTA=0B000110;
	delay(20000);
	PORTA=0B000000;
}

void forward()
{
SPEEDR=30;
SPEEDL=50;
PORTA=0B001001;
delay(20000);
PORTA=0B000000;
SPEEDR=70;
SPEEDL=100;
PORTA=0B001001;
delay(20000);	
PORTA=0B000000;	
}

void left()
{
	SPEEDR=30;
	SPEEDL=50;
	PORTA=0B001010;
	delay(10);
	PORTA=0B000000;
}

void right()
{
	SPEEDR=30;
	SPEEDL=50;
	PORTA=0B000101;
	delay(10);
	PORTA=0B000000;
}

			
void stop()
{

	PORTA=0B0000000;
}

void up()
{
	RD4 = 1;
	RD5 = 0;
	delay(10);
	PORTD=0B00000000;
}

void down()
{
	RD5 = 1;
	RD4 = 0;
	delay(10);
	PORTD=0B00000000;
}

//routine for up and down half auto center carrier//

void besi_tengah_turun()
{
	RD7=1;
		while(1)
			{
				if(RA4==1)
				{
					delay(1000);
					RD7=0;				//clear port RD0
					delay(500000);		//delay 5 seconds
					RD6=1;				//naik
					delay(20000);		//delay 2 seconds for up
					RD6=0;
				}
			}
}

return 0;
}