Diff: main.cpp

Revision:

2:28f4f676e446

Parent:

1:444af82e35cb

Child:

3:3b0c36115348

Child:

6:11bf0cae37a2

--- a/main.cpp	Sat Feb 22 18:41:39 2020 +0000
+++ b/main.cpp	Tue Feb 25 16:12:36 2020 +0000
@@ -4,7 +4,7 @@
 
 //========================= DEFINICOES =========================//
 
-#define threshold 10600 // parametro que diferencia a leitura do branco pro preto
+#define threshold 250 // 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 = 5.3;
-float Kp_Sensor = 0.145; //0.14;   //0.2
-float Kd_Sensor = 1.64; //1.75;   //1.15
-float BaseSpeed = 0.12;
+float ParametroCorrecao = 2;                 //2
+float Kp_Sensor = 0.16; //0.14;   //0.2     //0.16
+float Kd_Sensor = 0.3; //1.75;   //1.15     //0.3
+float BaseSpeed = 0.12;                     //0.12
 
 //==========================================================//
 //#define VMAX_PWM 100
@@ -112,7 +112,7 @@
     
     tempoDoSensor.start(); // iniciando o contador
     
-    wait_ms(10); // tempo para o output ir para high
+    wait_us(10); // tempo para o output ir para high
 
     multiplexadorInOut.input(); // colocando o InOut como In
     //pc.printf("Status do Sensor supostamente em HIGH: %d ", multiplexadorInOut.read());
@@ -145,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(0) == 1) {Position = 10;}    //Ele só vai ler os sensores das extremidades
-    else if (sensorCheck(7) == 1) {Position = 80;}    //se tiver certeza que não está no meio (ajuda o LF a passar pelos cruzamentos)
-    if (sensorCheck(1)==1) {Position = 20;}
-    if (sensorCheck(2)==1) {Position = 30;}
-    if (sensorCheck(3)==1) {Position = 40;}
-    if (sensorCheck(4)==1) {Position = 50;}
-    if (sensorCheck(5)==1) {Position = 60;}
-    if (sensorCheck(6)==1) {Position = 70;}
-    
+    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;} 
+   
     //pc.printf("\n\rPosicao: %d", Position); //Print de teste
 }
 
@@ -318,7 +318,7 @@
         
         Correction = PID_Sensores(Position); //Calcula a correção
         
-        Esquerda_Esperada = BaseSpeed + (Correction * 0.01); //Aplica a correção
+        Esquerda_Esperada = -1*(BaseSpeed + (Correction * 0.01)); //Aplica a correção
         Direita_Esperada = BaseSpeed - (Correction * 0.01);
         
         //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada);