Ayuda con servo construido con arduino y encoder

Hola

Estoy haciendo un servo con la ayuda de un arduino pro mini y un encoder, éste está conectado al arduino de manera que cuando el motor se mueva, el encoder cuente las vueltas que da y me indique la posición en la que se encuentre el motor. El problema es que la posición que le envío desde el ordenador al arduino para que el motor se mueva hasta ella, no es la correcta, por ej. le envio P100(que se mueva 20 clicks) y a lo mejor se mueve 10 clicks, no se si el problema está en el encoder(es de baja calidad), si el arduino no tiene suficiente potencia…

Gracias :smiley: :smiley:

  // Definiciones de motor
#define M1 5              // Patilla 5 es la dirección 1 del motor
#define M2 6              // Patilla 6 es la dirección 2 del motor
#define MVEL 9            // Patilla 8 es la velocidad del motor
#define FINCA 4            // Patilla 4 es el fin de carrera
#define dMaxVel 255        // Velocidad máxima de motor
#define dMedVel 150        // Velocidad media de motor
#define dMinVel 115        // Velocidad mínima de motor

  // Definiciones de encoder
#define dMinEnc 10        // Valor mínimo de movimiento para encoder
#define dMaxEnc 1000      // Valor máximo de movimiento para encoder
  // define encoder    
#define encoder0PinA  2
#define encoder0PinB  3
#include <Wire.h>
enum PinAssignments {
  encoderPinA = 2,   // rigth
  encoderPinB = 3,   // left
  clearButton = 8    // another two pins
};
volatile unsigned int encoderPos = 0;  // a counter for the dial
unsigned int lastReportedPos = 1;   // change management
static boolean rotating=false;      // debounce management

long lPosEnc;
long lTemp;
char cComando[50];    // Cadena de caracteres del comando
int iiCad;            // Contador 
char ch;
int newposition;
int oldposition;
int newtime;
int oldtime;
int PosServo;
float vel;
boolean A_set = false;  // interrupt service routine vars            
boolean B_set = false;

void setup()
{
  Wire.begin(5);                // join i2c bus with address #4
  Wire.onReceive(receiveEvent); // register event
  Serial.begin(9600);           // start serial for output
  
  pinMode(M1, OUTPUT);    // Patillas de dirección y velocidad como salida
  pinMode(M2, OUTPUT);
  pinMode(MVEL, OUTPUT);
  pinMode(FINCA, INPUT);  // Patilla de fin de carrera como entrada
  
    pinMode(encoderPinA, INPUT); 
  pinMode(encoderPinB, INPUT); 
  pinMode(clearButton, INPUT);
 // turn on pullup resistors
  digitalWrite(encoderPinA, HIGH);
  digitalWrite(encoderPinB, HIGH);
  digitalWrite(clearButton, HIGH);

// encoder pin on interrupt 0 (pin 2)
  attachInterrupt(0, doEncoderA, CHANGE);
// encoder pin on interrupt 1 (pin 3)
  attachInterrupt(1, doEncoderB, CHANGE);

  Serial.begin(9600);  // output
  
  encoderPos=0;

  reset();
  Serial.println ("Fin reset");

  delay(2000);              // Esperar 2 segundos para empezar
}

void loop()
{ 
  rotating = true;  // reset the debouncer

  if (lastReportedPos != encoderPos) {
    Serial.print("Index:");
    Serial.println(encoderPos, DEC);
    lastReportedPos = encoderPos;
  }
  if (digitalRead(clearButton) == LOW )  {
    encoderPos = 0;
  }

  if (iiCad>0)
  {
   HazComando(cComando);
   
   iiCad=0;
  }
  delay(500);
}

