#include <10F206.h> 

#FUSES NOWDT                      //No Watch Dog Timer 
#FUSES NOMCLR                     //Master Clear pin: disable 
#FUSES NOPROTECT                  //No code protected from reads 

#use delay(clock=4000000)

#byte STATUS = 0x003 
#bit  DC = STATUS.1 
#bit  PD = STATUS.3 
#bit  TO = STATUS.4 
#bit  GPWUF = STATUS.7 
#byte GPIO = 0x006 
#bit  GP0 = GPIO.0 //SWITCH
#bit  GP1 = GPIO.1 //SERVO
#bit  GP2 = GPIO.2 //NC
#bit  GP3 = GPIO.3 //FET

/*
#define SWITCH PIN_B0 
#define SERVO  PIN_B1 
#define FET    PIN_B2 
*/

int1 PinChangeFlag;
int32   i; 

void Init() 
{ 
output_high(GP2); //servo enable
}

// DELAY SERVO VOOR LINKS OM = 500uS
// DELAY SERVO VOOR RECHT OM = 1200uS
main() 
{ 
   PinChangeFlag = GPWUF;

    setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); 
    setup_comparator(NC_NC); 

    while(true) 
    { 
      Init();
      
      if(PinChangeFlag) // test for I/O pin change
      {
         do{
            for(i=0; i <400;i++)//linksom
            {
               output_high(GP1);
               delay_us(500);
               output_low(GP1);
               delay_ms(10);
            }
         }while(input(GP0));
            
         for(i=0; i <400;i++)//rechtsom
         {
            output_high(GP1);
            delay_us(1200);
            output_low(GP1);
            delay_ms(10);
         }
      }
	output_low(GP2); //servo disable
    sleep();
    } 
}