#include <Mega32.h>
#include <stdio.h>
#include <delay.h> 
//timeout values for each task

#define begin {
#define end }
#define maxkeys 16
#define nopush 1
#define maybepush 2
#define pushed 3
#define maybenopush 4
#define t1 20  // scan the keypad every 20
#define t2 40  // Display something on LCD every 40ms
#define t3 100 // send new siganl every 100ms
#define LCDwidth 16 //characters

#asm
    .equ __lcd_port=0x12
#endasm
#include <lcd.h> // LCD driver routines

char lcd_buffer[17];
unsigned char key,newkey,butnum,PushingFlag, modeFlag;
unsigned char RcSpeedSetFlag, AutoSpeedSetFlag, AutoModeSetConfirmFlag, RcModeSetConfirmFlag;
unsigned char SendModeFlag, SendRcSpeedFlag, SendAutoSpeedFlag, StopRcFlag; 
int time1, time2, time3;   
int pushstate, RcSpeed, AutoSpeed, speed;
//key pad scan table
flash unsigned char keytbl[16]={0xee, 0xed, 0xeb, 0xde, 0xdd, 0xdb, 0xbe, 0xbd, 0xbb, 0xe7, 0xd7, 0xb7, 0x77, 0x7d, 0x7e, 0x7b};
 
void signal(void); //transimitting siganls  
void keyscan(void); //scan the keypad
void HandleKey(void); //match the keys to which signal to send
void LcdDisplay(void); // Display something on LCD
void initialize(void); //all the usual mcu stuff 

//**********************************************************
//timer 0 compare ISR
interrupt [TIM0_COMP] void timer0_compare(void)
begin      
  //Decrement the three times if they are not already zero
  if (time1>0)	--time1;
  if (time2>0) 	--time2;
  if (time3>0) 	--time3;
end  

//**********************************************************           
//**********************************************************       
//Entry point and task scheduler loop
void main(void)
begin  
  initialize();
  
  //main task scheduler loop 
  while(1)
  begin     
    if (time1==0) keyscan();
    if (time2==0) LcdDisplay();
    if (time3==0) signal();
  end
end  
  
//*******************************    
void signal(void) 
begin
    time3 = t3;
    if (StopRcFlag == 0)
    begin
          if (PushingFlag == 1)
          begin
          switch(butnum)
            {
              case 5: //signal to move forward
                PORTB.1 = 1;
                PORTB.2 = 0;
                PORTB.5 = 1;
                PORTB.7 = 0;
                break;
              case 8: //signal to move backward
                PORTB.1 = 0;
                PORTB.2 = 1;
                PORTB.5 = 0;
                PORTB.7 = 1;
                break;
              case 7: //signal to turn left
                PORTB.1 = 0;
                PORTB.2 = 1;
                PORTB.5 = 1;
                PORTB.7 = 0;
                break;
              case 9: //signal to turn right
                PORTB.1 = 1;
                PORTB.2 = 0;
                PORTB.5 = 0;
                PORTB.7 = 1;
                break;
              default://signal to stop
                PORTB.1 = 0;
                PORTB.2 = 0;
                PORTB.5 = 0;
                PORTB.7 = 0;
                break;
            }
            end  
            else
            begin
                PORTB.1 = 0;
                PORTB.2 = 0;
                PORTB.5 = 0;
                PORTB.7 = 0;
            end
      end
      
      else if (SendModeFlag == 0)
      begin
        if (SendRcSpeedFlag == 1 || SendAutoSpeedFlag == 1)
        begin
          if (SendRcSpeedFlag == 1) 
          begin
          speed = RcSpeed;
          StopRcFlag = 0;
          end
          if (SendAutoSpeedFlag == 1) speed = AutoSpeed;
          switch(speed) //determine which speed level to send
            {
              case 1: //signal to set speed to level 1
                PORTB.1 = 1;
                PORTB.2 = 1;
                PORTB.5 = 0;
                PORTB.7 = 0;
                break;
              case 2: //signal to set speed to level 2
                PORTB.1 = 1;
                PORTB.2 = 1;
                PORTB.5 = 0;
                PORTB.7 = 1;
                break;
              case 3: //signal to set speed to level 3
                PORTB.1 = 1;
                PORTB.2 = 1;
                PORTB.5 = 1;
                PORTB.7 = 0;
                break;
            }
          SendRcSpeedFlag =0;
          SendAutoSpeedFlag =0;            
        end
        else
        begin
          PORTB.1 = 0;
          PORTB.2 = 0;
          PORTB.5 = 0;
          PORTB.7 = 0;          
        end
      end
      
      else
      begin
        switch(modeFlag)  // determine which mode to send
            {
              case 1:  //signal to set mode to remote control
                PORTB.1 = 0;
                PORTB.2 = 0;
                PORTB.5 = 0;
                PORTB.7 = 1;
                StopRcFlag = 0;
                break;
              case 2:  //signal to set mode to auto
                PORTB.1 = 0;
                PORTB.2 = 0;
                PORTB.5 = 1;
                PORTB.7 = 1;
                StopRcFlag = 1;
                break;
              default:
                PORTB.1 = 0;
                PORTB.2 = 0;
                PORTB.5 = 0;
                PORTB.7 = 0;
                break;               
            }
        SendModeFlag = 0;
      end
