   Serial.println(NextCol);
   //Verifico se próxima posição é um vizinho
   /* Se for,  eles estão na mesma linha, devo ir para direita ou esquerda */
-  if(ActualRow == NextRow){  
-    while(ActualCol != NextCol){   
-      if(ActualCol < NextCol){
+  if (ActualRow == NextRow) {
+    while (ActualCol != NextCol) {
+      if (ActualCol < NextCol) {
         turnRight();
         ActualRow = NextRow;
-        ActualCol +=1;
-      }
-      else{
-        turnLeft(); 
+        ActualCol += 1;
+      }
+      else {
+        turnLeft();
         ActualRow = NextRow;
         ActualCol -= 1;
       }
       pathRow[z] = ActualRow;
       pathCol[z] = ActualCol;
       z += 1;
-    }  
+    }
     return;
   }
   /* Se for,  eles estão na mesma linha, devo ir para frente ou trás */
-  if(ActualCol == NextCol){  
-     while(ActualRow != NextRow){ 
-      if(ActualRow < NextRow){
+  if (ActualCol == NextCol) {
+    while (ActualRow != NextRow) {
+      if (ActualRow < NextRow) {
         goBackward();
         ActualRow += 1;
         ActualCol = NextCol;
       }
-      else{
+      else {
         goForward();
         ActualRow -= 1;
         ActualCol = NextCol;

    printheuristic();
    delay(1000);
    /* Caso seja o fim, finalize e imprima o resultado da computaçao */
    if(isFim()){
       /* Imprime caminho final */
       defineWay();
       delay(10000);
    }
    delay(2000);
    /* Descomente a linha a seguir para parar o código após uma interação */
    /* intervencao(); */
  }
  
  else{
  /* Robo reexecuta o movimento correto ao apertar o botão */
  /* TODO */
  }
  Serial.println("Fim do loop!");
}

/* Printa o caminho percorrido*/
void printpath(){
  Serial.println("Este eh ao caminho percorrido ate aqui:");
  for(i = 0; i < z; i++){
    Serial.print(pathRow[i]);
    Serial.print(" ");
    Serial.println(pathCol[i]);

int finalCol[m];

/* Função usada no teste, para finalizar o programa após uma execução */
void intervencao() {
  for (i = 0; i < m; i++) {
    for (j = 0; j < m; j++) {
      Serial.println(grid[i][j]);
      fim = 1;
    }
  }
}

void setup() {
  Serial.begin(115200);
  Serial.println("Estou no Setup!");

  /* Inicializa minha matriz com as distâncias de Manhattan, função heurística */
  heuristicFunction();
  ActualRow = StartRow;
  ActualCol = StartCol;
  /* O caminho obviamente inicia pelo ponto inicial */
  pathRow[0] = StartRow;
  pathCol[0] = StartCol;
  z = 1;

  /* Setando todo o grid para 1000, isso irá auxiliar no momento da execução do A* */
  for (i = 0; i < m; i++) {
    for (j = 0; j < m; j++) {
      grid[i][j] = INT_MAX;
    }
  }

  /* Local das funções de ajustes de rodas */
  //calibratemotors()
}

void loop() {
  Serial.println("Inicio do loop!");
  delay(1000);
  if (!fim) {
    /* Giro o robo, enquanto calculo o A* para cada quadrado observado */
    a_star();
    delay(1000);
    /* Encontra o menor valor no Grid para definir próxima posição */