//*******************************************************************************************
// Realiza un reset del servo en cuatro pasos
// a) Mueve el servo rápido hasta que toca el fin de carrera
// b) Mueve el servo hasta que deja de tocarlo
// c) Mueve el servo despacio hasta que vuelve a tocarlo y pone a 0 la posición del encoder
// d) Mueve el motor hasta el mínimo del encoder
//*******************************************************************************************
void reset() 
{
  int ii;
  int EstadoFinca=LOW;         // variable para leer el fin de carrera

    // A) Mueve el motor a derecha hasta el fin de carrera
  analogWrite(MVEL, dMedVel);   // Velocidad rápida
  digitalWrite(M1, LOW);   // Giro a derecha
  digitalWrite(M2, HIGH);
  
    // Espera a tocar el fin de carrera
  for(ii=0;ii<30;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == HIGH) break;     
   delay(100);
  }
  analogWrite(MVEL, 0);     // Parada
  delay(500);

    // B) Mueve el motor a izquierda hasta dejar el fin de carrera
  analogWrite(MVEL, 127);   // Velocidad media
  digitalWrite(M1, HIGH);   // Giro a iaquierda
  digitalWrite(M2, LOW);
  
    // Espera a dejar el fin de carrera
  for(ii=0;ii<20;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == LOW) break;     
   delay(100);
  }
  delay(100);
  analogWrite(MVEL, 0);     // Parada
  delay(500);

    // C) Mueve el motor a derecha despacio hasta el fin de carrera
  analogWrite(MVEL, dMinVel);   // Velocidad lenta
  digitalWrite(M1, LOW);   // Giro a derecha
  digitalWrite(M2, HIGH);
  
    // Espera a tocar el fin de carrera
  for(ii=0;ii<1000;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == HIGH) break;     
   delay(20);
  }
  analogWrite(MVEL, 0);     // Parada
  delay(500);
  
    // D) Reset del contador de encoder y avanza hasta la posición mínima por margen de seguridad
  lPosEnc=0;
  analogWrite(MVEL, 127);   // Velocidad media
  digitalWrite(M1, HIGH);   // Giro a izquierda
  digitalWrite(M2, LOW);
  
    // Espera a dejar el fin de carrera
  for(ii=0;ii<5;ii++)
  {
   if (lPosEnc>=dMinEnc) break;     
   delay(100);
  }
  delay(100);
  analogWrite(MVEL, 0);     // Parada
  delay(500);
}

void doEncoderA(){
  // debounce
  if ( rotating ) delay (1);  // wait a little until the bouncing is done

  // Test transition, did things really change? 
  if( digitalRead(encoderPinA) != A_set ) {  // debounce once more
    A_set = !A_set;

    // adjust counter + if A leads B
    if ( A_set && !B_set ) 
      encoderPos += 1;

    rotating = false;  // no more debouncing until loop() hits again
  }
}

// Interrupt on B changing state, same as A above
void doEncoderB(){
  if ( rotating ) delay (1);
  if( digitalRead(encoderPinB) != B_set ) {
    B_set = !B_set;
    //  adjust counter - 1 if B leads A
    if( B_set && !A_set ) 
      encoderPos -= 1;

    rotating = false;
  }
}

// function that executes whenever data is received from master
// this function is registered as an event, see setup()
void receiveEvent(int howMany)
{
  iiCad=0;
  while(0 < Wire.available()) 
  {
    ch = Wire.read();     // receive byte as a character
    cComando[iiCad]=ch;
    iiCad++;
  }
  cComando[iiCad]=0;
}

void MueveServo(long Posicion)
{
    // B) Mueve el motor a izquierda hasta que el encoder sea igual a posicion
  analogWrite(MVEL, 127);   // Velocidad media
  digitalWrite(M1, HIGH);   // Giro a iaquierda
  digitalWrite(M2, LOW);
  
    // Espera a dejar el fin de carrera
  while(lPosEnc<Posicion)
  {
   delay(100);
   Serial.println(lPosEnc);
  }
  analogWrite(MVEL, 0);     // Parada
  delay(500);

}

void HazComando(char *Cad)
{
 char chCom;

 chCom=Cad[0];
 switch(chCom)
 {
  case 'R':      // Reset
    Serial.println("RESET");
    reset();
    break;
    
  case 'M':     // Movimiento
    lTemp=atol(&(Cad[1]));
    Serial.print("Mover a:");
    Serial.println(lTemp);
    MueveServo(lTemp);
    break;
    
  case 'P':    // Devuelve posición
    Serial.print("Posicion:");
    Serial.println(lPosEnc);
    sprintf(cComando,"P%ld",lPosEnc);
    EnviaCad(1,cComando);
    break;

  default:
    Serial.println("Comando no valido");
    break;  
 } 
}

