feedback control problem

#chip 16F876A, 16
#option explicit

#config FOSC = HS
#config WDTE = OFF
#config PWRTE = ON
#config CP = OFF
#config BOREN = ON

// ===== I/O =====
Dir PORTC.3 Out    ' H-bridge direction A
Dir PORTC.4 Out    ' H-bridge direction B  
Dir PORTC.2 Out    ' CCP1 - PWM output A
Dir PORTC.1 Out    ' CCP2 - PWM output B

PORTC = 0          

// ===== Variables =====
Dim phase As Word
//Dim phase_inc As Word
Dim idx As Byte
Dim sample As word ;Byte
Dim alt As Bit
Dim prev_idx As Byte
Dim duty As Byte

Dim temp_math As Word
Dim adc_vaL As Word
Dim feedback as Byte
Dim amp_scale  As Integer
Dim soft_start_done as bit

// Phase increment: 65536 / (20000/50) = 65536 / 400 = 163.84 \25HZ\
#Define PHASE_INC 328
#Define SET_POINT 512   ' 2.5v output voltage!

amp_scale = 0           ' Base amplitude

//===== PWM Setup =====\\
CCP1CON = 0xc   ' PWM mode on CCP1 1100
CCP2CON = 0xc   ' PWM mode on CCP2 1100

//===== Timer2 Setup - Direct Register Control =====\\
' F_pwm = 16MHz / (4 × Prescaler × (PR2+1))
' For 20kHz: 20000 = 16000000 / (4 × 1 × (PR2+1)) => PR2 = 199
' This gives max 8-bit duty of 200 (matches our scaled sine table)

PR2 = 0xc7      ' if i remove the PR2, allowing the compiler to set if automatically for me i wii be getting 15,625khz instead of 20,000khz
T2CON = 0x4     ' Timer2 ON, prescale 1:1, postscale 1:1
                        ' Bit 2 = TMR2ON
                        ' Bits 1:0 = T2CKPS = 00 (prescale 1)
                        ' Bits 6:3 = TOUTPS = 0000 (postscale 1)

' Enable Timer2 interrupt
PIR1.1 = 0             ' Clear TMR2IF
PIE1.1 = 1             ' Enable TMR2IE  
INTCON.PEIE = 1        ' Enable peripheral interrupts
INTCON.GIE = 1         ' Enable global interrupts

On Interrupt Timer2Match Call HandleSineWave

ADCON0 = 0b01000001

// ===== Main Loop =====
Do
Wait 20 MS 
        // === NORMAL VOLTAGE FOR FEEDBACK ===\\
adc_val = ReadAD10(AN0) ' 0 to 1023

    If adc_val < (SET_POINT - 2) Then  '512= 2.5V ADC
        If amp_scale < 255 Then
            amp_scale = amp_scale + 1
        End If
    Else If adc_val > (SET_POINT + 2) Then
        If amp_scale > 0 Then
            amp_scale = amp_scale - 1
        End If
    End If

Loop

//===== Interrupt Service Routine =====

Sub HandleSineWave
    PIR1.1 = 0 
    //===SHIFT PHASE RIGHT 9 TIME==\\
    phase = phase + phase_inc
    idx = FnLSR(phase, 9) ' 0-127

    If idx < prev_idx Then
        alt = Not alt
        PORTC.3 = 0 : PORTC.4 = 0 ' Zero-latency clear

        If alt = 0 Then
            PORTC.3 = 1
        Else
            PORTC.4 = 1
        End If
    End If
    prev_idx = idx

    ReadTable sin_table, idx , sample

    duty= sample * amp_scale
    duty = temp_math_H         ' Taking the High byte is faster than
     //duty=FnLSR(amp_scale, 8)

If alt = 0 Then
    CCPR1L = duty
    CCPR2L = 0
  Else
    CCPR1L = 0
    CCPR2L = duty
  End If
End Sub

//===== 128-Step Sine Table (0 to 200) =====\\
Table sin_table
 0, 5, 10, 15, 20, 24, 29, 34, 39, 44, 49, 53, 58, 63, 67, 72, 77, 81, 86, 90, 94, 99, 103, 107, 111, 115, 119, 123, 127, 131, 134, 138, 141, 145, 148, 151, 155, 158, 161, 164, 166, 169, 172, 174, 176, 179, 181, 183, 185, 187, 188, 190, 191, 193, 194, 195, 196, 197, 198, 198, 199, 199, 200, 200, 200, 200, 200, 199, 199, 198, 198, 197, 196, 195, 194, 193, 191, 190, 188, 187, 185, 183, 181, 179, 176, 174, 172, 169, 166, 164, 161, 158, 155, 151, 148, 145, 141, 138, 134, 131, 127, 123, 119, 115, 111, 107, 103, 99, 94, 90, 86, 81, 77, 72, 67, 63, 58, 53, 49, 44, 39, 34, 29, 24, 20, 15, 10, 5
End Table

duty= sample * amp_scale
duty = temp_math_H         ' Taking the High byte is faster than


but if i use "duty= sample" the freq and wave for will be okay