Problema retroalimentacion de servos

[estrike182]

Chicos estoy haciendo un carro que debe avanzar en una dirección, medida con una brujula (Compas) con un margen de 4°, por ejemplo:
34°min(Izquierda) y 38°max(Derecha). Mientras el carro apunte entra esa direccion, los dos motores deben encenderce, de lo contrario el motor opuesto hacia la direccion erronea debe apagarce. (Ej: Si se devia 31° el motor derecho se apaga) para asi estabilizar la direccion. Pero tengo los siguientes problemas.
El carro igual forma pierde la direccion y si la encuentra parece tonto buscandola.. El motor es un servo: GS-3630BB. El tiempo minimo de reaccion que he usado es de 25ms, si no mal recuerdo menos de eso el motor ni se mueve.
Y el algoritmo usado es el siguiente (Esta hecho es C pero es muy facil igual)

for(int u=0; u< 20;u++) {
          float AnguloLeido1 = compass.GetHeading();
          delay(15);
          float AnguloLeido2 = compass.GetHeading();
          float AnguloPromedio = (AnguloLeido1+AnguloLeido2)/2;
          if(funcionCorreccionAvance (AnguloPromedio, AnguloNecesario, Anguloopuesto, MargenPositivoNecesario, MargenNegativoNecesario) == true){
            if(funcionDetener() == true) {
              goto inicio;
            }
            delay(10); 
          } else {
            goto inicio; }
        }

Y el codigo de la funcion:

boolean funcionCorreccionAvance (float AnguloLeido,float  AnguloNecesario, float Anguloopuesto, float MargenPositivoNecesario,float  MargenNegativoNecesario)
{  
  //Sentencia para que los motores funcionen hacia una direccion
  if (AnguloLeido < MargenPositivoNecesario && AnguloLeido > MargenNegativoNecesario) {
    analogWrite (MotorDER, 220); 
    analogWrite (MotorIZQ, 10);   
        Serial.println("=======Dos motores encendidos========");  
    Serial.print("Direccion necesaria: ");
    Serial.println(AnguloNecesario);
    Serial.print("Direccion leida: ");
    Serial.println(AnguloLeido);
        Serial.println("=====================================");
    delay(25);
    return true;
  } 

  //Sentencia para que apague el motor derecho (Se esta llendo hacia la izquierda)
  if (AnguloLeido >= MargenPositivoNecesario && (AnguloLeido <= Anguloopuesto || AnguloLeido >= 180)) {
    int diferenciaAngular=0;
    if (AnguloLeido < MargenNegativoNecesario) {
      diferenciaAngular = AnguloLeido + (360 -  MargenPositivoNecesario); 
    } 
    else {
      diferenciaAngular = AnguloLeido - AnguloNecesario;  
    }
    analogWrite (MotorDER, 0); 
    analogWrite (MotorIZQ, 10);  

    Serial.println("=======Apagando motor derecho========");  
    Serial.println(AnguloNecesario);
    Serial.print("Direccion leida: ");
    Serial.println(AnguloLeido); 
    Serial.print("Diferencia angular hacia la izquierda: ");
    Serial.println(diferenciaAngular);
    Serial.println("=====================================");
    delay(25);
    return true;
  }


  //Sentencia para que apague el motor izquierdo (Se esta llendo hacia la derecha)
  if (AnguloLeido <= MargenNegativoNecesario && AnguloLeido >= Anguloopuesto   || AnguloLeido <= 180 ) {

    int diferenciaAngular=0;
    if (AnguloLeido > MargenPositivoNecesario) {
      diferenciaAngular = AnguloLeido + (360 -  MargenNegativoNecesario); 
    } 
    else {
      diferenciaAngular = AnguloLeido - AnguloNecesario;  
    }
    analogWrite (MotorDER, 220); 
    analogWrite (MotorIZQ, 0);   
    Serial.println("=======Apagando motor izquierdo========");
    Serial.print("Direccion necesaria: ");
    Serial.println(AnguloNecesario);
    Serial.print("Direccion leida: ");
    Serial.println(AnguloLeido);
    Serial.print("Diferencia angular hacia la derecha: ");
    Serial.println(diferenciaAngular);
    Serial.println("=====================================");
    delay(25);
    return true;
  }

  return false;



}

Chicos de ante mano gracias y espero sus respuestas.

https://dl.dropboxusercontent.com/u/19634672/IMG005.jpg ProtoBoard y arduino Uno
https://dl.dropboxusercontent.com/u/19634672/85433.jpg Carro
https://dl.dropboxusercontent.com/u/19634672/IMG006.jpg ]Ruedas ...