Ayuda Control de posicion de un motor de CD

[gzuz]

Hola estoy intentanto controlar un motor de CD mediante control PID con un PIC18F4550 para un proyecto de mi escuela, las ganancias Kp, Ki y Kd, las controlo mediante potenciometros a entradas analógicas, y las multiplico por 0.01, la posicion deseada del motor tambien la controlo con potenciómetro y la medicion de la posicion mediante un sensor de efecto hall, lo que pasa es si pongo todas las ganancias a cero el ciclo de trabajo es cero, pero con solo aumentar un poco cualquier ganancia los bits del ciclo de trabajo se van hasta 1024. Mi rograma es el siguiente:

#include <18F4550.h>
#device adc=10
#fuses HS,NOWDT,NOPROTECT,NOLVP
#use delay(clock=20000000)

#include <lcd.c>

int16 teta,duty,Kpm,Kim,Kdm,teta_d;
float Vmax=5.0;
float error,vel=0,velf=0,conv,teta_grados,teta_deseada,teta_grados_old=0;
float ts=0.01,U,Iz=0,alfa=0.9; // ts= Periodo de muestreo
float Kp,Ki,Kd;

#INT_TIMER1
void task_PID (){
set_timer1(40536);

//Obtención de valores de Kp,Ki, Kd, Angulo deseado y angulo obtenido por sensor.
set_adc_channel(0);
teta = read_adc();
set_adc_channel(1);
teta_d=read_adc();
set_adc_channel(2);
Kpm=read_adc();
set_adc_channel(3);
Kim=read_adc();
set_adc_channel(4);
Kdm=read_adc();

// Conversión de voltaje dado por sensor a angulo obtenido.
teta_grados=(teta*0.43988269794721407624633431085044)-45;

//Conversión de voltaje dado por potenciómetro a ángulo deseado.
teta_deseada=teta_d*0.3515625;

Kp=Kpm*0.01;
Ki=Kim*0.01;
Kd=Kdm*0.01;

error=teta_deseada-teta_grados;
vel=(teta_grados-teta_grados_old)/ts; //velocidad
velf=alfa*velf+(1-alfa)*vel; //velocidad filtrada
teta_grados_old=teta_grados;
Iz=Iz+ts*error;
U=(Kp*error)+(Ki*Iz)-(Kd*velf); //PID salida en voltaje
if(abs(U)>Vmax) U=Vmax;
conv=(1024.0*U)/5.0;
duty=(conv+0.5);

// Direccion del motor
if(U>=0)
output_high(PIN_C0);
else
output_low(PIN_C0);

set_pwm1_duty(duty);

if (input(PIN_B7)==0){
printf(lcd_putc, "\fTd=%f D=%ld", teta_deseada,duty);
printf(lcd_putc, "\nVs=%f", U);}
else{
printf(lcd_putc, "\fKp=%f Ki=%f",Kp, Ki);
printf(lcd_putc, "\nKd=%f", Kd);
}

}

void main() {

lcd_init();
set_tris_c(0x00);

setup_ccp1(CCP_PWM); // CCP1 como PWM
// (1/20000000)*4*1*128 = 25.8us o 38.75 khz
setup_timer_2(T2_DIV_BY_1, 128, 1);

setup_adc_ports(ALL_ANALOG);
setup_adc(adc_clock_internal);
set_adc_channel(0);

setup_timer_1(T1_INTERNAL|T1_DIV_BY_2);
set_timer1(40536); //10ms para cristal 20Mhz

enable_interrupts(INT_TIMER1);
enable_interrupts(GLOBAL);
while (1);
}

Agradeceria mucho a quien me pueda ayudar por favor