Go Down

Topic: problema con libreria timer one y lectura de angulo encoder omron control PID (Read 208 times) previous topic - next topic

juanka7

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

Code: [Select]


#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

surbyte

Hola @juanka7 bienvenido al foro Arduino, te pasé un privado, leelo por favor.


Prioridades con un PID!!

Si tienes un tiempo de muestro de 3.44 mseg a no ser que lea mal, no puedes tener algo como esto en tu loop

Code: [Select]
Serial.print("Humedad Suelo 1: " );    // Imprime en el monitor serie el valor Humedad Suelo en %
  Serial.println(mappedValue1);
  
  Serial.print("Humedad Suelo 2: " );    // Imprime en el monitor serie el valor Humedad Suelo en %
  Serial.println(mappedValue2);
  
  Serial.print("Humedad Campo: ");       // Imprime en el monitor serie el valor promedio de la Humedad Suelo Campo Completo.
  Serial.println(mappedValueCampo);

  Serial.print("Promedio 1: ");          // Imprime en el monitor serie el valor promedio 10 lecturas Humedad Suelo 1
  Serial.println(Promedio1);
  
  Serial.print("Promedio 2: ");          // Imprime en el monitor serie el valor promedio 10 lecturas Humedad Suelo 2
  Serial.println(Promedio2);
  
  Serial.print("Hora: ");                // Imprime en el monitor serie la hora actual
  Serial.println(rtc.getHours(), DEC);
  
  Serial.print("Minutos: ");             // Imprime en el monitor serie la hora actual
  Serial.println(rtc.getMinutes(), DEC);

  Serial.print("Segundos: ");            // Imprime en el monitor serie la hora actual
  Serial.println(rtc.getSeconds(), DEC);

  Serial.print("Temperatura: ");         // Imprime en el monitor serie la hora actual
  Serial.println(Temperatura);

  Serial.print (LxM, DEC);               //Imprime la cantidad de litros por minuto
  Serial.print (" L/min\r\n");           //Imprime "L/min" y salta a una nueva linea


simplemente eso sin medirlo consume mas tiempo que 3.44 mseg.
Usa micros() para medirlo y luego conversamos.
Debes minimizar tu información de debugging al extremo que el PID haga lo que daba hacer. Luego si los tiempos dan para alguna información iras complementando.

sabes medir tiempos usando micros() o millis()?

De nuevo y disculpa mi comentario pero esto es una BARBARIDAD en un loop PID

Code: [Select]
delay (5000);


Tienes delay() por todos lados.
Deberías reahacer tu código usando esta guia
 millis() y máquina de estados

Ve a Documentación => Indice de temas tutoriales => millis() y también lee máquina de estados.

juanka7

Gracias por tu respuesta surbyte queria comentarte que creo que hay un error porque parece que has visto un codigo que no es el que redacte en el post te lo vuelvo a redactar porque no uso delay en el codigo que  redacte. sobre lo del tiempo de muestreo te comento que lo que garantiza ese tiempo es la interrupción por timer que como ves en el Setup configuro con 3440 microsegundos para que me garantice que cada 3.44 milisegundos se ejecute el void TimerISR que es donde calculo la acción de control y la transfiero al pendulo. mi problema esta en que la lectura del encoder se va desfasando como si en cada iteración entre la interrupción y el void loop se leyera de nuevo entonces se va decayendo hacia la izquierda y el controlador sigue actuando pero cada vez menos vertical hasta que cae. use la libreria PID y millis pero la calidad del control es menos eficiente que usando la libreria timer

Code: [Select]


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

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
/// ---------------------------------------------

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);


 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
}


surbyte

Tienes mucha razón, no se que código copié... olvida lo anterior!!! Completamente ERRADO y acá si con mayúsculas

EDITO 1:
porque repites 3 veces esto
Code: [Select]

  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 (abs(encoderValue) >= 8000)
      encoderValue = 0;


Yo he hecho una rutina conversion()
Tmb modifique el ambio de encoderValue >= 8000 y <= -8000 por valor absoluto de encoderValue

