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!
#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!