//used as an example in the program organization doc.                   
#include <Mega32.h>
#include <delay.h> 
//timeout values for each task

#define t1 4000 //send a 20us pulse every 40ms
#define begin {
#define end }
 

void StopPulse(void); //Stop the pulse to trigger the ultrasound sensor  	
void SendPulse(void); // Send a pulse	
void Sensor1(void);  // Sense distance at sensor 1;
void Sensor2(void);  // Sense distance at sensor 2;
void auto(void);     // Autonomous mode
void Handle(void);   // Handle the receiving signals;

void initialize(void); //all the usual mcu stuff          
unsigned char signal1, signal2, signal3, signal4; //store 4 bit receiving signals
unsigned char triggerCount, ModeFlag,speedFlag;			
unsigned int echoPluseCount1,  meas1;
unsigned int echoPluseCount2,  meas2;			 
unsigned char TriggerFlag, EchoFlag1,EchoFlag2, stopFlag1, stopFlag2, stuckFlag;
unsigned int time1, time2;
         

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

//timer 0 compare ISR
  interrupt [TIM0_COMP] void timer0_compare(void)
begin      
   if (triggerCount>0 && TriggerFlag == 1) --triggerCount;
   if (time1>0) --time1;
   if (time2>0 && stuckFlag ==1) --time2;
   Sensor1();
   Sensor2();
  
end 

//**********************************************************    
//Entry point and task scheduler loop
void main(void)
begin  
  initialize();
  //main task scheduler loop 
  while(1)
  begin
    Handle();   
  end
end  
  
//*******************************   
//Handle the signal 
void Handle(void) 
begin
  if (triggerCount==0 && TriggerFlag == 1) StopPulse();
  if (time1==0) SendPulse();
  if (time2==0) stuckFlag = 0;
  
  signal1 = PINC.0;  //generate the messages
  signal2 = PINC.1;
  signal3 = PINC.2;
  signal4 = PINC.3;
   
  //set speed to level 1
  if (signal1 == 1 && signal2 == 1 && signal3 == 0 && signal4 == 0)
  begin
    OCR1A = 2000;
    speedFlag = 1; 
  end
  
  //set speed to level 2        
  else if (signal1 == 1 && signal2 == 1 && signal3 == 0 && signal4 == 1)
  begin
    OCR1A = 7500;
    speedFlag = 2; 
  end
  
  //set speed to level 3        
  else if (signal1 == 1 && signal2 == 1 && signal3 == 1 && signal4 == 0)
  begin
    OCR1A = 19900;
    speedFlag = 3; 
  end
  
  //set mode to auto mode
  else if (signal1 == 0 && signal2 == 0 && signal3 == 1 && signal4 == 1)
  begin
    DDRB.3 = 1;
    DDRB.1 = 1;
    delay_us(20);
    ModeFlag = 2;

  end
  
  //set mode to remote control mode
  else if (signal1 == 0 && signal2 == 0 && signal3 == 0 && signal4 == 1)
    begin
       DDRB.3 = 0;
       DDRB.1 = 0;
       delay_us(20);
       ModeFlag = 1;
    end
  
  //Handle the remote control mode  
  else if (ModeFlag == 1)
  begin
          //move forward
          if (signal1 == 1 && signal2 == 0 && signal3 == 1 && signal4 == 0)
          begin
             PORTC.7 = 1;
             PORTC.6 = 0;
             PORTC.5 = 1;
             PORTC.4 = 0;
          end
          
          //move backward
          else if (signal1 == 0 && signal2 == 1 && signal3 == 0 && signal4 == 1)
          begin
             PORTC.7 = 0;
             PORTC.6 = 1;
             PORTC.5 = 0;
             PORTC.4 = 1;
          end
          
          //Left turn
          else if (signal1 == 1 && signal2 == 0 && signal3 == 0 && signal4 == 1)
          begin
             PORTC.7 = 1;
             PORTC.6 = 0;
             PORTC.5 = 0;
             PORTC.4 = 1;
          end
          
          //Right turn
          else if (signal1 == 0 && signal2 == 1 && signal3 == 1 && signal4 == 0)
          begin
             PORTC.7 = 0;
             PORTC.6 = 1;
             PORTC.5 = 1;
             PORTC.4 = 0;
          end
          
          //stop 
          else
          begin
             PORTC.7 = 0;
             PORTC.6 = 0;
             PORTC.5 = 0;
             PORTC.4 = 0;
          end
  end
  
  //Go to auto mode
  else if (ModeFlag == 2) auto();
  
  //stop  
  else
  begin
    PORTC.7 = 0;
    PORTC.6 = 0;
    PORTC.5 = 0;
    PORTC.4 = 0;
  end 
end 


//**********************************************************          
void StopPulse(void) 
begin  
  TriggerFlag = 0; 
  PORTB.3 = 0;
  PORTB.1 = 0;
end  
//**********************************************************
void SendPulse(void) 
begin
  time1=t1;
  TriggerFlag = 1;
  triggerCount = 2; 
  PORTB.3 = 1;
  PORTB.1 = 1;
end 

//********************************************************** 
void Sensor1(void) 
begin
  //start counting
  if (PINB.4 == 1 && EchoFlag1 == 0)
  begin 
    echoPluseCount1 = 0;
    EchoFlag1 = 1;
  end
  
  //increase the counter by 1
  else if (PINB.4 == 1 && EchoFlag1 == 1) echoPluseCount1++;
  
  //stop counting if dectecting something
  else if ((PINB.4 == 0 && EchoFlag1 == 1) || echoPluseCount1>2000)
  begin 
    EchoFlag1 = 0;
    meas1 = echoPluseCount1;
    if (meas1 > 90) stopFlag1 =0; //detect if the distance is too small
    else stopFlag1 = 1;
  end
end 
  
//********************************************************** 
void Sensor2(void) 
begin
  //start counting
  if (PINB.2 == 1 && EchoFlag2 == 0)
  begin 
    echoPluseCount2 = 0;
    EchoFlag2 = 1;
  end
  
  //increase the counter by 1
  else if (PINB.2 == 1 && EchoFlag2 == 1) echoPluseCount2++;
  
  //stop counting if dectecting something
  else if ((PINB.2 == 0 && EchoFlag2 == 1) || echoPluseCount2>2000)
  begin 
    EchoFlag2 = 0;
    meas2 = echoPluseCount2;
    if (meas2 > 90) stopFlag2 =0;  //detect if the distance is too small
    else stopFlag2 = 1;
  end
end
//**********************************************************
void auto(void)
begin
  //turn left for a while if got stucked
  if (stuckFlag==1)  
  begin
    PORTC.7 = 1;
    PORTC.6 = 0;
    PORTC.5 = 0;
    PORTC.4 = 1;
  end
  
  // turn left if sensor1 detect a low distance
  else if (stopFlag1 == 1 && stopFlag2 == 0)
  begin
    PORTC.7 = 1;
    PORTC.6 = 0;
    PORTC.5 = 0;
    PORTC.4 = 1;
  end
  
  // turn right if sensor2 detect a low distance
  else if (stopFlag1 == 0 && stopFlag2 == 1)
  begin
    PORTC.7 = 0;
    PORTC.6 = 1;
    PORTC.5 = 1;
    PORTC.4 = 0;
  end
  
  //start a timer to count how long to turn left if got stucked
  else if (stopFlag1 == 1 && stopFlag2 == 1)
  begin
    stuckFlag=1;
    if (speedFlag == 1) time2 = 10000;
    if (speedFlag == 2) time2 = 20000;
    if (speedFlag == 3) time2 = 50000;
  end
  
  //move forward
  else
  begin
    PORTC.7 = 1;
    PORTC.6 = 0;
    PORTC.5 = 1;
    PORTC.4 = 0;
  end
end

void initialize(void)
begin
//set up the ports
  

  DDRA=0;
  DDRD =0xff;
  DDRC=0xf0;
  
  DDRB=0;	 
  PORTC.7 = 0;
  PORTC.6 = 0;
  PORTC.5 = 0;
  PORTC.4 = 0;
  ModeFlag = 0;    
  //init the message
  signal1=0;
  signal2=0;
  signal3=0;
  signal4=0; 
  
  //set up timer 0  
  TIMSK=2;	
  OCR0 = 20; // set to 20 ticks to count to 10us
  	//prescalar to 8 and turn on clear-on-match
  TCCR0=0b00001010;
  
  //set up timer1 prescale to 256 and use the Phase and Frequency correct PWM
  // Top = ICR1
  TCCR1A=0x80;
  TCCR1B=0x14;
  TCNT1H=0x00;
  TCNT1L=0x00;
  
  //
  ICR1H = 0x4e;
  ICR1L = 0x20;
  OCR1A = 20000;
  OCR1BH=0x00;
  OCR1BL=0x00;

  TriggerFlag = 0; 
  EchoFlag1 = 0;
  echoPluseCount1 = 0;
  
  EchoFlag2 = 0;
  echoPluseCount2 = 0;
  triggerCount = 0;
  stuckFlag = 0;
   
  time1=t1;
  time2=15000;
     
  //crank up the ISRs
  #asm
  	sei
  #endasm 
end