Picproje Elektronik Sitesi

#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)
}

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

if((e>dead_band) || (e+dead_band<0))

:1000000000308A0004280000FF238316900000308F
:100010008A00BB288E30A0003708A1003608A20055
:10002000A301A108031D1E282208A100A201083077
:10003000A002A108031D1E28A00126280310A11B51
:100040002528A20DA10DA0031E28A11308003A081F
:1000500003199728C2003E0803199728C2070318FE
:1000600038287F30C202031C9728031997283C28A0
:100070008130C207031897284208A000A101A201FD
:10008000A3013B08C600C6173C08C5003D08C400D4
:100090001830C200C301441C65284108A307031C93
:1000A0005728A20A031D5728A10A0319C31740089D
:1000B000A207031C5E28A10A0319C3173F08BC004E
:1000C000BC173C08A1070318C317C30DA10CA20C51
:1000D000A30CC60CC50CC40C0310C20B4A2801307B
:1000E000A00703189728A11B7C28C60DA30DA20DFD
:1000F000A10DA00303199728C61F8D28A30A031D6D
:100100008D28A20A031D8D28A10A031D8D28A10C8C
:10011000A20CA30CA00A031997283B08C3003F08B0
:10012000C306C31F9528A1179B28A1139B28A001D4
:10013000A101A201A30108008E30A0003608A00290
:100140003708A2003808A100A217A0080319B32895
:100150000310A208031DAF28A1080319B328A20C9D
:10016000A10CA00BA828B71FBA28A109A209A10AAF
:100170000319A20A080003081F3983000515B001FE
:10018000AF01B401B3011F1383161F109F101F117D
:100190009F110730831299000B30831685001330AE
:1001A0009F0083120512851281309F002130A0002C
:1001B000A00BD8289F149F18DB2883161E088312D3
:1001C000A5001E08A60085309F002130A000A00BCE
:1001D000E7289F149F18EA2883161E088312A70099
:1001E0001E08A800AC01AB0127082502A9002608BB
:1001F000AA002808031C280FAA022F082902B10010
:100200002A08B2003008031C300FB2022A08B000DE
:100210002908AF00AA08031D1B292908013C03185F
:10022000CA291B2901302907B6002A08B70003187C
:10023000B70A001CCA292908B3072A0803182A0F7D
:10024000B407A401AA1F2B290130A400A909AA09F7
:10025000A90A0319AA0A2A08B7002908B6000A2021
:100260002308BD002208BC002108BB002008BA00FA
:10027000CD30C100CC30C0004C30BF007C30BE005F
:1002800027202308B9002208B8002108B700200859
:10029000B6002308BD002208BC002108BB002008CE
:1002A000BA00C101C001BF018330BE00272023086E
:1002B000B9002208B8002108B7002008B6009C2029
:1002C0002208AC002108AB00AC0CAB0CAC0CAB0CA6
:1002D000AC0CAB0CAC0CAB0C0F30AC05A408031988
:1002E0007629AB09AC09AB0A0319AC0AA401B21F09
:1002F00080290130A400B109B209B10A0319B20A78
:10030000B208031D872931080A3C0318CA2932089C
:10031000B7003108B6000A202308BD002208BC003F
:100320002108BB002008BA00C101C001BF01BE0105
:1003300027202308B9002208B8002108B7002008A8
:10034000B6002308BD002208BC002108BB0020081D
:10035000BA00C101C001BF018330BE0027202308BD
:10036000B9002208B8002108B7002008B6009C2078
:100370002208AE002108AD00A4080319C429AD0964
:10038000AE09AD0A0319AE0A2D08AB072E080318F3
:100390002E0FAC07A401B41FD4290130A400B30967
:1003A000B409B30A0319B40A34084D3C0318E3290D
:1003B000FF3A031DDF293308203C0318E3294E30A0
:1003C000B4002030B3003408AE003308AD00AE0CEA
:1003D000AD0CAE0CAD0CAE0CAD0CAE0CAD0C0F30CC
:1003E000AE052E08B7002D08B6000A202308BD0070
:1003F0002208BC002108BB002008BA00C101C001CE
:10040000BF01BE0127202308B9002208B800210837
:10041000B7002008B6002308BD002208BC00210850
:10042000BB002008BA00C101C001BF018330BE007B
:1004300027202308B9002208B8002108B7002008A7
:10044000B6009C202208AE002108AD00AE0CAD0C19
:10045000AE0CAD0CAE0CAD0CAE0CAD0C0F30AE0551
:10046000A40803193D2AAD09AE09AD0A0319AE0A65
:10047000B309B409B30A0319B40A2D08AB072E084F
:1004800003182E0FAC072C08AE002B08AD002B086C
:100490002C040319632AAE1F542A0516AD09AE09B0
:1004A000AD0A0319AE0A552A85161E30AD0703188A
:1004B000AE0AAE08031D602A2D08F03C0318632A1B
:1004C000AE01F030AD00B501350F672A742A103047
:1004D000A000A00B692A000035082D02031D722A16
:1004E00005128512B50A642A05128512D428630004
:02400E00F411AB
:00000001FF