void EnviaCad(int Dir,char *Mensaje)
{
 Wire.beginTransmission(Dir);
 Wire.write(Mensaje);
 Wire.endTransmission();
}

Hola, deberías postear tu código para poder darte ayuda.
No olvides usar los tags.

Ya he subido el codigo

para que esto

 while(lPosEnc<Posicion)  {
  delay(100);
   Serial.println(lPosEnc);
  }

Esa es la rutina de posicionamiento, es la mas importante de tu programa y consumes 100mseg por paso leido para informar, reporta la info sin delay, usa millis() pero no pierdas tiempo sin checar la posicion

Yo haría asi

unsigned long time = millis();  //definelo al comienzo de tu rutina
  while(lPosEnc<Posicion)  {
   if (millis()-time > 100L) {
       Serial.println(lPosEnc);
       time = millis();
   }
  }

ahora esto hace lo mismo que antes pero no consume 100mseg de tu posicionamiento y en todo momento chequea tu posicion, verás que será tan preciso como tu enconder

Eso lo habiamos probado, y usamos otro codigo parecido al primero pero con algunos cambios el problema es que no indicaba bien la posición.

//
//
#include <Wire.h>

  // Definiciones generales del servo
#define dDirServo 4      // Dirección I2C del servo  
#define dDirMaestro 1    // Dirección I2C del servo  
  
  // Definiciones de motor
#define M1 5              // Patilla 5 es la dirección 1 del motor
#define M2 6              // Patilla 6 es la dirección 2 del motor
#define MVEL 9            // Patilla 8 es la velocidad del motor
#define FINCA 4            // Patilla 4 es el fin de carrera
#define dMaxVel 255        // Velocidad máxima de motor
#define dMedVel 150        // Velocidad media de motor
#define dMinVel 115        // Velocidad mínima de motor

  // Definiciones de encoder
#define dMinEnc 10        // Valor mínimo de movimiento para encoder
#define dMaxEnc 1000      // Valor máximo de movimiento para encoder
  // define encoder    
#define encoder0PinA  2
#define encoder0PinB  3
enum PinAssignments {
  encoderPinA = 2,   // rigth
  encoderPinB = 3,   // left
  clearButton = 8    // another two pins
};
volatile long lPosEnc = 0;         // Contador de posición del encoder
unsigned int lastReportedPos = 1;  // change management
static boolean rotating=false;     // debounce management

long lTemp;
char cComando[50];    // Cadena de caracteres del comando recibido
int iiCad;            // Contador para la cadena
char ch;
int newposition;
int oldposition;
int newtime;
int oldtime;
int PosServo;
float vel;
boolean A_set = false;  // interrupt service routine vars            
boolean B_set = false;

void setup()
{
  Wire.begin(dDirServo);        // Activa I2C con la dirección del servo
  Wire.onReceive(receiveEvent); // register event
  
  pinMode(M1, OUTPUT);    // Patillas de dirección y velocidad como salida
  pinMode(M2, OUTPUT);
  pinMode(MVEL, OUTPUT);
  pinMode(FINCA, INPUT);  // Patilla de fin de carrera como entrada
  
  pinMode(encoderPinA, INPUT); 
  pinMode(encoderPinB, INPUT); 
  
 // turn on pullup resistors
  digitalWrite(encoderPinA, HIGH);
  digitalWrite(encoderPinB, HIGH);

// encoder pin on interrupt 0 (pin 2)
  attachInterrupt(0, doEncoderA, CHANGE);

// encoder pin on interrupt 1 (pin 3)
  attachInterrupt(1, doEncoderB, CHANGE);

  Serial.begin(115200);
  
  reset();
  Serial.println ("Fin reset");
  delay(500);               // Pausa para empezar
}

