main.cpp

00001 #include "mbed.h"
00002 #include "HCSR04.h"
00003 
00004 #define velocidade(a, b)    MDPWM = a; MEPWM = b 
00005 #define sentido(a, b)       MDdirect = a; MEdirect = b
00006 
00007 //DigitalOut myled(LED1);
00008 
00009 Serial pc(USBTX,USBRX);     
00010 HCSR04 sonarF(D13, D12);
00011 
00012 DigitalOut MEdirect(D4);    //Motor 2 Direction control
00013 DigitalOut MDdirect(D7);    //Motor 1 Direction control
00014 PwmOut MEPWM(D5);           //Motor 2 PWM control
00015 PwmOut MDPWM(D6);           //Motor 1 PWM control
00016 
00017 float intA = 0.0;                         //Valor da ação de controle integral no instante anterior
00018 float 
00019     tIni,
00020     tFin,
00021     T = 0.1;                //Tempo de amostragem  
00022 float 
00023     SP = 25.0,             //SetPoint = 25 cm
00024     v,                      //Variável de controle, velocidade
00025     VM;                     //Variável controlada, distância
00026 float 
00027     Kp = 0.03, 
00028     Ki = 0.02, 
00029     Kd = 0.005;             //Ganho de cada ação de controle
00030 float
00031     aProp, 
00032     aDeri, 
00033     aInt;                   //Valor de cada ação de controle   
00034 float 
00035     e, 
00036     eA = 0.0;               //erro do sistema                              
00037 
00038 int main() {
00039     wait(1.0);
00040     velocidade(0.4, 0.4);
00041     while(1){
00042         VM = sonarF.getCm();
00043         printf("%1.2f\n", VM);
00044         e = SP - VM;
00045         
00046         aProp = Kp * e;
00047         aInt = intA + Ki * (T * ((eA + e)/ 2.0));   
00048         aDeri = Kd * ((e - eA)/T);
00049         
00050         eA = e;
00051         intA = aInt;
00052         
00053         v = aProp + aInt + aDeri;
00054               
00055         if(e <= 0){
00056             velocidade(v*(-1), v*(-1));
00057             sentido(1, 1);
00058         }
00059         else{
00060             velocidade(v, v);
00061             sentido(0, 0);
00062         }
00063         wait(T);
00064     }
00065 }