//       delay_ms(50);
end 
  
//********************************************************** 
//Set it all up
void initialize(void)
begin
//set up the ports
  DDRB=0xff;	     

  pushstate = nopush;
  PushingFlag = 0;
  modeFlag = 0;
  RcSpeedSetFlag =0;
  AutoSpeedSetFlag = 0;
  
  SendModeFlag = 0;
  SendRcSpeedFlag = 0;
  SendAutoSpeedFlag =0;
  StopRcFlag = 1;
  
  RcModeSetConfirmFlag = 0;
  AutoModeSetConfirmFlag = 0;
  RcSpeed = 3;
  AutoSpeed = 3;
  speed = 3;
  
  TIMSK=2;		//turn on timer 0 cmp match ISR 
  OCR0 = 250;  	//set the compare re to 250 time ticks
  	//prescalar to 64 and turn on clear-on-match
  TCCR0=0b00001011;
  
  time1 = t1;
  time2 = t2;
  time3 = t3;
  lcd_init(LCDwidth);     //initialize the display
  lcd_clear();       	  //clear the display

  
    
  //crank up the ISRs
  #asm
  	sei
  #endasm 
end
//********************************************************** 
//standard keypad scan with debounce
void keyscan(void)
  {   
  	time1 = t1;
  	//get lower nibble
  	 DDRC = 0x0f;
  	 PORTC = 0xf0; 
  	 delay_us(5);
  	 key = PINC;
  	  
  	 //get upper nibble
  	 DDRC = 0xf0;
  	 PORTC = 0x0f; 
  	 delay_us(5);
  	 key = key | PINC;
  	  
  	 //find matching keycode in keytbl
  	 if (key != 0xff)
  	 	{   
  	    for (butnum=0; butnum<maxkeys; butnum++)
  	     	{    
  	        if (keytbl[butnum]==key)  break;   
  	    	}
  	   	if (butnum==maxkeys) butnum=0;
  	   	else butnum++;	   //adjust by one to make range 1-16
  	 	}  
  	 else butnum=0;
  	 
  	 
switch(pushstate){
	case nopush:
	if(butnum!=0) // button pushed?
	   pushstate = maybepush;
	else 
	   pushstate = nopush;
	break;
	
	case maybepush:
	if(butnum!=0)
	   pushstate = pushed;
	else 
	   pushstate = nopush;
	break;
	
	case pushed:
	if(butnum!=0)                                       // button pushed?
	  begin
	  pushstate = pushed;
	  newkey = butnum;
	  PushingFlag = 1;
	  end
	else 
	  pushstate = maybenopush;
	break;
	
	case maybenopush:
	if(butnum!=0) // button pushed?
	  pushstate =  pushed;
	else
	  pushstate = nopush;
	  PushingFlag = 0;
	  HandleKey();
        break;	
	} 
  } 
  
