Diff: main.cpp

Revision:

1:3d924dfe29d6

Parent:

0:802513e9e494

Child:

2:0d6718386397

--- a/main.cpp	Mon Nov 25 23:13:48 2019 +0000
+++ b/main.cpp	Wed Nov 27 18:13:30 2019 +0000
@@ -59,9 +59,14 @@
 }
 
 int main() {
+    float janela[4] = {0, 0, 0, 0};
+    float angulo_degree = 0;
+    float angulo_media;
     float P = 0.002; //v4 3*0.001;//3*0.00001;
-    float I = 1.6*5;//v4 1.6*50;
-    float D = 3.5/200;//v4 3.5/50;
+    //float I = 1.6*5;//v4 1.6*50;
+    //float D = 3.5/200;//v4 3.5/50;
+    float I = 0.0002;//v4 1.6*50;
+    float D = 0.0005;//v4 3.5/50;
     float CP = 0;
     float CI = 0;
     float CD = 0;    
@@ -96,25 +101,34 @@
          if(loopFlag)
          {
             loopFlag = 0;
-            angle =  encoder.read_u16()>>4; //Tensão em 12 bits
-            /******
-            Espaço reservado para colocar a ação de controle
-            O exemplo é de um P
-            ************/
-            angulo_V = 0.000806*angle;
-            angulo_d = 27.779*angulo_V - 41.1;
+            int k = 0;
+            while (k < 4){
+                angle =  encoder.read_u16()>>4; //Tensão em 12 bits
+                /******
+                Espaço reservado para colocar a ação de controle
+                O exemplo é de um P
+                ************/
+                angulo_V = 0.000806*angle;
+                angulo_degree = 27.779*angulo_V - 41.1;
+                janela[k] = angulo_degree;
+                k++;
+                } 
+
+            angulo_media = (janela[0]+janela[1]+janela[2]+janela[3])/4;
             e_anterior = e_atual;
-            e_atual = -(angulo_d - ref);
+            e_atual = -(angulo_media - ref);
             CP = P * e_atual;
             
             
             integral_atual = integral_anterior + (e_atual + e_anterior)*Ts/2;
             integral_anterior = integral_atual;
             
-            CI = P*I*integral_atual;
+            //CI = P*I*integral_atual;
+            CI = I*integral_atual;
             
             derivada = (e_atual - e_anterior)/Ts;
-            CD = P*D*derivada;
+            //CD = P*D*derivada;
+            CD = D*derivada;
             
             controlAction = CP+CI+CD;
             if (controlAction >= 0.45){
@@ -135,7 +149,7 @@
                 controlAction= 0.0f;
             }*/
             
-            motorEsquerda.write(0.5 - controlAction);
+            motorEsquerda.write((0.8*(0.5 - controlAction)));
             motorDireita.write(0.5 + controlAction);
             pc.printf("A[%4d]U[%.2f]\r\n",angle,controlAction);    
          }