Buenos días a todos, escribo para solicitar de manera cordial su ayuda. me encuentro trabajando en un proyecto que es un control PID para un péndulo invertido, estoy usando el Arduino MEGA para el control y lectura de datos, el encoder Omron E6B2-CWZJE para leer la posición angular del péndulo.
Estoy utilizando la libreria TimerOne para garantizar que el tiempo de muestreo que identifique del péndulo se garantice este tiempo de muestreo es de 3.44 milisegundos es decir que cada 3.44 ms estoy calculando una acción de control y transfiriéndola a través de un driver BTS7960 al pendulo y leyendo el encoder en el void de la interrupción y uso el void loop para imprimir datos hacia simulink para visualizar. el problema es que en la ejecución de las instrucciones por parte del arduino me he percatado de que la lectura del encoder se reinicia es decir que el setpoint se corre como si en cada cambio entre la interrupción y el void loop se reiniciara la lectura del encoder el problema hace que el pendulo se estabilice cada vez menos vertical hasta que cae.
serian tan amables de indicarme como evitar que la lectura de mi encoder se reinicie y se vaya desfasando y perdiendo la verticalidad del pendulo sino que se mantenga repetible en el tiempo, de manera que el controlador pueda aplicarse eficientemente y quiero agregar que el encoder usa una interrupción externa CHANGE para leer los cambios de pulso
agradezco su ayuda adjunto el codigo para ver si me pueden guiar
#include <TimerOne.h>
/// --------------------------
/// VARIABLES DEL ENCODER
/// --------------------------
int encoderPin1 = 2;
int encoderPin2 = 3;
volatile int lastEncoded = 0;
volatile double encoderValue = 0;
long lastencoderValue = 0;
int lastMSB = 0;
int lastLSB = 0;
volatile double u=0;
volatile double y=0;
volatile double anguloencoder=0;
/// -----------------------------
/// VARIABLES PARA EL CONTROLADOR
/// -----------------------------
volatile double yk=0;
volatile double yk1=0;
volatile double xk=0;
volatile double e=0;
volatile double e2=0; //PID
volatile double e1=0;
volatile double sp=0;
volatile double acc =0;
volatile double pwm1=0;
volatile double pwm2=0;
/// --------------------------------
/// VARIABLES PARA ENVIAR A SIMULINK
/// --------------------------------
float acciondecontrol=0;
float error=0;
float setpoint=0;
float angulo=0;
/// --------------------------------
/// PINES DE SALIDA HACIA LA PLANTA
/// --------------------------------
int ENA=7;
int ENB=8;
int PWM1=9;
int PWM2=10;
/// --------------------------------
/// PARO DE EMERGENCIA
/// --------------------------------
volatile int paro=0;
int pulsador=6;
void setup()
{
/// --------------------------------
/// DECLARACIÓN PINES DE SALIDA
/// --------------------------------
pinMode(ENA, OUTPUT);
pinMode(ENB, OUTPUT);
pinMode(PWM1, OUTPUT);
pinMode(PWM2, OUTPUT);
/// ------------------
/// CONFIGURACIÓN STOP
/// -------------------
pinMode(pulsador,INPUT);
/// --------------------------------
/// INICIALIZACIÓN DEL PUERTO SERIAL
/// --------------------------------
Serial.begin(19200);
/// ------------------------------------------------------------------------------
/// DECLARACIÓN Y CONFIGURACIÓN DE INTERRUPCIÓN PARA GARANTIZAR TIEMPO DE MUESTREO
/// ------------------------------------------------------------------------------
Timer1.initialize(3440);
Timer1.attachInterrupt( timerIsr );
/// -------------------------------------------------------------------------
/// DECLARACIÓN Y CONFIGURACIÓN DE LA INTERRUPCIÓN PARA LEER EL ENCODER OMRON
/// -------------------------------------------------------------------------
pinMode(encoderPin1, INPUT_PULLUP);
pinMode(encoderPin2, INPUT_PULLUP);
digitalWrite(encoderPin1, HIGH); //turn pullup resistor on
digitalWrite(encoderPin2, HIGH); //turn pullup resistor on
attachInterrupt(0, updateEncoder, CHANGE);
attachInterrupt(1, updateEncoder, CHANGE);
/// ---------------------------------------------
/// CONVERSIÓN A ANGULO DE LA LECTURA DEL ENCODER Y ASIGNACIÓN DE SETPOINT
/// ---------------------------------------------
u=encoderValue;
if(u > 0 && u <= 8000)
{
y= (u*360)/8000;
}
if(u < 0 && u >= -8000)
{
u=u+8000;
y=(u*360)/8000;
}
if(y>= 0 && y <= 180 )
{
anguloencoder=y+180;
}
if(y > 180 && y <= 360 )
{
anguloencoder=y-180;
}
/// -------------
/// RESET ENCODER
/// -------------
if(encoderValue >= 8000)
{
encoderValue=0;
}
if(encoderValue <= -8000)
{
encoderValue=0;
}
sp=anguloencoder;
}
void loop()
{
/// ---------------------------------------------
/// CONVERSIÓN A ANGULO DE LA LECTURA DEL ENCODER
/// ---------------------------------------------
u=encoderValue;
if(u > 0 && u <= 8000)
{
y= (u*360)/8000;
}
if(u < 0 && u >= -8000)
{
u=u+8000;
y=(u*360)/8000;
}
if(y>= 0 && y <= 180 )
{
anguloencoder=y+180;
}
if(y > 180 && y <= 360 )
{
anguloencoder=y-180;
}
/// --------------------------------------------------------
/// IMPRESIÓN SERIAL HACIA SIMULINK PARA GRAFICAR
/// --------------------------------------------------------
acciondecontrol=(float)(yk+220);
angulo=(float)anguloencoder;
setpoint=(float)sp;
error=(float)e;
Serial.print(",");
Serial.print(angulo);
Serial.print(",");
Serial.print(acciondecontrol);
Serial.print(",");
Serial.print(error);
Serial.print(",");
Serial.print(setpoint);
Serial.println(",");
}
/// ------------------------------
/// CONTROLADOR PI O PID CON TIMER
/// ------------------------------
void timerIsr()
{
/// ---------------------------------------------
/// CONVERSIÓN A ANGULO DE LA LECTURA DEL ENCODER PARA CALCULAR CONTROLADOR
/// ---------------------------------------------
u=encoderValue;
if(u > 0 && u <= 8000)
{
y= (u*360)/8000;
}
if(u < 0 && u >= -8000)
{
u=u+8000;
y=(u*360)/8000;
}
if(y>= 0 && y <= 180 )
{
anguloencoder=y+180;
}
if(y > 180 && y <= 360 )
{
anguloencoder=y-180;
}
/// -------------
/// RESET ENCODER
/// -------------
if(encoderValue >= 8000)
{
encoderValue=0;
}
if(encoderValue <= -8000)
{
encoderValue=0;
}
/// ---------------------------------------------
/// CALCULO DEL ERROR Y DE LA ACCIÓN DE CONTROL
/// ---------------------------------------------
xk=anguloencoder;
e=sp-xk;
//yk=((0.013736712*e)+(0.013736712*e1)+(yk1)); PI
yk=(-0.124203785674419*e)+(0.210078307348837*e1)+(-0.089040697674419*e2)+yk1;
/// -----------------------------------------------------
/// TRANSFERENCIA DE LA ACCIÓN DE CONTROL HACIA LA PLANTA
/// -----------------------------------------------------
acc=yk;
if(acc > 0)
{
pwm1=acc;
pwm1=pwm1+220;
//control=pwm1;
if(pwm1 >= 255)
{
pwm1=255;
}
digitalWrite(ENA,HIGH);
digitalWrite(ENB,HIGH);
analogWrite(PWM1, pwm1);
analogWrite(PWM2,0);
}
if(acc < 0)
{
pwm2=abs(acc);
pwm2=pwm2+218;
//control=pwm2;
if(pwm2 >= 255)
{
pwm2=255;
}
digitalWrite(ENA,HIGH);
digitalWrite(ENB,HIGH);
analogWrite(PWM1,0);
analogWrite(PWM2,pwm2);
}
/// ------------------------------
/// LEER PARO DE EMERGENCIA
/// ------------------------------
paro=digitalRead(pulsador);
//EL ENCODER ES MUY SENSIBLE POR ESTA RAZON SE CREA UN RANGO DE OPERABILIDAD DEL CONTROLADOR
if(e >= -0.3 && e <= 0.3 || paro==1 )
{
pwm1=0;
pwm2=0;
digitalWrite(ENA,LOW);
digitalWrite(ENB,LOW);
analogWrite(PWM1,0);
analogWrite(PWM2,0);
digitalWrite(ENA,LOW);
digitalWrite(ENB,LOW);
analogWrite(PWM1,0);
analogWrite(PWM2,0);
}
/// --------------------------------------------------------
/// REGISTROS DE ERROR ANTERIOR Y ACCIÓN DE CONTROL ANTERIOR
/// --------------------------------------------------------
e2=e1; //PID
e1=e;
yk1=yk;
}
/// ---------------------------
/// SKETCH PARA LEER EL ENCODER
/// ---------------------------
void updateEncoder(){
int MSB = digitalRead(encoderPin1); //MSB = most significant bit
int LSB = digitalRead(encoderPin2); //LSB = least significant bit
int encoded = (MSB << 1) |LSB; //converting the 2 pin value to single number
int sum = (lastEncoded << 2) | encoded; //adding it to the previous encoded value
if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) encoderValue ++;
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) encoderValue --;
lastEncoded = encoded; //store this value for next time
}
C