#include <16F690.h>
#use delay(clock=4000000)
#fuses NOWDT, HS, PUT, NOPROTECT, BROWNOUT, MCLR, NOCPD
//------------------------------------------------------------
#use i2c(SLAVE, SDA=PIN_B4, SCL=PIN_B6, address=0x10, FORCE_HW)
//------------------------------------------------------------
#use rs232(baud=9600, parity=N, xmit=PIN_B7, rcv=PIN_B5, bits=8, errors) 
//------------------------------------------------------------
// PORTB.5 [RB5 pin12]  -> PicRX (RS232 input)
// PORTB.7 [RB7 pin10]  -> PicTX (RS232 output)
//------------------------------------------------------------
#define PWM_POWER PIN_C2
#define PWM_SERVO1 PIN_C0
#define PWM_SERVO2 PIN_C1
//------------------------------------------------------------
BYTE incoming, state; // I2C vars
BYTE command, data_lsb, data_msb;
int16 data16bits, pwm1, pwm1_aux, pwm2, pwm2_aux;
int1 flag_i2c, flag_command, flag_data_lsb, flag_data_msb, flag_pwm1_data_ready, flag_pwm2_data_ready;

#INT_SSP
void ssp_interupt ()
{
  state = i2c_isr_state();
  flag_i2c = true;

  if(state < 0x80) //Master is sending data
  {
    if(state == 0)
    {
    }
    if(state == 1)  //First received byte is Command
    {
      incoming = i2c_read();
      command = incoming;
      flag_command = true;
    }
    if(state == 2)  //Second received byte is data (LSB)
    {
      incoming = i2c_read();
      data_lsb = incoming;
      flag_data_lsb = true;
    }
    if(state == 3)  //Third received byte is data (MSB)
    {
      incoming = i2c_read();
      data_msb = incoming;
      flag_data_msb = true;
    }
  }
  if(state == 0x80)  //Master is requesting data
  {
    i2c_write(command);
    i2c_write(data_lsb);
    i2c_write(data_msb);
  }
}
//------------------------------------------------------------------------------
void command_dispatcher()
{
  data16bits = data_msb;
  data16bits = data16bits<<8;
  data16bits = data16bits+data_lsb;

  if(command == 1) // Servos Power On/Off
  {
    if(data16bits == 1)
      output_high(PWM_POWER);
    else
      output_low(PWM_POWER);
  }
  else if(command == 10) // servo 1
  {
    pwm1 = data16bits;
    if(pwm1>1800)
      pwm1 = 1800;
    pwm1_aux = 1800 - pwm1;
    flag_pwm1_data_ready = true;
  }
  else if(command == 20) // servo 2
  {
    pwm2 = data16bits;
    if(pwm2>1800)
      pwm2 = 1800;
    pwm2_aux = 1800 - pwm2;
    flag_pwm2_data_ready = true;
  }

  // flag reset
  flag_i2c = false;
  flag_command = false;
  flag_data_lsb = false;
  flag_data_msb = false;
}
//------------------------------------------------------------------------------
void pwm_pulse_pwm()
{
  if(flag_pwm1_data_ready == true)
  {
    output_high(PWM_SERVO1);
    delay_us(500);
    delay_us(pwm1);
    output_low(PWM_SERVO1);
    delay_us(pwm1_aux);
  } 
  if(flag_pwm2_data_ready == true)
  {
    output_high(PWM_SERVO2);
    delay_us(500);
    delay_us(pwm2);
    output_low(PWM_SERVO2);
    delay_us(pwm2_aux);
  } 

  if(flag_pwm1_data_ready == true && flag_pwm2_data_ready == true)
    delay_ms(16);
  else if(flag_pwm1_data_ready == false && flag_pwm2_data_ready == false)
    delay_ms(20);
  else
    delay_ms(18);
}
//------------------------------------------------------------------------------
void main()
{
  delay_ms(200); // power up delay

  setup_comparator(NC_NC_NC_NC);
  setup_vref(FALSE);
  set_tris_A ( 0b11111111 );      // Port A
  set_tris_B ( 0b01111111 );      // Port B 00110000 ( Bit5/7 - RS232- Rx/tx)     
  set_tris_C ( 0b01011100 );      // Port C
  
  printf("\r\nI2C Slave Started!\r\n");

  flag_i2c = false;
  flag_command = false;
  flag_data_lsb = false;
  flag_data_msb = false;

  flag_pwm1_data_ready = false;
  flag_pwm1_data_ready = false;

  enable_interrupts(INT_SSP);
  enable_interrupts(GLOBAL);

  while(TRUE)
  {
    if(flag_i2c == true && flag_command == true && flag_data_lsb == true && flag_data_msb == true)
      command_dispatcher();

    pwm_pulse_pwm();
  }
}