void loop()
{ 
  rotating = true;  // reset the debouncer

/*  if (lastReportedPos != lPosEnc) 
  {
   Serial.print("Index:");
   Serial.println(lPosEnc, DEC);
   lastReportedPos = lPosEnc;
  }
*/

    // Comprueba si se ha recibido un comando y lo procesa
  if (iiCad>0)
  {
   ProcesaComando(cComando);
  
   iiCad=0;
  }
  else delay(500);
}

//*******************************************************************************************
// Realiza un reset del servo en cuatro pasos
// a) Mueve el servo rápido hasta que toca el fin de carrera
// b) Mueve el servo hasta que deja de tocarlo
// c) Mueve el servo despacio hasta que vuelve a tocarlo y pone a 0 la posición del encoder
// d) Mueve el motor hasta el mínimo del encoder
//*******************************************************************************************
void reset() 
{
  int ii;
  int EstadoFinca=LOW;         // variable para leer el fin de carrera

    // A) Mueve el motor a derecha hasta el fin de carrera
  analogWrite(MVEL, dMedVel);   // Velocidad rápida
  digitalWrite(M1, LOW);   // Giro a derecha
  digitalWrite(M2, HIGH);
  
    // Espera a tocar el fin de carrera
  for(ii=0;ii<500;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == HIGH) break;     
   delay(10);
  }
  analogWrite(MVEL, 0);     // Parada
  delay(500);

    // B) Mueve el motor a izquierda hasta dejar el fin de carrera
  analogWrite(MVEL, 127);   // Velocidad media
  digitalWrite(M1, HIGH);   // Giro a iaquierda
  digitalWrite(M2, LOW);
  
    // Espera a dejar el fin de carrera
  for(ii=0;ii<200;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == LOW) break;     
   delay(10);
  }
  delay(100);
  analogWrite(MVEL, 0);     // Parada
  delay(500);

    // C) Mueve el motor a derecha despacio hasta el fin de carrera
  analogWrite(MVEL, dMinVel);   // Velocidad lenta
  digitalWrite(M1, LOW);   // Giro a derecha
  digitalWrite(M2, HIGH);
  
    // Espera a tocar el fin de carrera
  for(ii=0;ii<1000;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == HIGH) break;     
   delay(10);
  }
  analogWrite(MVEL, 0);     // Parada
  delay(500);
  
    // D) Reset del contador de encoder y avanza hasta la posición mínima por margen de seguridad
  lPosEnc=0;
  analogWrite(MVEL, dMinVel);   // Velocidad mínima
  digitalWrite(M1, HIGH);       // Giro a izquierda
  digitalWrite(M2, LOW);
  
    // Espera a dejar el fin de carrera
  for(ii=0;ii<50;ii++)
  {
   if (lPosEnc>=dMinEnc) break;     
   delay(10);
  }
  delay(100);
  analogWrite(MVEL, 0);     // Parada
  delay(500);
}

//*******************************************************************************************
// Interrupción de encoder patilla A, activa en cambio de estado
//*******************************************************************************************
void doEncoderA()
{
  // debounce
  if ( rotating ) delay (1);  // wait a little until the bouncing is done

  // Test transition, did things really change? 
  if( digitalRead(encoderPinA) != A_set ) {  // debounce once more
    A_set = !A_set;

    // adjust counter + if A leads B
    if ( A_set && !B_set ) 
      lPosEnc += 1;

    rotating = false;  // no more debouncing until loop() hits again
  }
}

//*******************************************************************************************
// Interrupción de encoder patilla B, activa en cambio de estado
//*******************************************************************************************
void doEncoderB()
{
  if ( rotating ) delay (1);
  if( digitalRead(encoderPinB) != B_set ) {
    B_set = !B_set;
    //  adjust counter - 1 if B leads A
    if( B_set && !A_set ) 
      lPosEnc -= 1;

    rotating = false;
  }
}


//**************************************************
// Interrupción receptor de I2C
// Se ejecuta cuando se recibe un dato de master
//**************************************************
void receiveEvent(int howMany)
{
  iiCad=0;
  while(0 < Wire.available()) 
  {
    ch = Wire.read();     // receive byte as a character
    cComando[iiCad]=ch;
    iiCad++;
  }
  cComando[iiCad]=0;
}

