#include <16F876.h>
#fuses HS,NOWDT,NOPROTECT,NOLVP,PUT

#use delay (clock=20000000)

//#use fast_io(B)
//#use fast_io(C)
#use rs232(baud=38400, bits=8, xmit=PIN_C6, rcv=PIN_C7)

#define RS232_XMT       PIN_C6  // (output) RS232 serial transmit
#define RS232_RCV       PIN_C7  // (input)  RS232 serial receive
#define ENC				PIN_B0  // (input) ENCODER SIGNAL PROP BLADE
#define CAPPWM			PIN_B5	// (output) CAPACITANCE PULSING
#define CAPPIN			PIN_B4	// (input) SENSING CAP CHARGE TIME

#define GLED_OFF    output_low(PIN_C5)
#define GLED_ON     output_high(PIN_C5)
#define RLED_OFF    output_low(PIN_C0)
#define RLED_ON     output_high(PIN_C0)
#define PRESS1	    !input(PIN_B1)
#define PRESS2	    !input(PIN_B2)
#define CAPSENSE    !input(PIN_B4)

#use I2C(MASTER, sda=PIN_C4, scl=PIN_C3, SLOW)

int default_pwm1 = 96;
int default_pwm2 = 96;
int default_pwm3 = 96;

int pwm1, pwm2, pwm3;
int cap1, cap2, cap3;

byte default_cap1;
byte default_cap2;
byte default_cap3;

int pwm_address = 10;
int cap_address = 20;

//byte data;

// capacitance charge time asm code
byte cap_asm(){
	byte time = 0x00;
//	#asm
//
//	#endasm
	return(time);
//
}

// set pwm of devices
void write_pwm(byte address,byte value){
	if (address == 0x00)
		set_pwm1_duty(value);
	else{
	i2c_start();
	i2c_write(address);
	i2c_write(value);
	i2c_stop();
	}
}


// read charge time of devices
byte read_cap(byte address){
	byte data;
	
	if (address == 0x01)
		data = cap_asm();
	else{
		i2c_start();
		i2c_write(address);
		data = i2c_read(0);
		i2c_stop();
	}
	return(data);
}


main()
{
  byte value;
  setup_ccp1(CCP_PWM);   // Configure CCP1 as a PWM
  setup_timer_2(T2_DIV_BY_16, 127, 1);
  
  output_high( PIN_C0 );
  output_high( PIN_C5 );
  delay_ms( 250 );
  output_low( PIN_C0 );
  output_low( PIN_C5 );
  delay_ms( 250 );

  //RLED_ON;
  //GLED_ON;
  //delay_ms( 250 );
  //RLED_OFF;
  //GLED_OFF;
  //delay_ms( 250 );
	
  while( true )
    {
      //output_high( PIN_C5 ); // turn the green led on
  	  GLED_ON;
	  RLED_OFF;

	  //write_pwm(pwm_address,default_pwm);
	  write_pwm(0x00,default_pwm1);
	  write_pwm(0xA0,default_pwm2);
	  write_pwm(0xB0,default_pwm3);
	  read_cap(0x01);
	  read_cap(0xA1);
	  read_cap(0xB1);

	  GLED_OFF;
	  RLED_ON;
	
	  delay_ms( 5000 );

	  RLED_OFF;
	  GLED_ON;

	  write_pwm(0x00,default_pwm1+16);
	  write_pwm(0xA0,default_pwm2+16);
	  write_pwm(0xB0,default_pwm3+16);
	  read_cap(0x01);
	  read_cap(0xA1);
	  read_cap(0xB1);
	  
	  RLED_ON;
	  GLED_OFF;

	  delay_ms( 5000 );

	  RLED_OFF;
	  GLED_ON;

	  write_pwm(0x00,default_pwm1-8);
	  write_pwm(0xA0,default_pwm2-8);
	  write_pwm(0xB0,default_pwm3-8);
	  read_cap(0x01);
	  read_cap(0xA1);
	  read_cap(0xB1);	  
	  
	  RLED_ON;
	  GLED_OFF;

	  delay_ms( 5000 );

	  RLED_OFF;
	  GLED_ON;

	  write_pwm(0x00,default_pwm1+16);
	  write_pwm(0xA0,default_pwm2+16);
	  write_pwm(0xB0,default_pwm3+16);
	  read_cap(0x01);
	  read_cap(0xA1);
	  read_cap(0xB1);
	  
	  RLED_ON;
	  GLED_OFF;

	  delay_ms( 5000 );

    }

}