Buenas a todos, lo primero decir q este foro esta de p mare ya que gracias a el he podido solucionar muchos problemas, pero no todos
![Llorica]( "Llorica")
![Llorica]( "Llorica")
.
Os explico, estoy realizando un robot diferencial (creo q se dice asi jeje) a cada motor dc le entra una señal de pwm; como sabeis todos una cosa es simular
y otra la realidad
, asi q mi problema es q debido a factores externos el robot no me va recto, por lo tento estoy intentando realizar un control pi para la trayectoria.
Aqui os pongo el control con ecuaciones diferenciales:
error(k)=w2(k)-w1(k)
y(k)=y(k-1)+K*T*error(k)
d1(k)=dutty(k)+y(k)
d2(k)=dutty(k) (o en el caso de hacerlo simetrico:d2(k)=dutty(k)-y(k))
w2 y w1 son las entradas de los encoders conectados a las ruedas y dutty es el valor del dutty cargado en los pwm.
Para hacer esto he pensao utilizar los contadores del timer 0 y del timer 1 para poder saber cuantos pulsos tendre durante cierto tiempo.
Aqui os pongo el programa del control:
#include <16f876.h>
#fuses XT, NOPROTECT, NOPUT, NOWDT, NOBROWNOUT, NOLVP, NOCPD, WRT
#use delay(clock=4000000)
#use fast_io(A)
#use fast_io(B)
#byte port_a = 5 // Identificador asociado al registro de dirección 5
#byte port_b = 6 // Identificador asociado al registro de dirección 6
#byte port_c = 7 // Identificador asociado al registro de dirección 6
void main(void){
int e1,e2,x;
long duty_PWM1,duty_PWM2,b;
signed long y,e;
set_tris_a(0x3F);
setup_timer_1(T1_external);
setup_timer_2(T2_DIV_BY_16, 249, 1);//pwm f=250Hz
setup_ccp1 (CCP_PWM);
setup_ccp2 (CCP_PWM);
set_timer0(0);
set_timer1(0);
port_b=0x00;
y=e=0;
while(1)
{
x = port_a&0b00001111; //el dutty va de 0%a100% de 10% en 10%
b = x;
if(x>10)
{
b=0;
}
duty_PWM1 = 100 * b;//para un dutty del 10% necesito cargar 100
duty_PWM2 = 100 * b;
while((x)==(port_a&0b00001111))//mientras no haya cambios del puertoA
{
set_pwm1_duty(duty_PWM1);
set_pwm2_duty(duty_PWM2);
if(input(PIN_A5)){
//set_timer0(0);
//set_timer1(0);
delay_ms(2000); //tiempo de adquisicion de datos de los encoders
e1=get_timer0();
e2=get_timer1();
e=e2-e1;
y=y+(0.1*e);//aqui tengo dudas si debo de poner T=1/4000000
duty_PWM1=duty_PWM1+y;
}
}
}
}
Este programa simplemente quiero ver si funciona el control pero no se pq al simlularlo el contador del timer1 tiene valores si yo no he metido pulsos, es como si actuaso como temporizador o algo. Podemos observar como en el caso en que la rueda2(e2) gire mas rapido q la rueda1(e1) tendre un error positivo por lo tanto el dutty del motor 1(pwm1) aumentara.
Otra duda es si los tipos de variables estan bien declaradas, y tambien en la formula de y=y+K*T*e debo poner T ya q si la pongo necesito q el error sea bastante grande para que la y me cambie modificando el dutty.
Haber si me podeis echar una mano y asi pongo el programa completo del control tanto de velocidad como de sentido con su control de trayectoria.
Muchas gracias.
