çizgi izleyen robot pid kontrolü

[yusuf.ozyer]

hocalarım ilk önce hayırlı akşamlar dilerim
hocalarım bir çizgi izleyen rpobpot için  pid bulunan kod yazdım fakat motorlarda hiçbir değişim olmuyor sürekli düz gidiyor
bunu yapamadım yardımcı olurmusunuz lütfen
hatam nerede tam olarak

'****************************************************************
'*  Name    : UNTITLED.BAS                                      *
'*  Author  : [select VIEW...EDITOR OPTIONS]                    *
'*  Notice  : Copyright (c) 2015 [select VIEW...EDITOR OPTIONS] *
'*          : All Rights Reserved                               *
'*  Date    : 31.01.2015                                        *
'*  Version : 1.0                                               *
'*  Notes   :                                                   *
'*          :                                                   *
'****************************************************************
@ DEVICE pic16F877a, WDT_OFF              'Watch Dog timer Kapatıldı.
@ DEVICE pic16F877a, PWRT_ON             'Power on timer açık
@ DEVICE pic16F877a, PROTECT_OFF         'Kod Protek kapal
@ DEVICE pic16F877a, BOD_OFF
@ DEVICE pic16F877a, XT_OSC

DEFINE CCP1_REG PORTC                'HPWM PORTC.1
DEFINE CCP2_REG PORTC               'HPWM PORTC.2
DEFINE CCP1_BIT 2              'CCP1 PİNİ PORTC.2 'sag
DEFINE CCP2_BIT 1              'CCP1 PİNİ PORTC.1 'sol
DEFINE ADC_BITS 8
DEFINE ADC_CLOCK 1
DEFINE ADC_SAMPLEUS 11
DEFINE OSC 4


TRISA=%11111111
TRISB=%00000000
TRISC=%00000000
TRISD=%11111111
TRISE=%111

ADCON1=%0000
CMCON=7
SAGSEN3 VAR BYTE
SAGSEN2 VAR BYTE
SAGSEN1 VAR BYTE
ONSEN1 VAR BYTE
ONSEN2 VAR BYTE
SOLSEN1 VAR BYTE
SOLSEN2 VAR BYTE
SOLSEN3 VAR BYTE


P VAR WORD
I VAR WORD
D VAR WORD
KI VAR BYTE 
KP VAR BYTE
KD VAR BYTE
LAST_ERROR VAR WORD

DUZELTME VAR WORD
SOL_HIZ VAR WORD
SAG_HIZ VAR WORD
KONUM VAR WORD
ERROR VAR BYTE

konum=0
ERROR=0
LAST_ERROR=0
sag_hiz=255
sol_hiz=255

SYMBOL MZ80=   PORTD.0
SYMBOL BASLAT= PORTD.1
SYMBOL EGIM=   PORTD.2


SYMBOL  BUZZER=PORTC.0
SYMBOL  SAGILERI = PORTC.6
SYMBOL  SAGGERI  = PORTC.7
SYMBOL  SOLILERI = PORTC.5
SYMBOL  SOLGERI  = PORTC.4
PORTB=%11111111
PAUSE 50
PORTB=%00000000
PAUSE 50
PORTB=%11111111
PAUSE 50
PORTB=%00000000
PAUSE 50
PORTB=%11111111
PAUSE 50
PORTB=%00000000
PAUSE 50

ILK:
IF BASLAT = 0 THEN
    PORTB=%00000000 
    PAUSE 1000
    PORTB=%00000001
    PAUSE 1000
    PORTB=%0000011
    PAUSE 1000
    PORTB=%0000111
    PAUSE 1000
    HIGH BUZZER
    PAUSE 1000
    PORTB=%00000000 :LOW BUZZER:I=0 :GOTO BASLA
ELSE
    GOTO ILK
ENDIF
'-----------------------------------PROGRAM BAŞLANGICI-----------------------------
BASLA:

ADCOKU:

adcin 0 ,SAGSEN3
adcin 1 ,SAGSEN2
adcin 2 ,SAGSEN1
adcin 3 ,ONSEN1
adcin 4 ,ONSEN2
adcin 5 ,SOLSEN1
adcin 6 ,SOLSEN2
adcin 7 ,SOLSEN3

SAGSEN3= 1023 - SAGSEN3
SAGSEN2= 1023 - SAGSEN2
SAGSEN1= 1023 - SAGSEN1
ONSEN1= 1023 - ONSEN1
ONSEN2= 1023 - ONSEN2
SOLSEN1= 1023 - SOLSEN1
SOLSEN2= 1023 - SOLSEN2
SOLSEN3= 1023 - SOLSEN3

KP=3 : KI=1 : KD=1

KONUM= (SAGSEN3*(-4000)+ SAGSEN2*(-3000)+ SAGSEN1*(-2000)+ ONSEN1*(-1000) + ONSEN2*(1000)+ SOLSEN1* (2000) + SOLSEN2 * (3000)+ SOLSEN3 *(4000))
KONUM= KONUM/ SAGSEN3+ SAGSEN2+ SAGSEN1+ ONSEN1+ONSEN2+SOLSEN1 +SOLSEN2+SOLSEN3
ERROR=KONUM

P=ERROR*KP
i=(i+ERROR)*KI
D=(ERROR-LAST_ERROR)*KD
LAST_ERROR=ERROR

duzeltme=P+i+D

IF DUZELTME > 10000 THEN DUZELTME = 10000


'duzeltme = map(duzeltme , 0, 10000, 0, 255);

IF DUZELTME <=0 THEN
SAG_HIZ=255
SOL_HIZ=255-DUZELTME
ENDIF 

IF DUZELTME >0 THEN
SAG_HIZ=255-DUZELTME
SOL_HIZ=255
ENDIF

HPWM 1,SAG_HIZ,1000
HPWM 2,SOL_HIZ,1000
HIGH SAGILERI
LOW SAGGERI
HIGH SOLILERI
LOW SOLGERI

goto BASLA
END