u/Dominetdude

i've been working hard on the code, but even so, i can't reach to something satisfactory yet. How can I make that my robot doesn't make any mistakes like looping in circles or turning around. this is my current code:
#include "MeMegaPi.h"
//codigo antiguo 
MeMegaPiDCMotor motorIzq(PORT1); //motor izquierdo
MeMegaPiDCMotor motorDer(PORT2); //motor derecho
MeLineFollower moduloIzq(PORT5);
MeLineFollower moduloCen(PORT6);
MeLineFollower moduloDer(PORT7);
//parametros de velocidad
const int VEL_RECTA = 90;
const int VEL_CURVA_SUAVE = 80;
const int VEL_CURVA_FUERTE = 70;
const int VEL_MAX = 110;
//PID
float Kp = 20.0;
float Kd = 40.0;
int ultimoError = 0;


unsigned long tiempoBlanco = 0;
bool perdiendoLinea = false;
const int sentido_ORC = 2; //Preferencia de sentido de aguja del reloj, si es 1 es izquierda si es 2 es derecha.
bool arranqueORCResuelto = false; //variable de giro preferencial forzado al arranque 


void setup() {
  delay(1500);
}


void loop() {
  int stI = moduloIzq.readSensors();//sensor izquierda
  int stC = moduloCen.readSensors();//sensor centram
  int stD = moduloDer.readSensors();//sensor derecho
//condicionales
//condicion giro izquierdo en caso de T
  if (stC != 3 &amp;&amp; stI != 3 &amp;&amp; stD == 3) {
    mover(-125, 135);
    perdiendoLinea = false;
    tiempoBlanco = 0;
    ultimoError = 0;
    return;
  }
//condicion giro derecho en caso de T
  if (stC != 3 &amp;&amp; stD != 3 &amp;&amp; stI == 3) {
    mover(135, -125);
    perdiendoLinea = false;
    tiempoBlanco = 0;
    ultimoError = 0;
    return;
  }
//Recta
  if (stC == 0) {
    mover(VEL_RECTA, VEL_RECTA);
    perdiendoLinea = false;
    tiempoBlanco = 0;
    ultimoError = 0;
    return;
  }
//condicional en caso de que todo sea negro, si no se ha resuelto el arranque orc, usar sentido preferencial ORC.
  if (stC != 3 || stI != 3 || stD != 3) {
    ejecutarPID(stI, stC, stD);
    perdiendoLinea = false;
    tiempoBlanco = 0;
  } else {
    if (!perdiendoLinea &amp;&amp; !arranqueORCResuelto) {
      if (sentido_ORC == 1) mover(-135, 135);
      else mover(135, -135);
      arranqueORCResuelto = true;
      tiempoBlanco = millis();
      perdiendoLinea = true;
    }
    if (!perdiendoLinea) {
      tiempoBlanco = millis();
      perdiendoLinea = true;
    }
    unsigned long duracionBlanco = millis() - tiempoBlanco;
    if (duracionBlanco &lt; 130) { //Delay de reacciob principal
      mover(VEL_RECTA - 20, VEL_RECTA - 20); //girar
    } else if (duracionBlanco &lt; 900) { //tiempo que el robot espera para girar a otro lado
      if (ultimoError &gt; 0) mover(135, -135);
      else mover(-135, 135);
    } else if (duracionBlanco &lt; 1700) { //modo recuperacion
      if (ultimoError &gt; 0) mover(-135, 135);
      else mover(135, -135);
    } else if (duracionBlanco &lt; 2800) {
      if ((duracionBlanco / 200) % 2 == 0) mover(-100, -60);
      else mover(-60, -100);
    } else {
      mover(-90, -90);
    }
  }
}
//ejecucion del PID
void ejecutarPID(int stI, int stC, int stD) {
  int error = calcularError(stI, stC, stD);
  float correccion;
  if (abs(error) &lt;= 1) {
    correccion = error * Kp;
  } else {
    correccion = (error * Kp) + ((error - ultimoError) * Kd);
  }
  int velBaseActual = (abs(error) &lt;= 1) ? VEL_RECTA : VEL_CURVA_SUAVE;
  int vI = velBaseActual + (int)correccion;
  int vD = velBaseActual - (int)correccion;
  if (abs(error) &gt;= 9) {
    if (error &gt; 0) vD = -125;
    else vI = -125;
  }
  mover(constrain(vI, -VEL_MAX, VEL_MAX), constrain(vD, -VEL_MAX, VEL_MAX));
  if (error != 0) ultimoError = error;
}
//Paramemtros de PID en errores
int calcularError(int stI, int stC, int stD) {
  if (stC == 0) return 0;
  if (stC == 2) return -1;
  if (stC == 1) return 1;
  if (stI == 1) return -4;
  if (stI == 0) return -7;
  if (stI == 2) return -11;
  if (stD == 2) return 4;
  if (stD == 0) return 7;
  if (stD == 1) return 11;
  return ultimoError;
}
//funcion de mover motores
void mover(int izq, int der) {
  motorIzq.run(izq);
  motorDer.run(-der);
}
 #include "MeMegaPi.h"
