Revision 1:bdfcf96dc8b1, committed 2015-03-20

Comitter:

Nestordp

Date:

Fri Mar 20 17:48:46 2015 +0000

Parent:

0:baab7b3370d5

Commit message:

Controle PID

Changed in this revision

diff -r baab7b3370d5 -r bdfcf96dc8b1 HC-SR04.lib
--- a/HC-SR04.lib	Sun Feb 22 17:16:46 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/Nestordp/code/HC-SR04/#12d6d751f4fc

diff -r baab7b3370d5 -r bdfcf96dc8b1 main.cpp
--- a/main.cpp	Sun Feb 22 17:16:46 2015 +0000
+++ b/main.cpp	Fri Mar 20 17:48:46 2015 +0000
@@ -5,8 +5,8 @@
 #define sentido(a, b)       MDdirect = a; MEdirect = b
 
 //DigitalOut myled(LED1);
-//Serial pc(USBTX,USBRX);     
 
+Serial pc(USBTX,USBRX);     
 HCSR04 sonarF(D13, D12);
 
 DigitalOut MEdirect(D4);    //Motor 2 Direction control
@@ -14,18 +14,44 @@
 PwmOut MEPWM(D5);           //Motor 2 PWM control
 PwmOut MDPWM(D6);           //Motor 1 PWM control
 
-float SP = 25.0;
-float VM;
-float Kp = 0.06;
-float e;
-float v;
+float intA = 0.0;                         //Valor da ação de controle integral no instante anterior
+float 
+    tIni,
+    tFin,
+    T = 0.1;                //Tempo de amostragem  
+float 
+    SP = 25.0,             //SetPoint = 25 cm
+    v,                      //Variável de controle, velocidade
+    VM;                     //Variável controlada, distância
+float 
+    Kp = 0.03, 
+    Ki = 0.02, 
+    Kd = 0.005;             //Ganho de cada ação de controle
+float
+    aProp, 
+    aDeri, 
+    aInt;                   //Valor de cada ação de controle   
+float 
+    e, 
+    eA = 0.0;               //erro do sistema                              
 
 int main() {
+    wait(1.0);
     velocidade(0.4, 0.4);
     while(1){
         VM = sonarF.getCm();
+        printf("%1.2f\n", VM);
         e = SP - VM;
-        v = Kp * e;
+        
+        aProp = Kp * e;
+        aInt = intA + Ki * (T * ((eA + e)/ 2.0));   
+        aDeri = Kd * ((e - eA)/T);
+        
+        eA = e;
+        intA = aInt;
+        
+        v = aProp + aInt + aDeri;
+              
         if(e <= 0){
             velocidade(v*(-1), v*(-1));
             sentido(1, 1);
@@ -34,16 +60,6 @@
             velocidade(v, v);
             sentido(0, 0);
         }
-        wait(0.2);
+        wait(T);
     }
-    
-    /*while(1){
-        if(sonarF.getCm() < 25){
-            sentido(0, 0);
-        }
-        else{
-            sentido(1, 1);
-        }
-        wait(0.2);
-    }*/
 }