Diff: main.cpp

Revision:

5:41a59c155649

Parent:

4:247c0fc124db

--- a/main.cpp	Tue Feb 25 21:20:05 2020 +0000
+++ b/main.cpp	Tue Feb 25 21:25:23 2020 +0000
@@ -65,6 +65,7 @@
 int TotalError = 0;
 int LastSensor = 0;
 int SET_POINT = 45;
+int lastRead;
 
 float turbo = 0.05;
 
@@ -146,14 +147,14 @@
 void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição
     
     if (sensorCheck(3)== 1 && sensorCheck(4) == 1) {Position = 45;} //Se ler os dois do meio, está no SetPoint
-    else if (sensorCheck(3)==1) {Position = 40;}
-    else if (sensorCheck(4)==1) {Position = 50;}
-    else if (sensorCheck(2)==1) {Position = 30;}
-    else if (sensorCheck(5)==1) {Position = 60;}
-    else if (sensorCheck(1)==1) {Position = 20;}
-    else if (sensorCheck(6)==1) {Position = 70;}
-    else if (sensorCheck(0)==1) {Position = 10;}
-    else if (sensorCheck(7) == 1) {Position = 80;} 
+    else if (sensorCheck(3)==1 && (lastRead == 2 || lastRead==4)) {Position = 40;}
+    else if (sensorCheck(4)==1 && (lastRead == 3 || lastRead==5)) {Position = 50;}
+    else if (sensorCheck(2)==1 && (lastRead == 1 || lastRead==3)) {Position = 30;}
+    else if (sensorCheck(5)==1 && (lastRead == 4 || lastRead==6)) {Position = 60;}
+    else if (sensorCheck(1)==1 && (lastRead == 0 || lastRead==3)) {Position = 20;}
+    else if (sensorCheck(6)==1 && (lastRead ==5 || lastRead==7)) {Position = 70;}
+    else if (sensorCheck(0)==1 && lastRead == 1) {Position = 10;}
+    else if (sensorCheck(7) == 1 && lastRead = 6) {Position = 80;} 
    
     //pc.printf("\n\rPosicao: %d", Position); //Print de teste
 }