CÓDIGO DE EJEMPLO PARA SOLUCIÓN ALGORITMO DE LABERINTO POR MANO IZQUIERDA MÉTODO POR INTRODUCCIÓN DE DATOS SERIAL

Cordial Saludo,

Adjunto código de ejemplo para la asignatura Microcontroladores 1.


Código:

/* Solucion ALgoritmo Robot Laberinto tipo seguidor de linea con tipologia de mano izquierda
 *  
 *  Estas son las tres secuencias a determinar:
    LBR = B
    LBS = R
    RBL = B
    SBL = R
    SBS = B
    LBL = S
 *  
 */
char c;
int pos=0;
char letras[200];
int imp = 0;

void setup() {
  Serial.begin(9600);
}

void loop() {
  recogerdatos();
  procesardatosaltiempo();
  imprimirdatos();
}

void recogerdatos(){
  if (Serial.available()>0){
    c = Serial.read();
    if ((c == 'R')||(c == 'L')||(c == 'S')||(c == 'B')){
      letras[pos] = c;
      Serial.print(c);
      pos++;
    }else if (c == 'F'){
      imp = 1;
    }else{
      Serial.print(c);
      Serial.println(" No es un Dato Permitido");  
    }
  }  
}

void imprimirdatos(){
  if (imp == 1){
    Serial.println();
    Serial.println(letras);
    imp = 0;
    for (int i=0;i<pos;i++){
      letras[i] = ' ';
    }
    pos = 0;
  }
 }

void procesardatosaltiempo(){
  if (pos>=3){
    if (letras[pos-2]=='B'){
      if (((letras[pos-3]=='L')&&(letras[pos-1]=='R'))||
          ((letras[pos-3]=='R')&&(letras[pos-1]=='L'))||
          ((letras[pos-3]=='S')&&(letras[pos-1]=='S'))){
        letras[pos-3] = 'B';
        letras[pos-2] = ' ';
        letras[pos-1] = ' ';
        pos = pos - 2;
      }
      if (((letras[pos-3]=='L')&&(letras[pos-1]=='S'))||
          ((letras[pos-3]=='S')&&(letras[pos-1]=='L'))){
        letras[pos-3] = 'R';
        letras[pos-2] = ' ';
        letras[pos-1] = ' ';
        pos = pos - 2;
      }
      if ((letras[pos-3]=='L')&&(letras[pos-1]=='L')){
        letras[pos-3] = 'S';
        letras[pos-2] = ' ';
        letras[pos-1] = ' ';
        pos = pos - 2;
      }
    }
  }
}