File content as of revision 2:0d6718386397:

#include "mbed.h"
#include "Servo.h"

/*****************
Código base para o controlador P. 

O sistema fica no aguardo do usuário para começar.
Manda um cabeçalho na serial.
Depois ele pisca o led @ 5  Hz, 50 vezes.
Ele liga os motores em IDLE.
Manda um aviso de pronto.
Começa o loop de controle @ 50 Hz
    - Le ADC ( Analogic Digital COnverter)
     - Manda o valor lido pelo ad
*****************/

#define MOTOR_RESET_CONDITION 0.0f
#define MOTOR_IDLE_CONDITION 0.05f

//Não mexer - Definição dos Pinos da Bancada    
DigitalOut myled(LED1);
InterruptIn button(USER_BUTTON);
Servo motorEsquerda(D5);
Servo motorDireita(D6);
AnalogIn encoder(PB_0);
Serial pc(SERIAL_TX, SERIAL_RX);
Ticker looper;

bool _btnFunction=false;
bool loopFlag = false;
float ref = 0.0f;
uint16_t angle;

void pressed()
{
   _btnFunction = !_btnFunction;
}

void wait_user()
{
    while(!_btnFunction) {
        wait_ms(10);
    }
    _btnFunction = 0;
}

void blink_led(int period_in_ms, int times)
{
    for(int i=0; times; i++) {
        myled = !myled;
        wait_ms(period_in_ms);
    }
    myled=0;
}

void loop()
{
    loopFlag = true;
}

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 = 0.0002;//v4 1.6*50;
    float D = 0.0005;//v4 3.5/50;
    float CP = 0;
    float CI = 0;
    float CD = 0;    
    float e_atual = 0;
    float e_anterior = 0;
    float integral_atual = 0;
    float integral_anterior = 0;
    float integral_temp = 0;
    float Ts = 0.02;
    float derivada = 0;
    float angulo_V;
    float angulo_d;
    float controlAction = 0;
        
     pc.baud(115200);
     button.fall(&pressed);
     motorEsquerda.write(MOTOR_RESET_CONDITION);
     motorDireita.write(MOTOR_RESET_CONDITION);     
     myled = 1;
     wait_user();
     myled = 0;
     pc.printf("Nucleo F401 turned on!\r\n");
     //blink_led(20, 50);
     motorDireita.write(MOTOR_IDLE_CONDITION);
     motorEsquerda.write(MOTOR_IDLE_CONDITION);
     pc.printf("Control should start in 10 s.\r\n");
     wait(1);
     looper.attach(&loop,0.02); 
     float sumError=0;   
     while(1) 
     {
         if(loopFlag)
         {
            loopFlag = 0;
            int k = 0;
            janela = 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 = janela + angulo_degree;
                k++;
                } 

            angulo_media = janela/k; //(janela[0]+janela[1]+janela[2]+janela[3])/4;
            e_anterior = e_atual;
            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 = I*integral_atual;
            
            derivada = (e_atual - e_anterior)/Ts;
            //CD = P*D*derivada;
            CD = D*derivada;
            
            controlAction = CP+CI+CD;
            if (controlAction >= 0.45){
                controlAction  = 0.45;
                }
           
            if (controlAction <= -0.45){
                controlAction  = -0.45;
            }
            
            //controlAction = 0.1*(ref - (float)angle);
            
            /*
            if(controlAction<0.95f) {
                controlAction= 0.95f;
            }
            if(controlAction>0.0f) {
                controlAction= 0.0f;
            }*/
            
            motorEsquerda.write((0.8*(0.5 - controlAction)));
            motorDireita.write(0.5 + controlAction);
            pc.printf("A[%4d]U[%.2f]\r\n",angle,controlAction);    
         }
          
      }
}