Diff: main.cpp

Revision:

8:66d5045d9a5b

Parent:

7:0dc01968a6e7

Child:

9:fccecc817082

--- a/main.cpp	Wed Feb 26 02:07:12 2020 +0000
+++ b/main.cpp	Wed Feb 26 04:38:56 2020 +0000
@@ -4,7 +4,7 @@
 
 //========================= DEFINICOES =========================//
 
-#define threshold 250 // parametro que diferencia a leitura do branco pro preto
+#define threshold 127 // parametro que diferencia a leitura do branco pro preto
 PwmOut pwm(p25);
 DigitalOut dir (p26);
 PwmOut pwm1(p24);
@@ -44,10 +44,10 @@
 //============== VARIAVEIS MAIS IMPORTANTES ================//
 //Separadas do resto para facilitar a alteração
 
-float ParametroCorrecao = 4.63;                //2   //       4.63
-float Kp_Sensor = 0.2; //0.14;   //0.2     //0.16   //     0.2
-float Kd_Sensor = 2.7; //1.75;   //1.15     //0.3   //       2.7
-float BaseSpeed = 0.15;                     //0.12  //     0.15
+float ParametroCorrecao = 4.968;                //2   //       4.63
+float Kp_Sensor = 0.225; //0.14;   //0.2     //0.16   //     0.2
+float Kd_Sensor = 4.1; //1.75;   //1.15     //0.3   //       2.7
+float BaseSpeed = 0.18;                     //0.12  //     0.15
 
 //==========================================================//
 //#define VMAX_PWM 100
@@ -65,6 +65,7 @@
 int TotalError = 0;
 int LastSensor = 0;
 int SET_POINT = 45;
+int lastRead = 3;
 
 //float Turbo = BaseSpeed;
 
@@ -144,15 +145,15 @@
 
 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;} 
+    if (sensorCheck(3)== 1 && sensorCheck(4) == 1) {Position = 45;lastRead = 3;} //Se ler os dois do meio, está no SetPoint
+    else if (sensorCheck(3)==1) {Position = 40;lastRead = 3;}
+    else if (sensorCheck(4)==1) {Position = 50;lastRead = 4;}
+    else if (sensorCheck(2)==1) {Position = 30;lastRead = 2;}
+    else if (sensorCheck(5)==1) {Position = 60;lastRead = 5;}
+    else if (sensorCheck(1)==1) {Position = 20;lastRead = 1;}
+    else if (sensorCheck(6)==1) {Position = 70;lastRead = 2;}
+    else if (sensorCheck(0)==1 && (lastRead == 2 || lastRead == 1)) {Position = 10;lastRead = 0;}
+    else if (sensorCheck(7) == 1 && (lastRead == 6 ||lastRead == 5)) {Position = 80;lastRead = 7;} 
    
     //pc.printf("\n\rPosicao: %d", Position); //Print de teste
 }
@@ -201,7 +202,7 @@
     Error = Position - SET_POINT;
     //bt.printf("\n\rErro: %d", Error);
     
-    if (Error < 20 && Error > -20){
+    if (Error < 25 && Error > -25){
         Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError);
     }
     else{