//codigo antiguo 
MeMegaPiDCMotor motorIzq(PORT1); //motor izquierdo
MeMegaPiDCMotor motorDer(PORT2); //motor derecho
MeLineFollower moduloIzq(PORT5);
MeLineFollower moduloCen(PORT6);
MeLineFollower moduloDer(PORT7);
//parametros de velocidad
const int VEL_RECTA = 90;
const int VEL_CURVA_SUAVE = 80;
const int VEL_CURVA_FUERTE = 70;
const int VEL_MAX = 110;
//PID
float Kp = 20.0;
float Kd = 40.0;
int ultimoError = 0;


unsigned long tiempoBlanco = 0;
bool perdiendoLinea = false;
const int sentido_ORC = 2; //Preferencia de sentido de aguja del reloj, si es 1 es izquierda si es 2 es derecha.
bool arranqueORCResuelto = false; //variable de giro preferencial forzado al arranque 


void setup() {
  delay(1500);
}


void loop() {
  int stI = moduloIzq.readSensors();//sensor izquierda
  int stC = moduloCen.readSensors();//sensor centram
  int stD = moduloDer.readSensors();//sensor derecho
//condicionales
//condicion giro izquierdo en caso de T
  if (stC != 3 &amp;&amp; stI != 3 &amp;&amp; stD == 3) {
    mover(-125, 135);
    perdiendoLinea = false;
    tiempoBlanco = 0;
    ultimoError = 0;
    return;
  }
//condicion giro derecho en caso de T
  if (stC != 3 &amp;&amp; stD != 3 &amp;&amp; stI == 3) {
    mover(135, -125);
    perdiendoLinea = false;
    tiempoBlanco = 0;
    ultimoError = 0;
    return;
  }
//Recta
  if (stC == 0) {
    mover(VEL_RECTA, VEL_RECTA);
    perdiendoLinea = false;
    tiempoBlanco = 0;
    ultimoError = 0;
    return;
  }
//condicional en caso de que todo sea negro, si no se ha resuelto el arranque orc, usar sentido preferencial ORC.
  if (stC != 3 || stI != 3 || stD != 3) {
    ejecutarPID(stI, stC, stD);
    perdiendoLinea = false;
    tiempoBlanco = 0;
  } else {
    if (!perdiendoLinea &amp;&amp; !arranqueORCResuelto) {
      if (sentido_ORC == 1) mover(-135, 135);
      else mover(135, -135);
      arranqueORCResuelto = true;
      tiempoBlanco = millis();
      perdiendoLinea = true;
    }
    if (!perdiendoLinea) {
      tiempoBlanco = millis();
      perdiendoLinea = true;
    }
    unsigned long duracionBlanco = millis() - tiempoBlanco;
    if (duracionBlanco &lt; 130) { //Delay de reacciob principal
      mover(VEL_RECTA - 20, VEL_RECTA - 20); //girar
    } else if (duracionBlanco &lt; 900) { //tiempo que el robot espera para girar a otro lado
      if (ultimoError &gt; 0) mover(135, -135);
      else mover(-135, 135);
    } else if (duracionBlanco &lt; 1700) { //modo recuperacion
      if (ultimoError &gt; 0) mover(-135, 135);
      else mover(135, -135);
    } else if (duracionBlanco &lt; 2800) {
      if ((duracionBlanco / 200) % 2 == 0) mover(-100, -60);
      else mover(-60, -100);
    } else {
      mover(-90, -90);
    }
  }
}
//ejecucion del PID
void ejecutarPID(int stI, int stC, int stD) {
  int error = calcularError(stI, stC, stD);
  float correccion;
  if (abs(error) &lt;= 1) {
    correccion = error * Kp;
  } else {
    correccion = (error * Kp) + ((error - ultimoError) * Kd);
  }
  int velBaseActual = (abs(error) &lt;= 1) ? VEL_RECTA : VEL_CURVA_SUAVE;
  int vI = velBaseActual + (int)correccion;
  int vD = velBaseActual - (int)correccion;
  if (abs(error) &gt;= 9) {
    if (error &gt; 0) vD = -125;
    else vI = -125;
  }
  mover(constrain(vI, -VEL_MAX, VEL_MAX), constrain(vD, -VEL_MAX, VEL_MAX));
  if (error != 0) ultimoError = error;
}
//Paramemtros de PID en errores
int calcularError(int stI, int stC, int stD) {
  if (stC == 0) return 0;
  if (stC == 2) return -1;
  if (stC == 1) return 1;
  if (stI == 1) return -4;
  if (stI == 0) return -7;
  if (stI == 2) return -11;
  if (stD == 2) return 4;
  if (stD == 0) return 7;
  if (stD == 1) return 11;
  return ultimoError;
}
//funcion de mover motores
void mover(int izq, int der) {
  motorIzq.run(izq);
  motorDer.run(-der);
}
I would apreciatte it if any of you guys help me, thanks anyway :)
How can i improve my code for my line follower robot?