//****************************************************
// Mueve el servo a una posición determinada de encoder
//************
void MueveServo(long Posicion)
{
    // Activa velocidad del motor
 analogWrite(MVEL, dMedVel);   // Velocidad media

    // Comprueba dirección de movimiento
 if (Posicion>lPosEnc)
 {   
    // Mueve el motor a izquierda hasta que el encoder sea igual a posicion
  digitalWrite(M1, HIGH);   // Giro a izquierda
  digitalWrite(M2, LOW);
  
    // Espera a llegar a la posición deseada
  while(lPosEnc<Posicion)
  {
   delay(2);
  }
 } 
 else
 {   
    // Mueve el motor a derecha hasta que el encoder sea igual a posicion
  digitalWrite(M1, LOW);   // Giro a derecha
  digitalWrite(M2, HIGH);
  
    // Espera a llegar a la posición deseada
  while(lPosEnc>Posicion)
  {
   delay(2);
  }
 } 
  
 analogWrite(MVEL, 0);     // Parada
 delay(100);
}

//***********************************************
// Procesa un comando recibido del maestro
//***************************************************
void ProcesaComando(char *Cad)
{
 char chCom;

 chCom=Cad[0];
 switch(chCom)
 {
  case 'R':      // Reset
    Serial.println("RESET");
    reset();
    break;
    
  case 'M':     // Movimiento
    lTemp=atol(&(Cad[1]));
    Serial.print("Mover a:");
    Serial.println(lTemp);
    MueveServo(lTemp);
    break;
    
  case 'P':    // Devuelve posición
    Serial.print("Posicion:");
    Serial.println(lPosEnc);
    sprintf(cComando,"P%ld",lPosEnc);
    EnviaCad(dDirMaestro,cComando);
    break;

  default:
    Serial.println("Comando no valido");
    break;  
 } 
}

//********************************************
// Envía una cadena de caracteres por I2C
//*******************************************************
void EnviaCad(int Dir,char *Mensaje)
{
 Wire.beginTransmission(Dir);
 Wire.write(Mensaje);
 Wire.endTransmission();
}

a ver.

    // Espera a llegar a la posición deseada
  while(lPosEnc<Posicion)
  {
   delay(2);
  }

esto es lo que me respondes a mi sugerencia.

unsigned long time = millis();  //definelo al comienzo de tu rutina
  while(lPosEnc<Posicion)  {
   if (millis()-time > 100L) {
       Serial.println(lPosEnc);
       time = millis();
   }
  }

tu código ahora consume 2 milisegundos. El mio con millis() fue respuesta a tu delay(100) y no consumía nada salvo por la impresión en el Monitor Serie.

Todas las opciones que estas manejando son mejores que delay(100)

Hola, he probado lo que usted me sugirió, y el resultado es el siguiente:

Fin reset
Posicion:-5
Mover a:100
-86
-84
-40
-3
Posicion:15
Mover a:200
40
126
Posicion:239
Mover a:100
279
334
419
538
613
621
608
567
508
428
363
293
232
182
114

Esto son los datos del monitor serie al darle los valores, son totalmente desproporcionados. le envío el código con su sugerencia incluida:

//
//
//
#include <Wire.h>

  // Definiciones generales del servo
#define dDirServo 4      // Dirección I2C del servo  
#define dDirMaestro 1    // Dirección I2C del servo  
  
  // Definiciones de motor
#define M1 5              // Patilla 5 es la dirección 1 del motor
#define M2 6              // Patilla 6 es la dirección 2 del motor
#define MVEL 9            // Patilla 8 es la velocidad del motor
#define FINCA 4            // Patilla 4 es el fin de carrera
#define dMaxVel 255        // Velocidad máxima de motor
#define dMedVel 150        // Velocidad media de motor
#define dMinVel 115        // Velocidad mínima de motor

  // Definiciones de encoder
#define dMinEnc 10        // Valor mínimo de movimiento para encoder
#define dMaxEnc 1000      // Valor máximo de movimiento para encoder
  // define encoder    
