Diff: main.cpp

Revision:

13:733030741583

Parent:

12:00ba3be0d995

Child:

14:6acac859ceb0

--- a/main.cpp	Fri Feb 28 22:45:38 2020 +0000
+++ b/main.cpp	Sat Feb 29 15:13:23 2020 +0000
@@ -4,7 +4,7 @@
 
 //========================= DEFINICOES =========================//
 
-#define threshold 160 // parametro que diferencia a leitura do branco pro preto
+#define threshold 110 // parametro que diferencia a leitura do branco pro preto
 PwmOut pwm(p25);
 DigitalOut dir (p26);
 PwmOut pwm1(p24);
@@ -44,10 +44,11 @@
 //============== VARIAVEIS MAIS IMPORTANTES ================//
 //Separadas do resto para facilitar a alteração
 
-float ParametroCorrecao = 1.02;                //1.1   //       4.63    //  2  ----> 1.15----1.02
-float Kp_Sensor = 0.92; //0.14;   //0.2     //0.4   //      0.2     //  0.35--->0.55--0.7--0.64--0.92
-float Kd_Sensor = 12.1; //1.75;   //1.15     //0.6   //       2.7     //  4.3 --->7.1--8.4--8.8--12.1
-float BaseSpeed = 0.18;                     //0.12  //       0.15    //  0.18--->0.18
+float ParametroCorrecao = 4.63;                //1.1   //       4.63    //  2  ----> 1.15----1.02
+float Kp_Sensor = 0.2; //0.14;   //0.2     //0.4   //      0.2     //  0.35--->0.55--0.7--0.64--0.92
+float Kd_Sensor = 2.7; //1.75;   //1.15     //0.6   //       2.7     //  4.3 --->7.1--8.4--8.8--12.1
+float BaseSpeed = 0.15;                     //0.12  //       0.15    //  0.18--->0.18
+float Ki_Sensor = 0;
 
 //==========================================================//
 //#define VMAX_PWM 100
@@ -65,7 +66,7 @@
 int TotalError = 0;
 int LastSensor = 0;
 int SET_POINT = 45;
-
+int Integral=0;
 
 //float Turbo = BaseSpeed;
 
@@ -77,8 +78,8 @@
 
 int LastError = 0;
 //int Integral_Esquerda = 0, Integral_Direita = 0;
-int MAX_INTEGRAL = 50;
-int MIN_INTEGRAL = -50;
+int MAX_INTEGRAL = 25;
+int MIN_INTEGRAL = -25;
 float Correcao_Sensor;
 
 //int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1;
@@ -221,13 +222,18 @@
 
 float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção
     Error = Position - SET_POINT;
+    Integral=Integral + Error;
     //bt.printf("\n\rErro: %d", Error);
+    if (Integral>MAX_INTEGRAL){
+        Integral=MAX_INTEGRAL;}
+    else if (Integral<MIN_INTEGRAL){
+        Integral=MIN_INTEGRAL;}
     
     if (Error < 25 && Error > -25){
-        Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError);
+        Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError) + Integral*Ki_Sensor ;
     }
     else{
-        Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError);
+        Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError) + Integral*Ki_Sensor;
     }
     
     //bt.printf("\n\rCorrecao: %.2f | Erro: %d | LastError: %d | KP: %.1f | KD: %.1f", Correcao_Sensor, Error, LastError, (Kp_Sensor * (float)Error), (Kd_Sensor * ((float)Error - (float)LastError)));