//**********************************************************
  void HandleKey(void)
  begin
    if (modeFlag == 0)  //indicate to set mode to
    begin
      SendModeFlag = 1;
      if(newkey == 1) modeFlag = 1;  //indicate to set mode to remote control
      if(newkey == 2) modeFlag = 2;  //indicate to set mode to auto
    end
    
    if (modeFlag == 1) 
    begin
      if (RcSpeedSetFlag == 0 && AutoModeSetConfirmFlag == 0)
      begin
        if(newkey == 10) //Change to auto mode?
        begin
        AutoModeSetConfirmFlag = 1;
        StopRcFlag = 1;
        end
        if(newkey == 11) //Want to set speed?
        begin 
        RcSpeedSetFlag = 1;
        StopRcFlag = 1;
        end
      end
      else if (RcSpeedSetFlag == 1 && AutoModeSetConfirmFlag == 0) 
      begin          
        if(newkey == 1)  //indicate to set speed to level 1
        begin 
        RcSpeed = 1;
        RcSpeedSetFlag = 0;
        SendRcSpeedFlag = 1;
        end
        
        if(newkey == 2) //indicate to set speed to level 2
        begin 
        RcSpeed = 2;
        RcSpeedSetFlag = 0;
        SendRcSpeedFlag = 1;
        end
                
        if(newkey == 3) //indicate to set speed to level 3
        begin 
        RcSpeed = 3;
        RcSpeedSetFlag = 0;
        SendRcSpeedFlag = 1;
        end
      end
      else if (RcSpeedSetFlag == 0 && AutoModeSetConfirmFlag == 1)
      begin
        if(newkey == 15)  //indicate to confirm
        begin
        AutoModeSetConfirmFlag = 0;
        modeFlag = 2;
        SendModeFlag = 1;
        end
        
        if(newkey == 16)  //indicate to cancle
        begin  
          AutoModeSetConfirmFlag = 0;
          StopRcFlag = 0;
        end
      end  
    end
    
    if (modeFlag == 2)
    begin
      if (AutoSpeedSetFlag == 0 && RcModeSetConfirmFlag == 0)
      begin
        if(newkey == 12) RcModeSetConfirmFlag = 1;  //Change to remote control mode?
        if(newkey == 13) AutoSpeedSetFlag = 1;   //Want to set speed?
      end
      else if (AutoSpeedSetFlag == 1 && RcModeSetConfirmFlag == 0) 
      begin  
        if(newkey == 1)   //indicate to set speed to level 1
        begin 
        AutoSpeed = 1;
        AutoSpeedSetFlag = 0;
        SendAutoSpeedFlag = 1;
        end
        
        if(newkey == 2)  //indicate to set speed to level 2
        begin 
        AutoSpeed = 2;
        AutoSpeedSetFlag = 0;
        SendAutoSpeedFlag = 1;
        end
                
        if(newkey == 3)  //indicate to set speed to level 3
        begin 
        AutoSpeed = 3;
        AutoSpeedSetFlag = 0;
        SendAutoSpeedFlag = 1;
        end
      end
      
      else if (AutoSpeedSetFlag == 0 && RcModeSetConfirmFlag == 1)
      begin
        if(newkey == 15) 
        begin
          RcModeSetConfirmFlag = 0;
          modeFlag = 1;
          SendModeFlag = 1;
        end
        
        if(newkey == 16)
          RcModeSetConfirmFlag = 0;
      end  
    end
  end
  
//**********************************************************  
// Interface between human and remote control
void LcdDisplay(void)
begin
   time2 = t2;
   if (modeFlag == 0)
    begin
      lcd_clear();
      lcd_gotoxy(0,0);  	 
      lcd_putsf("Choose a mode:"); 	
      lcd_gotoxy(0,1); 	    
      lcd_putsf(" 1)RC   2)Auto"); 
    end
    
    if (modeFlag == 1)
    begin
      if (RcSpeedSetFlag == 0 && AutoModeSetConfirmFlag == 0)
      begin
        lcd_clear();
        sprintf(lcd_buffer,"M:RC  Speed:%d",speed);
        lcd_gotoxy(0,0);  	 
        lcd_puts(lcd_buffer);	
        lcd_gotoxy(0,1); 	    
        lcd_putsf("A)Auto B)Set sp");
      end
      else if (RcSpeedSetFlag == 1 && AutoModeSetConfirmFlag == 0) 
      begin  
        lcd_clear();	 
        lcd_putsf("Set speed to:");	
        lcd_gotoxy(0,1); 	    
        lcd_putsf("1(Low) 2 3(High)");
      end
      else if (RcSpeedSetFlag == 0 && AutoModeSetConfirmFlag == 1)
      begin
        lcd_clear();
        lcd_gotoxy(0,0);  	 
        lcd_putsf("Set to Auto Mode");	
        lcd_gotoxy(0,1); 	    
        lcd_putsf("*)OK  #)Cancel");
      end  
    end
    
    if (modeFlag == 2)
    begin
      if (AutoSpeedSetFlag == 0 && RcModeSetConfirmFlag == 0)
      begin
        lcd_clear();
        sprintf(lcd_buffer,"M:Auto  Speed:%d",speed);
        lcd_gotoxy(0,0);  	 
        lcd_puts(lcd_buffer);	
        lcd_gotoxy(0,1); 	    
        lcd_putsf("C)RC  D)Set sp");
      end
      else if (AutoSpeedSetFlag == 1 && RcModeSetConfirmFlag == 0) 
      begin  
        lcd_clear();	 
        lcd_putsf("Set speed to:");	
        lcd_gotoxy(0,1); 	    
        lcd_putsf("1(Low) 2 3(High)");
      end
      else if (AutoSpeedSetFlag == 0 && RcModeSetConfirmFlag == 1)
      begin
        lcd_clear();
        lcd_gotoxy(0,0);  	 
        lcd_putsf("Set to RC Mode?");	
        lcd_gotoxy(0,1); 	    
        lcd_putsf("*)OK  #)Cancel");
      end  
    end
end