Problema con PID para carro + pendulo invertido

[jadacuor]

hola a todos, les cuento que me he decidido a realizar un control PID en pic16f877a para un carrito con pendulo invertido (mi primer controlador digital), ya que el profe que me da clase de control digital dice que esa materia es solo teorica y que no es para estar haciendo proyectos y demas cosas, bueno... volviendo al tema, realice el codigo en CCS COMPILER y aunque tuve muchas dificultades logre hacer algo, al armar todo y conectar el pic el carrito empezaba a tambalear tan pronto le aplicaba una perturbacion y aunque unas cuantas veces intento estabilizarse, la mayoria de las veces o el programa se perdia o el carrito oscilaba tanto que era imposible volver a la posicion de equilibrio.... por eso acudo a ustedes con el animo de encontrar respuestas a mis dudas, adjunto el codigo en CCS, disculparan el programita ya que es mi primer controlador y no tengo muy claras las ideas.. una cosa es la teoria y otra cosa es la practica.... de antemano muchas gracias.

Código: [Seleccionar]

#include <16F877a.h>
#device adc=10
#use delay(clock=4000000)
#fuses XT,NOWDT,NOPUT,NOLVP,NOBROWNOUT,NOWRT,NOPROTECT,NODEBUG
#include <lcd.c>

int start=0;
int kp,Ti,Td;
float T=0.001;
int16 valor;
int16 control; //valor del PWM
float a,b,c;
float rT,eT,iT,dT,yT,uT;
float iT0=0.0;
float eT0=0.0; //variables de ecuaciones
float max=1020.0;
float min=-1020.0;     //límites máximo y mínimo de control.


#INT_EXT

void inicializacion(){
    set_adc_channel(0);
    delay_us(150);
    rT=5.0*(read_adc())/1024.0;
           
    a=500;
    b=1000;
    c=1;
    start=1;
    control=0;
    output_high(PIN_C0);
    setup_ccp1(ccp_pwm);                  //Módulo CCP a modo PWM
    setup_ccp2(ccp_pwm);                  //Módulo CCP a modo PWM
      }

void main ( )
   {
  rt=a=b=c=0;
  set_tris_c(0);
  set_tris_b(255);
  set_tris_b(255);
  setup_timer_2(t2_div_by_16,255,1);     //periodo de la señal PWM a 1ms 
  setup_ccp1(ccp_off);                  //Módulo CCP a modo PWM
  setup_ccp2(ccp_off);                  //Módulo CCP a modo PWM
  setup_adc_ports(all_analog);         //Puerto A analógico
  setup_adc(ADC_CLOCK_INTERNAL);       //reloj convertidor AD interno
  ext_int_edge( H_TO_L );
  enable_interrupts(INT_EXT);
  enable_interrupts(GLOBAL);
  lcd_init();
  output_c(0);
  set_pwm1_duty(0);
  set_pwm2_duty(0);
 
  while(1){
  while(start==1){
  set_adc_channel(0);
  delay_us(50);
  valor=read_adc();               //Lectura adc posicion angular

  yT= 5.0*valor/1024.0;       //posicion angular actual

  lcd_gotoxy(1,1);
  printf(lcd_putc,"%4.2g %4.2g %4.2g",a,b,c);
  lcd_gotoxy(1,2);
  printf(lcd_putc,"%lu %4.2g %4.2g",control,eT,iT);

  eT=rT-yT;            //Cálculo error 
  iT=b*eT+iT0;        //Cálculo del término integral
  if(iT>5000){iT=5000;}
  if(iT<-5000){iT=-5000;}
  dT=c*(eT-eT0);      //Cálculo del término derivativo
  uT=iT+a*eT+dT;        //Cálculo de la salida PID

  if(uT<0){uT=(-1)*uT;}

  if (uT>max) {        //Salida PID si es mayor que el MAX
     uT=max;}
  else {
     if (uT<min){      //Salida PID si es menor que el MIN
        uT=min;}
        }
   control=uT;        //Transferencia de salida PID a señal PWM

  if(eT>0)
  {set_pwm1_duty(control);
   set_pwm2_duty(0);}
  if(eT==0)
  {uT=iT=eT=0;
   set_pwm1_duty(0);
   set_pwm2_duty(0);}
  if(eT<0)
  {set_pwm2_duty(control);
   set_pwm1_duty(0);}

  iT0=iT;            //Guardar variables
  eT0=eT;
 
  }
  }
   }
   
 


 
saludos

[Suky]

Yo fijaría un tiempo de muestreo mediante un timer. Con eso se define el sistema muestreado y los parámetros del PID. Y guarda con el envío de datos a un LCD o por UART (formateado) ya que agregan tiempos no definidos.


Saludos!

[jadacuor]

ya antes habia establecido un tiempo de muestreo de 10ms y 1ms pero no mejoro nada.. lo de la lcd solo la utilizo para la simulacion en proteus pero no la utilizo en el circuito fisico ya que el carrito es bastante pequeño y tengo donde ponerla.

[Suky]

Y cual es el código entonces?

[jadacuor]

gracias por responder... el codigo es el el primero que postee pero sin utilizar la lcd

[Suky]

Y como controlas cada cuanto se ejecuta?

[jadacuor]

pues realmente es el primer intento de controlador que hago y no tengo claro muhcas cosas, entre ellas como defino el tiempo de muestreo.... habia intenatado con  un delay al final del codigo pero luego lo quite... les agradeceria si me ayudan .... tmabien me fue dificil cuadrar el pwm para el cambio de giro, y aunque ya cambia de giro, creo que la señal de salida u(t) no converge y cada vez hay mas oscilaciones, cosas que no se como arreglar...
saludos

[Suky]

Para fijar un periodo de muestreo es adecuado usar la interrupción de un timer. Las constantes del PID como la determinas? En el foro hay varios temas que tratan el tema de PID y robot equilibrista, puedes darles una revisada.


Saludos!