Diff: main.cpp

Revision:

3:3b0c36115348

Parent:

2:28f4f676e446

Child:

4:247c0fc124db

--- a/main.cpp	Tue Feb 25 16:12:36 2020 +0000
+++ b/main.cpp	Tue Feb 25 21:06:44 2020 +0000
@@ -4,7 +4,7 @@
 
 //========================= DEFINICOES =========================//
 
-#define threshold 250 // parametro que diferencia a leitura do branco pro preto
+#define threshold 120 // parametro que diferencia a leitura do branco pro preto
 PwmOut pwm(p25);
 DigitalOut dir (p26);
 PwmOut pwm1(p24);
@@ -22,7 +22,7 @@
 
 //Ticker END;
 //Ticker END1;
-
+DigitalIn sensorTurbo(p11);
 DigitalOut multiplexador1(p5); // portas do multiplexador que selecionam o sensor de linha
 DigitalOut multiplexador2(p6);
 DigitalOut multiplexador3(p8); // PRECISA AJUSTAR OS PINOS
@@ -44,10 +44,10 @@
 //============== VARIAVEIS MAIS IMPORTANTES ================//
 //Separadas do resto para facilitar a alteração
 
-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
+float ParametroCorrecao = 2.43;                 //2
+float Kp_Sensor = 0.24; //0.14;   //0.2     //0.16
+float Kd_Sensor = 0.7; //1.75;   //1.15     //0.3
+float BaseSpeed = 0.15;                     //0.12
 
 //==========================================================//
 //#define VMAX_PWM 100
@@ -66,7 +66,7 @@
 int LastSensor = 0;
 int SET_POINT = 45;
 
-//float Turbo = BaseSpeed;
+float turbo = 0.05;
 
 //short LimiteFim;
 //short ContaFim = 0;
@@ -97,6 +97,7 @@
 
 void Setup_Sensores (void){ //Define o modo de cada sensor
     multiplexadorInOut.mode(PullNone);
+    sensorTurbo.mode(PullUp);
 }
 
 bool sensorCheck(int sensor){// int sensor é qual sensor se deseja ler,entre 0 a 15
@@ -198,7 +199,7 @@
 =================================================*/
 
 float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção
-    Error = Position - SET_POINT;
+    Error = Position - SET_POINT;  
     //bt.printf("\n\rErro: %d", Error);
     
     if (Error < 20 && Error > -20){
@@ -321,6 +322,11 @@
         Esquerda_Esperada = -1*(BaseSpeed + (Correction * 0.01)); //Aplica a correção
         Direita_Esperada = BaseSpeed - (Correction * 0.01);
         
+        if (sensorTurbo.read() == 0 && sensorCheck(3)){
+         Esquerda_Esperada += turbo;
+         Direita_Esperada += turbo;   
+        }
+        
         //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada);
         //Print das velocidades que estão chegando nas rodas após o PID