#define encoder0PinA  2
#define encoder0PinB  3
enum PinAssignments {
  encoderPinA = 2,   // rigth
  encoderPinB = 3,   // left
  clearButton = 8    // another two pins
};
volatile long lPosEnc = 0;         // Contador de posición del encoder
unsigned int lastReportedPos = 1;  // change management
static boolean rotating=false;     // debounce management

long lTemp;
char cComando[50];    // Cadena de caracteres del comando recibido
int iiCad;            // Contador para la cadena
char ch;
int newposition;
int oldposition;
int newtime;
int oldtime;
int PosServo;
float vel;
boolean A_set = false;  // interrupt service routine vars            
boolean B_set = false;

void setup()
{
  Wire.begin(dDirServo);        // Activa I2C con la dirección del servo
  Wire.onReceive(receiveEvent); // register event
  
  pinMode(M1, OUTPUT);    // Patillas de dirección y velocidad como salida
  pinMode(M2, OUTPUT);
  pinMode(MVEL, OUTPUT);
  pinMode(FINCA, INPUT);  // Patilla de fin de carrera como entrada
  
  pinMode(encoderPinA, INPUT); 
  pinMode(encoderPinB, INPUT); 
  
 // turn on pullup resistors
  digitalWrite(encoderPinA, HIGH);
  digitalWrite(encoderPinB, HIGH);

// encoder pin on interrupt 0 (pin 2)
  attachInterrupt(0, doEncoderA, CHANGE);

// encoder pin on interrupt 1 (pin 3)
  attachInterrupt(1, doEncoderB, CHANGE);

  Serial.begin(115200);
  
  reset();
  Serial.println ("Fin reset");
  delay(500);               // Pausa para empezar
}

void loop()
{ 
  rotating = true;  // reset the debouncer

/*  if (lastReportedPos != lPosEnc) 
  {
   Serial.print("Index:");
   Serial.println(lPosEnc, DEC);
   lastReportedPos = lPosEnc;
  }
*/

    // Comprueba si se ha recibido un comando y lo procesa
  if (iiCad>0)
  {
   ProcesaComando(cComando);
  
   iiCad=0;
  }
  else delay(500);
}

//***********************************
// Realiza un reset del servo en cuatro pasos
// a) Mueve el servo rápido hasta que toca el fin de carrera
// b) Mueve el servo hasta que deja de tocarlo
// c) Mueve el servo despacio hasta que vuelve a tocarlo y pone a 0 la posición del encoder
// d) Mueve el motor hasta el mínimo del encoder
//***********************************
void reset() 
{
  int ii;
  int EstadoFinca=LOW;         // variable para leer el fin de carrera

    // A) Mueve el motor a derecha hasta el fin de carrera
  analogWrite(MVEL, dMedVel);   // Velocidad rápida
  digitalWrite(M1, LOW);   // Giro a derecha
  digitalWrite(M2, HIGH);
  
    // Espera a tocar el fin de carrera
  for(ii=0;ii<500;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == HIGH) break;     
   delay(10);
  }
  analogWrite(MVEL, 0);     // Parada
  delay(500);

    // B) Mueve el motor a izquierda hasta dejar el fin de carrera
  analogWrite(MVEL, 127);   // Velocidad media
  digitalWrite(M1, HIGH);   // Giro a iaquierda
  digitalWrite(M2, LOW);
  
    // Espera a dejar el fin de carrera
  for(ii=0;ii<200;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == LOW) break;     
   delay(10);
  }
  delay(100);
  analogWrite(MVEL, 0);     // Parada
  delay(500);

    // C) Mueve el motor a derecha despacio hasta el fin de carrera
  analogWrite(MVEL, dMinVel);   // Velocidad lenta
  digitalWrite(M1, LOW);   // Giro a derecha
  digitalWrite(M2, HIGH);
  
    // Espera a tocar el fin de carrera
  for(ii=0;ii<1000;ii++)
  {
   EstadoFinca = digitalRead(FINCA);
   if (EstadoFinca == HIGH) break;     
   delay(10);
  }
  analogWrite(MVEL, 0);     // Parada
  delay(500);
  
    // D) Reset del contador de encoder y avanza hasta la posición mínima por margen de seguridad
  lPosEnc=0;
  analogWrite(MVEL, dMinVel);   // Velocidad mínima
  digitalWrite(M1, HIGH);       // Giro a izquierda
  digitalWrite(M2, LOW);
  
    // Espera a dejar el fin de carrera
  for(ii=0;ii<50;ii++)
  {
   if (lPosEnc>=dMinEnc) break;     
   delay(10);
  }
  delay(100);
  analogWrite(MVEL, 0);     // Parada
  delay(500);
}

//*******************************************************************************************
// Interrupción de encoder patilla A, activa en cambio de estado
//*********************************
void doEncoderA()
{
  // debounce
  if ( rotating ) delay (1);  // wait a little until the bouncing is done

  // Test transition, did things really change? 
  if( digitalRead(encoderPinA) != A_set ) {  // debounce once more
    A_set = !A_set;

    // adjust counter + if A leads B
    if ( A_set && !B_set ) 
      lPosEnc += 1;

    rotating = false;  // no more debouncing until loop() hits again
  }
}

//**************************
// Interrupción de encoder patilla B, activa en cambio de estado
//*************************
void doEncoderB()
{
  if ( rotating ) delay (1);
  if( digitalRead(encoderPinB) != B_set ) {
    B_set = !B_set;
    //  adjust counter - 1 if B leads A
    if( B_set && !A_set ) 
      lPosEnc -= 1;

    rotating = false;
  }
}


//***********************************
// Interrupción receptor de I2C
// Se ejecuta cuando se recibe un dato de master
//***********************************
void receiveEvent(int howMany)
{
  iiCad=0;
  while(0 < Wire.available()) 
  {
    ch = Wire.read();     // receive byte as a character
    cComando[iiCad]=ch;
    iiCad++;
  }
  cComando[iiCad]=0;
}

//**************************
// Mueve el servo a una posición determinada de encoder
//***************************
void MueveServo(long Posicion)
{
 unsigned long time = millis();  //definelo al comienzo de tu rutina

  // Activa velocidad del motor
 analogWrite(MVEL, dMedVel);   // Velocidad media

    // Comprueba dirección de movimiento
 if (Posicion>lPosEnc)
 {   
    // Mueve el motor a izquierda hasta que el encoder sea igual a posicion
  digitalWrite(M1, HIGH);   // Giro a izquierda
  digitalWrite(M2, LOW);
  
  while(lPosEnc<Posicion)  {
  if (millis()-time > 100L) {
       Serial.println(lPosEnc);
       time = millis();
   }
  }
 } 
 else
 {   
    // Mueve el motor a derecha hasta que el encoder sea igual a posicion
  digitalWrite(M1, LOW);   // Giro a derecha
  digitalWrite(M2, HIGH);
  
  while(lPosEnc>Posicion)  {
   if (millis()-time > 100L) {
       Serial.println(lPosEnc);
       time = millis();
   }
  }
 } 
  
 analogWrite(MVEL, 0);     // Parada
 delay(100);
}

//*********************************
// Procesa un comando recibido del maestro
//*********************************
void ProcesaComando(char *Cad)
{
 char chCom;

 chCom=Cad[0];
 switch(chCom)
 {
  case 'R':      // Reset
    Serial.println("RESET");
    reset();
    break;
    
  case 'M':     // Movimiento
    lTemp=atol(&(Cad[1]));
    Serial.print("Mover a:");
    Serial.println(lTemp);
    MueveServo(lTemp);
    break;
    
  case 'P':    // Devuelve posición
    Serial.print("Posicion:");
    Serial.println(lPosEnc);
    sprintf(cComando,"P%ld",lPosEnc);
    EnviaCad(dDirMaestro,cComando);
    break;

  default:
    Serial.println("Comando no valido");
    break;  
 } 
}

//******************************
// Envía una cadena de caracteres por I2C
//******************************
void EnviaCad(int Dir,char *Mensaje)
{
 Wire.beginTransmission(Dir);
 Wire.write(Mensaje);
 Wire.endTransmission();
}

Pero ahi tienes otro problema. No parece ser el servo el problema.
Lo veo y respondo mas puntualmente.