Pages: [1]   Go Down
Author Topic: AYUDA CON CONTROLADOR PD!!  (Read 289 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 3
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hola a todos! estoy haciendo un proyecto denominado "ball and plate" y bueno el problema es el controlador...
no se si lo que estoy haciendo esta correcto, ya que aun soy un novato en esto, pero les subo el sketch para ver si lo pueden chekear y dejar algun comentario o ayuda! se los agradecería mucho.
el problema es que el servo motor se mueve, pero no controla como es debido... no se cual es el problema.

de ante mano muchas gracias!
Code:
#include <PID_v1.h>
#include <Servo.h>

  #define cte_kpx  -1.048
  #define cte_kpy  1.048 /*verificar bien si es punto o coma*/
  #define cte_kdx  0.5626
  #define cte_kdy  -0.5626
  
   /*COMUNICACIÓN*/
   byte byteRead;
   double num1, num2;
   int ballx,bally;
   int setPointx,setPointy;
   double complNum,counter,complNum1;
   int numOfDec;
   boolean mySwitch=false;
  
   /*SERVO*/
   Servo servoX;
   Servo servoY;
   int posx;
   int posy;
  
   /*OUTPUT*/
   float finalx;
   float finaly;
   float dutyCyclex;
   float dutyCycley;
  
   /*PD*/  
   float errorx;
   float errory;
   float prev_errorx;
   float prev_errory;
   float proporcionalx;
   float proporcionaly;
   float derivativox;
   float derivativoy;
   float kpx;
   float kpy;
   float kdx;
   float kdy;
   long lastTime;
   long sampleTime;
   long timeOut;

/*------------------------------------------------------------*/  
   void setup() {                
 //Inicia la comunicación
     Serial.begin(57600);
     num1=0;
     num2=0;
     complNum=0;
     complNum1=0;
     counter=1;
     numOfDec=0;
    
    
    /*Pin 9 y 10 salida*/
     pinMode(9,OUTPUT);
     pinMode(10,OUTPUT);
    
     /* PD */
     lastTime= millis();
     kpx=cte_kpx;
     kpy=cte_kpy;
     kdx=cte_kdx;
     kdy=cte_kdy;
    
     /*ServoMotor*/
     posx=95;
     posy=90;
     servoX.attach(9);
     servoY.attach(10);
     servoX.write(posx);
     servoY.write(posy);
   }//void setup

/*---------------------------------------------------------------------*/
   void loop() {
  // Siempre que el buffer tenga información será leida.
     while(Serial.available()){    
    
/*----------------COMUNICACIÓN-----------------------------------------*/
      
       byteRead = Serial.read();
       if(byteRead>47 && byteRead<58){  
       //encontró el número.
       /*Si mySwitch es cierto, entonces llenara la variable
        num1 de lo contrario llenará la variable num2*/
          if(!mySwitch){
            num1=(num1*10)+(byteRead-48);
          }else{
            num2=(num2*10)+(byteRead-48);
            
            counter=counter*10;
            numOfDec++;      
          }
       }
     /* si la información termina con el signo igual
     entonces imprimir num1 y num2*/      
      if(byteRead==61){    
       ballx=num1;
       bally=num2;
      complNum=ballx+(bally/(counter));    
      Serial.print(complNum,numOfDec);
  
      
      //resetea todas las variables
      num1=0;
      num2=0;
      complNum=0;
      complNum1=0;
      counter=1;
      numOfDec=0;
      mySwitch=false;  
   /*si la información termina con el signo punto y coma
     guarda num1 y num2 en x e y respectivamente*/  
   }else if (byteRead==59){
      setPointx=num1;
      setPointy=num2;
      complNum1=setPointx+(setPointy/(counter));
    
      Serial.print(complNum1,numOfDec);

      
      //resetea las variables
      num1=0;
      num2=0;
      complNum=0;
      complNum1=0;
      counter=1;
      numOfDec=0;
      mySwitch=false;  
    
     }else if (byteRead==44){
        mySwitch=true;  
     }

/* -------------CONTROLADOR PD-------------------------------------*/
 
 /*Se hacen los cálculos de forma periódica*/
  sampleTime=(millis()-lastTime);
  if(sampleTime>=100){
    
    lastTime=millis();
    /*proporcional kp*/
    prev_errorx=(float)errorx;
    errorx=(int)ballx-(int)setPointx;
    proporcionalx=(float)kpx*(float)errorx;
    
    prev_errory=(float)errory;
    errory=(int)bally-(int)setPointy;
    proporcionaly=(float)kpy*(float)errory;
    
    /*Derivativa kd*/    
  
    derivativox=(float)kdx*((float)errorx-(float)prev_errorx);
    derivativoy=(float)kdy*((float)errory-(float)prev_errory);
    
/*------------OUTPUT----------------------------------------------*/
  dutyCyclex=(int)proporcionalx+(int)derivativox;
  dutyCycley=(int)proporcionaly+(int)derivativoy;
/*-----------SERVO MOTORES----------------------------------------*/  
 
  finalx=95-dutyCyclex;
  finaly=90-dutyCycley;

    servoX.write(finalx);
    delay(30);
    

  }/*if sample time*/
 }//while
}//Void loop
  

por el momento estoy utilizando solo un servo, ya que el otro no esta disponible.

Saludos!
Logged

0
Offline Offline
Edison Member
*
Karma: 17
Posts: 1413
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Esquema de conexión, foto, descripción del proyecto...
Logged

* Si preguntas, pon el código de tu programa, hace mucho mas fácil ayudarte. Y me ahorro un mensaje pidiendo que lo hagas.
* Si consigues solucionar tu problema, dedica unos minutos a explicar en tu post como lo conseguiste para beneficio de todos.
* Cambia el 'Subject' de tu hilo y añade 'SOLUCIONADO' cuando hayas llegado a una solución al problema que planteaste.
* Utiliza un 'Subject' para tu hilo que explique de que va el hilo.
Si estas empezando:
* Comienza a usar Arduino
* Guías de iniciación a Arduino
* Ejemplos
* Referencia del Lenguaje
* Conceptos básicos
Guia de usuario de arduino
Tutoriales en Ingles
Si necesitas que alguien te escriba el código: http://www.freelancer.com/  o esta  http://www.guru.com/

0
Offline Offline
God Member
*****
Karma: 1
Posts: 524
Arduino inside
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

pues imaginate nosotros,¿ que es lo tenemos que interpretar?

el problema es que el servo motor se mueve, pero no controla como es debido... no se cual es el problema.

no sabemos como lo tienes conectado, ni como se mueve, ni como quieres que tu se mueva....
Logged

Anaco, Venezuela
Offline Offline
Jr. Member
**
Karma: 8
Posts: 88
Electronic Engineer
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Hola,

        Estas incluyendo la libreria PID pero no la estas usando sino que estas haciendo los calculos manuales, deberias haber declarado un obtejo PID para X y otro para Y, inicializarlo en el constructor, seteando el tiempo, y ya solo llamarias a compute dentro de tu ciclo loop.

        Por otro lado deberias ver si la ejecucion de tu ciclo es lo suficientemente rapida para que se ejecute en el tiempo que escogiste, y bueno debiste haber hecho la simulación de todo el sistema modelado, con los parametros del controlador y el tiempo que colocaste.

        Pensando que lo que tienes esta bien, igual deberias empezar controlado un solo eje para probar el controlador, y luego que este bien llevarlo a los dos ejes, y asegurarte que el calculo de los dos no exceda el tiempo que colocaste.

Saludos.
Logged

Pages: [1]   Go Up
Jump to: