PID en motor con encoder pololu utilizando interrupciones

Que tal compañeros

Estoy trabajando para sacar un PID con un motor CD con encoder pololu, he intentado de muchas formas utilizando funciones, la ultima prueba que hice es el código que les mostraré acontinuación. El problema aquí es que nunca funciona el PID nunca varía la salida pwm asignada, creo que se debe al manejo de la interrupción para adquirir los datos recibidos por el encoder. Lo que quiero es utilizar el valor que nos brinda el enconder para compararlo con el setpoint y posteriormente aplicar el PID, si observan a detalle la función que maneja el código del PID es el que se encuentra en la librería PID adaptative. Help!

int input;
#include <PID_v1.h>
#define encoder0PinA  18  // cable amarillo pololu
#define encoder0PinB  19  // cable blanco pololu  //cable azul va a 5-12V, //cable verde tierra // negro a tierra, cable rojo 12V 
volatile  int encoder0Pos = 0;
int encoder0dir=0;
int contador=0;
////////////////////////////////////motor////////////////
const int buttonPin = 52;      // pushboton pin
const int dirMotor =  22;      //dirMotor
int pwmPin = 14;               // pwm
int buttonState = 0;          
///////////PID parameters//////////////////

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);


void setup() { 

  pinMode(encoder0PinA, INPUT); 
  digitalWrite(encoder0PinA, LOW);       // turn on pullup resistor
  pinMode(encoder0PinB, INPUT); 
  digitalWrite(encoder0PinB, LOW);       // turn on pullup resistor
  attachInterrupt(4, doEncoder, RISING);  // encoder pin on interrupt 0 - pin 2
  Serial.begin (9600);
  Serial.println("start");                // a personal quirk
//////////////////motor////////
  pinMode(dirMotor, OUTPUT);
  pinMode(pwmPin, OUTPUT);
  pinMode(buttonPin, INPUT);
  digitalWrite(buttonPin, HIGH);
//////PID parameters/////////
   /////////////PID/////////
   //initialize the variables we're linked to
  Input = (contador);
  Setpoint = 500;

  //turn the PID on
  myPID.SetMode(AUTOMATIC);

} 

void loop(){
  
Input = (contador);
  
  double gap = abs(Setpoint-Input); //distance away from setpoint
  if(gap<10)
  {  //we're close to setpoint, use conservative tuning parameters
    myPID.SetTunings(consKp, consKi, consKd);
  }
  else
  {
     //we're far from setpoint, use aggressive tuning parameters
     myPID.SetTunings(aggKp, aggKi, aggKd);
  }
  
  myPID.Compute();
  analogWrite(14,Output);

  
  if (Serial.available()>0)
   {  
   input=Serial.read();
      if (input=='1'){
      digitalWrite(dirMotor, HIGH); //mueve corona CCW
      analogWrite(pwmPin, 255);   
    }
    if (input=='2')
    {
      digitalWrite(dirMotor, LOW); //mueve corona CW
      analogWrite(pwmPin, 255);   
    }
    if (input=='0')
    {
      digitalWrite(dirMotor, LOW); 
      analogWrite(pwmPin, 0);  
    }
    
  }

   
}

void doEncoder() {
  contador++;
  if (digitalRead(encoder0PinA) == digitalRead(encoder0PinB)) {
    encoder0Pos++;
    encoder0dir=1;
  } 
  if (digitalRead(encoder0PinA) != digitalRead(encoder0PinB)){
    encoder0Pos--;
    encoder0dir=-1;
  }
  // print the results to the serial monitor:
  Serial.println (contador, DEC);

  
   
}


Saludos :)

Usas contador pero no lo defines como volatile y cuando usas variables que serán modificadas por las interrupciones debe agregarse volatile. lo mismo encoder0dir y encoder0dir Eso ya es un serio error. Asi que como dices que no cambia la salida PWM del PID estoy seguro que debes empezar por ahi.

gracias por tu respuesta surbyte
en la interrupción, agarra los valores del encoder A y B para hacer el contador, luego este contador lo declare como volatile, si cuenta el encoder pero no regula el PID, a continuación te dejo el resultado:

#include <PID_v1.h>
#define encoder0PinA  18  // cable amarillo pololu
#define encoder0PinB  19// cable blanco pololu  //cable azul va a 5-12V, //cable verde tierra // negro a tierra, cable rojo 12V 
#define PIN_OUTPUT 13
volatile  int encoder0Pos = 0;
char input;
volatile int encoder0dir=0;
volatile int contador=0;
////////////////////////////////////motor////////////////
const int buttonPin = 52;      // pushboton pin
const int dirMotor =  22;      //dirMotor              
int buttonState = 0;          
///////////PID parameters//////////////////
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Define the aggressive and conservative Tuning Parameters
double aggKp=4, aggKi=0.2, aggKd=1;
double consKp=1, consKi=0.05, consKd=0.25;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
void setup() { 

  pinMode(encoder0PinA, INPUT); 
  digitalWrite(encoder0PinA, LOW);       // turn on pullup resistor
  pinMode(encoder0PinB, INPUT); 
  digitalWrite(encoder0PinB, LOW);       // turn on pullup resistor
  attachInterrupt(4, doEncoder, RISING);  // encoder pin on interrupt 0 - pin 2 
  Serial.begin (9600);
  Serial.println("start");                // a personal quirk
//////////////////motor////////
  pinMode(dirMotor, OUTPUT);
  pinMode(buttonPin, INPUT);
  digitalWrite(buttonPin, HIGH);
//////PID parameters/////////
  Setpoint = 500;
  Input=contador;
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
} 

void loop(){
  
  if (Serial.available()>0)
   {  
   input=Serial.read();
      if (input=='1'){
      digitalWrite(dirMotor, HIGH); //mueve corona CCW  
    }
    if (input=='2')
    {
      digitalWrite(dirMotor, LOW); //mueve corona CW  
    }
    if (input=='0')
    {
      analogWrite(PIN_OUTPUT, 0);  
    }
  }
  controlador(contador);
}

void doEncoder() {
  
  contador++;
  if (digitalRead(encoder0PinA) == digitalRead(encoder0PinB)) {
    encoder0Pos++;
    encoder0dir=1;
  } 
  if (digitalRead(encoder0PinA) != digitalRead(encoder0PinB)){
    encoder0Pos--;
    encoder0dir=-1;
  }
  // print the results to the serial monitor:
  Serial.println (contador, DEC);
  detachInterrupt(4);
  
}
void controlador(volatile double Input){
  
  double gap = abs(Setpoint-Input); //distance away from setpoint
  if (gap < 100)
  {  //we're close to setpoint, use conservative tuning parameters
    myPID.SetTunings(consKp, consKi, consKd);
    Serial.print("Ya mero");
  }
  else
  {
     //we're far from setpoint, use aggressive tuning parameters
     myPID.SetTunings(aggKp, aggKi, aggKd);
     Serial.print("aqui ando al 100");
  }

  myPID.Compute();
  analogWrite(PIN_OUTPUT, Output);
  Serial.print("\t Salida = ");
  Serial.println(Output);
  attachInterrupt(4, doEncoder, RISING);
}

En una interrupción no se puede enviar datos al Serial. Involucra demasiado tiempo y no es la idea. coloca la impresion del contador debajo de

void loop() {  
  Input = contador;

  Serial.println (contador, DEC);

Aca la subrutina de interrupción doEnconder() modificada y sutilmente mas rápida al no tener que leer el estado del PinA o PinB en 4 oportunidades frente a 2.

void doEncoder() {
  byte encoA, encoB;

  encoA = digitalRead(encoder0PinA);
  encoB = digitalRead(encoder0PinB);
  
  contador++;
  if (encoA == encoB) {
    encoder0Pos++;
    encoder0dir=1;
  } 
  if (encoA != encoB){
    encoder0Pos--;
    encoder0dir=-1;
  }
}

Veamos como va ahora mientras sigo mirando