#include <mega16.h>
#include <delay.h>
// Alphanumeric LCD Module functions
#asm
   .equ __lcd_port=0x15 ;PORTC
#endasm
#include <lcd.h>

// Declare your global variables here
bit bk,j;
void motor(float p1,float p2)
{
   if(p1>=0)OCR1AL=(int)p1;else OCR1AL=-(int)p1;
   if(p2>=0)OCR1BL=(int)p2;else OCR1BL=-(int)p2;
   if(p1>0)
   {
      PORTD.0=1;
      PORTD.1=0;  
   }else
   {
      PORTD.0=0;
      PORTD.1=1;
   }    
   if(p2>0)
   {
      PORTD.2=1;
      PORTD.3=0;  
   }else
   {
      PORTD.2=0;
      PORTD.3=1;
   }
}
void main(void)
{
// Declare your local variables here

// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In 
// State7=P State6=P State5=P State4=P State3=P State2=P State1=P State0=P 
PORTA=0xFF;
DDRA=0x00;

// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In 
// State7=P State6=P State5=P State4=P State3=P State2=P State1=P State0=P 
PORTB=0xFF;
DDRB=0x00;

// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In 
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T 
PORTC=0x00;
DDRC=0x00;

// Port D initialization
// Func7=Out Func6=Out Func5=Out Func4=Out Func3=Out Func2=Out Func1=Out Func0=Out 
// State7=0 State6=0 State5=0 State4=0 State3=0 State2=0 State1=0 State0=0 
PORTD=0x00;
DDRD=0xFF;

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=FFh
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;

// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 31.250 kHz
// Mode: Fast PWM top=00FFh
// OC1A output: Non-Inv.
// OC1B output: Non-Inv.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer 1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
TCCR1A=0xA1;
TCCR1B=0x0C;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;

// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer 2 Stopped
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;

// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
MCUCR=0x00;
MCUCSR=0x00;

// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;

// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

// LCD module initialization
lcd_init(16);

while (1)
{  
   if(PINA.6==1) j=1.275; else j=1;
   lcd_gotoxy(0,0);
   lcd_putchar(PINB.4+'0');
   lcd_putchar(PINB.3+'0');
   lcd_putchar(PINB.2+'0');
   lcd_putchar(PINB.1+'0');
   lcd_putchar(PINB.0+'0');
   lcd_putchar(PINA.5+'0');
   lcd_putchar(PINA.4+'0');
   lcd_putchar(PINA.3+'0');
   lcd_putchar(PINA.2+'0');
   lcd_putchar(PINA.1+'0');
   lcd_putchar(PINA.0+'0'); 
   lcd_gotoxy(11,0);
   if(bk)lcd_putsf("Black");else lcd_putsf("White");
   if((PINA.5==1&&PINA.4+PINA.3+PINA.2+PINA.1+PINA.0>=4&&PINB.4+PINB.3+PINB.2+PINB.1+PINB.0==0&&!bk)||
   (PINA.5==0&&PINA.4+PINA.3+PINA.2+PINA.1+PINA.0<=1&&PINB.4+PINB.3+PINB.2+PINB.1+PINB.0==5&&bk))
   {
      motor(200*j,-200*j);
      lcd_gotoxy(0,1);  
      lcd_putsf("Left 60 ");
      delay_ms(300);
      continue;
   }
   if((PINA.5==1&&PINA.4+PINA.3+PINA.2+PINA.1+PINA.0==0&&PINB.4+PINB.3+PINB.2+PINB.1+PINB.0>=4&&!bk)||
   (PINA.5==0&&PINA.4+PINA.3+PINA.2+PINA.1+PINA.0==5&&PINB.4+PINB.3+PINB.2+PINB.1+PINB.0<=1&&bk))
   {
      motor(-200*j,200*j);
      lcd_gotoxy(0,1);  
      lcd_putsf("Right 60 ");
      delay_ms(300);
      continue;
   }
   if((PINA.5==1&&!bk)||(PINA.5==0&&bk))//MOSTAGHIM
   {
      motor(200*j,200*j);
      lcd_gotoxy(0,1);  
      lcd_putsf("Straight");
      delay_ms(10);
      continue;
   }
   if(PINA.4+PINA.3+PINA.2+PINA.1+PINA.0>=2&&PINB.4+PINB.3+PINB.2+PINB.1+PINB.0>=2)bk=1;else bk=0;
   if((PINB.4==1&&!bk)||(PINB.4==0&&bk))
   {
      motor(200*j,-200*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Left 5  ");
   }
   if((PINB.3==1&&!bk)||(PINB.3==0&&bk))
   {
      motor(200*j,-80*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Left 4  ");
   }
   if((PINB.2==1&&!bk)||(PINB.2==0&&bk))
   {
      motor(200*j,-30*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Left 3  ");
   }
   if((PINB.1==1&&!bk)||(PINB.1==0&&bk))
   {
      motor(200*j,0*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Left 2  ");
   }
   if((PINB.0==1&&!bk)||(PINB.0==0&&bk))
   {
      motor(200*j,30*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Left 1  ");
   }
   if((PINA.0==1&&!bk)||(PINB.0==0&&bk))
   {
      motor(200*j,-200*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Right 5 ");
   }
   if((PINA.1==1&&!bk)||(PINB.1==0&&bk))
   {
      motor(200*j,-80*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Right 4 ");
   }
   if((PINA.2==1&&!bk)||(PINB.2==0&&bk))
   {
      motor(200*j,-30*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Right 3 ");
   }
   if((PINB.3==1&&!bk)||(PINB.3==0&&bk))
   {
      motor(200*j,0*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Right 2 ");
   }
   if((PINB.4==1&&!bk)||(PINB.4==0&&bk))
   {
      motor(200*j,30*j);
      lcd_gotoxy(0,1);
      lcd_putsf("Right 1 ");
   }
};
}