Code: [Select]
if (abs(encoderValue) >= 8000)
      encoderValue = 0;


No entiendo porque si lo tienes en la rutina ISR tmb lo repites en el loop?
porque modifcar dichos valores a cada momento? Que lo haga la ISR solamente.
Igualmente esto no soluciona tu problema.
Pero es algo que mejorará la perfomance general.


juanka7

Surbyte. la rutina que me preguntas porque la repito tres veces es debido a lo siguiente:

1. coloco el pendulo verticalmente y cargo el sketch a arduino, leyendo el angulo en el void setup garantizo que se cargue esa lectura solo una vez al setpoint

Edito1. leo el angulo en el loop y en el ISR de la interrupción tratando de que no me ocurra el problema que menciono donde la lectura del encoder se reinicia cada que ocurre la interrupción y cada vez que se reinicia se carga 180 grados que es como si estuviese vertical pero el ya esta inclinado

gracias por el consejo sobre el valor absoluto no lo habia tenido en cuenta

surbyte

Me refiero a que repetir algo es diferente a necesitarlo las 3 veces.
Repetirlo es una redundancia desde el punto de vista de la programación.

Esa rutina puede programarse asi

Code: [Select]
double conversion() {

  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 (abs(encoderValue) >= 8000)
      encoderValue = 0;

  return anguloencoder; // puede mejorarse esto
}


Incluso para mi gusto tienes demasiadas variables volatiles. No creo que necesites tantas.

Usando una sola rutina la llamas las 3 veces.

Como dije esto no te cambia nada.

Otra cosa porque no usas la libreria PID disponible para Arduino o es obligación en tu tarea hacerlo como lo has hecho?

juanka7

hola surbyte lo que me dices sobre la rutina debo hacerlo fuera del void loop como un metodo?

sobre lo de la libreria PID no la uso porque el controlador es menos eficiente en esa libreria, con el timer es mas robusto y eficaz solo con el detalle que se desfaza la medida del angulo en el sensor


surbyte

Busque algo que te permita trabajar a tu estilo y encontré este código que es muy similar.
Te pido que lo mires y en todo caso se puede reemplazar el TIMER por la librería TimerOne pero todo lo demás luce parecido.

Tomate un minuto y observalo
Tiene tmb intervaz en Visual Studio 2012 asi que te dejo el link Arduino Motor PID SPeed Control.

Esta claro que en este caso la variable de control es velocidad pero eso es lo de menos.

Code: [Select]
/*
  Motor - PID speed control
  (1) Receive command from Visual Studio (via COM4): set_speed, kP, kI, kD
  (2) Control motor speed through PWM (PWM is base on PID calculation)
  (3) Send pv_speed to Visual Studio -> show in graph
 
 Created 31 Dec. 2016
 This example code is in the public domain.

 http://engineer2you.blogspot.com
 */

#define MAXPULSOS 8000
String mySt = "";
char myChar;
boolean stringComplete = false;  // whether the string is complete
boolean motor_start = false;
const byte pin_a    = 2;   //for encoder pulse A
const byte pin_b    = 3;   //for encoder pulse B
const byte pin_fwd  = 4; //for H-bridge: run motor forward
const byte pin_bwd  = 5; //for H-bridge: run motor backward
const byte pin_pwm  = 6; //for H-bridge: motor speed
int encoder         = 0;
int m_direction     = 0;
int sv_speed        = 100;     //this value is 0~255
double pv_speed     = 0;
double set_speed    = 0;
double e_speed      = 0; //error of speed = set_speed - pv_speed
double e_speed_pre  = 0;  //last error of speed
double e_speed_sum  = 0;  //sum error of speed
double pwm_pulse    = 0;     //this value is 0~255
double kp           = 0;
double ki           = 0;
double kd           = 0;
int timer1_counter; //for timer
int i               = 0;


void setup() {
  pinMode(pin_a,INPUT_PULLUP);
  pinMode(pin_b,INPUT_PULLUP);
  pinMode(pin_fwd,OUTPUT);
  pinMode(pin_bwd,OUTPUT);
  pinMode(pin_pwm,OUTPUT);
  attachInterrupt(digitalPinToInterrupt(pin_a), detect_a, RISING);
  // start serial port at 9600 bps:
  Serial.begin(9600);
  //--------------------------timer setup
  noInterrupts();           // disable all interrupts
  TCCR1A = 0;
  TCCR1B = 0;
  timer1_counter = 59286;   // preload timer 65536-16MHz/256/2Hz (34286 for 0.5sec) (59286 for 0.1sec)

 
  TCNT1 = timer1_counter;   // preload timer
  TCCR1B |= (1 << CS12);    // 256 prescaler
  TIMSK1 |= (1 << TOIE1);   // enable timer overflow interrupt
  interrupts();             // enable all interrupts
  //--------------------------timer setup
 
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }

  analogWrite(pin_pwm,0);   //stop motor
  digitalWrite(pin_fwd,0);  //stop motor
  digitalWrite(pin_bwd,0);  //stop motor
}

void loop() {
  if (stringComplete) {
    // clear the string when COM receiving is completed
    mySt = "";  //note: in code below, mySt will not become blank, mySt is blank until '\n' is received
    stringComplete = false;
  }

  //receive command from Visual Studio
  if (mySt.substring(0,8) == "vs_start"){
    digitalWrite(pin_fwd,1);      //run motor run forward
    digitalWrite(pin_bwd,0);
    motor_start = true;
  }
  if (mySt.substring(0,7) == "vs_stop"){
    digitalWrite(pin_fwd,0);
    digitalWrite(pin_bwd,0);      //stop motor
    motor_start = false;
  }
  if (mySt.substring(0,12) == "vs_set_speed"){
    set_speed = mySt.substring(12,mySt.length()).toFloat();  //get string after set_speed
  }
  if (mySt.substring(0,5) == "vs_kp"){
    kp = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kp
  }
  if (mySt.substring(0,5) == "vs_ki"){
    ki = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_ki
  }
  if (mySt.substring(0,5) == "vs_kd"){
    kd = mySt.substring(5,mySt.length()).toFloat(); //get string after vs_kd
  } 
}

void detect_a() {
  encoder+=1; //increasing encoder at new pulse
  m_direction = digitalRead(pin_b); //read direction of motor
}
ISR(TIMER1_OVF_vect)        // interrupt service routine - tick every 0.1sec
{
  TCNT1 = timer1_counter;   // set timer
  pv_speed = 60.0*(encoder/200.0)/0.1;  //calculate motor speed, unit is rpm
  encoder=0;
  //print out speed
  if (Serial.available() <= 0) {
    Serial.print("speed");
    Serial.println(pv_speed);         //Print speed (rpm) value to Visual Studio
    }


  //PID program
  if (motor_start){
    e_speed = set_speed - pv_speed;
    pwm_pulse = e_speed*kp + e_speed_sum*ki + (e_speed - e_speed_pre)*kd;
    e_speed_pre = e_speed;  //save last (previous) error
    e_speed_sum += e_speed; //sum of error
    if (e_speed_sum >MAXPULSOS) e_speed_sum = MAXPULSOS;
    if (e_speed_sum <-MAXPULSOS) e_speed_sum = -MAXPULSOS;
  }
  else{
    e_speed = 0;
    e_speed_pre = 0;
    e_speed_sum = 0;
    pwm_pulse = 0;
  }
 

  //update new speed
  if (pwm_pulse <255 & pwm_pulse >0){
    analogWrite(pin_pwm,pwm_pulse);  //set motor speed 
  }
  else{
    if (pwm_pulse>255){
      analogWrite(pin_pwm,255);
    }
    else{
      analogWrite(pin_pwm,0);
    }
  }
 
}
void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    if (inChar != '\n') {
      mySt += inChar;
    }
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}


Go Up