Diff: main.cpp

Revision:

10:d546eec37792

Parent:

9:fccecc817082

Child:

11:4f3dcdd4d8ce

--- a/main.cpp	Wed Feb 26 20:57:52 2020 +0000
+++ b/main.cpp	Thu Feb 27 00:01:42 2020 +0000
@@ -44,9 +44,9 @@
 //============== VARIAVEIS MAIS IMPORTANTES ================//
 //Separadas do resto para facilitar a alteração
 
-float ParametroCorrecao = 1.2;                //2   //       4.63    //  2  ----> 1.2
-float Kp_Sensor = 0.55; //0.14;   //0.2     //0.16   //      0.2     //  0.35--->0.55
-float Kd_Sensor = 7.2; //1.75;   //1.15     //0.3   //       2.7     //  4.3 --->7.2
+float ParametroCorrecao = 1.2;                //1.1   //       4.63    //  2  ----> 1.2
+float Kp_Sensor = 0.55; //0.14;   //0.2     //0.4   //      0.2     //  0.35--->0.55
+float Kd_Sensor = 7.2; //1.75;   //1.15     //0.6   //       2.7     //  4.3 --->7.2
 float BaseSpeed = 0.18;                     //0.12  //       0.15    //  0.18--->0.18
 
 //==========================================================//
@@ -65,7 +65,7 @@
 int TotalError = 0;
 int LastSensor = 0;
 int SET_POINT = 45;
-int lastRead = 3;
+
 
 //float Turbo = BaseSpeed;
 
@@ -90,6 +90,8 @@
 //int Esquerda_Count = 0, Esquerda_LastCount = 0;
 //int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed;
 
+void Final_Stop(){ while(1) { dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } } //Função de parada do LF depois do percurso 
+
 /*=================================================
 -------------- CODIGO DOS SENSORES ----------------
 =================================================*/
@@ -100,7 +102,7 @@
     multiplexadorInOut.mode(PullNone);
 }
 
-bool sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15
+int sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15
     // função para ler os motores,retorna true se for branco e false se for preto
    
     multiplexador1 = tabelaVerdade1[sensor]; // selecionando o sensor
@@ -130,7 +132,7 @@
             tempoDoSensor.reset();
             multiplexadorInOut.output();
             multiplexadorInOut = 0;
-            return false; // retorna quando for preto
+            return 0; // retorna quando for preto
             }
         
     }
@@ -140,21 +142,25 @@
     tempoDoSensor.reset();
     multiplexadorInOut.output();
     multiplexadorInOut = 0;
-    return true;// retorna quando for branco
+    return 1;// retorna quando for branco
     }
 
 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;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 = 6;}
-    else if (sensorCheck(0)==1 && (lastRead == 1||lastRead == 2)) {Position = 10;lastRead = 0;}
-    else if (sensorCheck(7) == 1 && (lastRead == 6||lastRead == 5)) {Position = 80;lastRead = 7;} 
-   
+    int leitura[8]={0,0,0,0,1,0,0,0}; // leitura representa os sensores lidos,onde 1 é branco e 0 é preto
+    for (int i=0;i<8;i++){leitura[i] = sensorCheck(i);} // leitura dos sensores
+    //analizando a leitura e setando a posição
+    if (leitura[3]== 1 && leitura[4] == 1) {Position = 45;} //Se ler os dois do meio, está no SetPoint
+    else if (leitura[2]== 1 && leitura[5] == 1) {Position = 45;}
+    else if (leitura[1]== 1 && leitura[6] == 1) {Position = 45;}
+    else if (leitura[3]==1) {Position = 40;}
+    else if (leitura[2]==1) {Position = 30;}
+    else if (leitura[5]==1) {Position = 60;}
+    else if (leitura[1]==1) {Position = 20;}
+    else if (leitura[6]==1) {Position = 70;}
+    else if (leitura[0]==1) {Position = 10;}
+    else if (leitura[7]==1) {Position = 80;}
+    else if (leitura[4]==1) {Position = 50;}
+    //else {Final_Stop();}
     //pc.printf("\n\rPosicao: %d", Position); //Print de teste
 }
 
@@ -193,7 +199,7 @@
     else { LastSensor = Error; }
 }
 */
-void Final_Stop(){ while(1) { dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } } //Função de parada do LF depois do percurso
+
 /*=================================================
 ---------------  PID DOS SENSORES -----------------
 =================================================*/
@@ -202,7 +208,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{