#include <12F675.H>
#fuses INTRC_IO,NOWDT,NOPUT //,NOBROWNOUT
#use delay(clock=4000000)
#use fast_io(a)
#use rs232(baud=9600, xmit=PIN_A2)

//analog implementation
//AN0=target
//AN1=potentiomenter

//driver type:
//type 0: ch0-left, ch1-right,
//type 1: ch0-on/off, ch1-direction
#define driver_type 0

//output (type=0)
#define motorL    PIN_A4
#define motorR 	PIN_A5

//output (type=1)
#define motorON   PIN_A4
#define motorDIR	PIN_A5

//PID
#define Kp 0.2 * 16
#define Ki 0.0 * 16
#define Kd 0.0 * 16

#define dead_band   1
#define offset     30
#define max_pwm    240
#define max_integration 20000
#define derivate_dead_band 10


#byte ADRESH=0x1e
#byte ADRESL=0x9e
#byte ADCON0=0x1f
#byte ANSEL=0x9f


#define get_analog(var) #asm \
	movf	ADRESL,w \
	movwf	&var \
	movf	ADRESH,w \
	movwf	&var+1 \
   #endasm

#define neg16(a) #asm \
  COMF   a,F \
  COMF   &a+1,F \
  INCF   a,F \
  BTFSC  03,2 \
  INCF   &a+1,F \
  #endasm

#define txd16(dato) putc((int)dato); putc((int)(dato>>8))

//long BARG;
#define sgn16(i) (bit_test(i,15))
int sign;
int16 target,pot,e,pwm,tmp,prev_e=0,derivate,integration=0;

void main()
{
int cont;

 set_tris_a(0b00001011);
 ANSEL=0b00010011;  //0:nc, 001:fosc/8, 0011:DDAA


 #if driver_type==0
 output_low(motorL); output_low(motorR);
 #else
 output_low(motorON);
 #endif

 while(true) //bucle
 {
   //Get analog channels

   //TARGET
   ADCON0=0b10000001; //1: Right, 0: vref=5v, 00:NC, 00: ch=0, 1: start, 1: AD_ON
   delay_us(100);
   bit_set(ADCON0,1); //start
   while(bit_test(ADCON0,1));
   get_analog(target);

   //POTENTIOMETER
   ADCON0=0b10000101; //1: Right, 0: vref=5v, 00:NC, 01: ch=1, 1: start, 1: AD_ON
   delay_us(100);
   bit_set(ADCON0,1); //start
   while(bit_test(ADCON0,1));
   get_analog(pot);

//** IMPLEMENTS PID *******
   pwm=0;
   e=target-pot; //error
   derivate=e-prev_e; prev_e=e; //derivate

   if((e>dead_band) || (e+dead_band<0))
   {
      //integrative
      integration=integration+e;

      //Kp
      sign=false;
      if (sgn16(e)) { sign=true; neg16(e); }
      pwm=e*Kp; pwm=pwm>>4;
      if (sign) neg16(pwm);


      //Kd
      sign=false;
      if (sgn16(derivate)) { sign=true; neg16(derivate); }
      if(derivate>derivate_dead_band)
      {
        tmp=derivate*Kd; //tmp=tmp>>4;
        if (sign) neg16(tmp);
        pwm=pwm+tmp;
      }
   }

   //Ki
   sign=false;
   if (sgn16(integration)) { sign=true; neg16(integration); }
   if (integration>max_integration) integration=max_integration;    //check limits
   tmp=integration; tmp=tmp>>4;
   tmp=tmp*Ki; tmp=tmp>>4;
   if (sign) {neg16(tmp); neg16(integration) }
   pwm=pwm+tmp;

   tmp=pwm;
   if(pwm)
   {
      if(sgn16(tmp))
      {
         //negative
         #if driver_type==0
         output_high(motorL);
         #else
         output_high(motorON);
         output_high(motorDIR);
         #endif
         neg16(tmp);
      }
      else
      {
         //positive
         #if driver_type==0
         output_high(motorR);
         #else
         output_high(motorON);
         output_low(motorDIR);
         #endif
      }

      tmp+=offset;
      if(tmp>max_pwm) tmp=max_pwm;
   }

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

   //implements PWM
   for(cont=0;cont<255;cont++)
   {
     delay_us(50);
     if ((int)tmp==cont)
     {
       #if driver_type==0
       output_low(motorL); output_low(motorR);
       #else
       output_low(motorON);
       #endif
     }
   }
   #if driver_type==0
   output_low(motorL); output_low(motorR);
   #else
   output_low(motorON);
   #endif

/*     //monitor
     putc('0');  txd16(POT);
     putc(0x3c); txd16(TARGET);
     putc(0x3d); txd16(PWM);
*/
  } //while(1)
}