File content as of revision 0:b803244146f7:

//========================= BIBLIOTECAS =========================//

#include "mbed.h"

//========================= DEFINICOES =========================//

PwmOut pwm(p25);
DigitalOut dir (p26);
PwmOut pwm1(p24);
DigitalOut dir1 (p23);
//DigitalIn button(p18);
//DigitalOut led(p20);

Serial bt(p28, p27);

//InterruptIn A (p21);
//DigitalIn B (p22);
//InterruptIn C (p29);
//DigitalIn D (p30);

//Ticker END;
//Ticker END1;

DigitalIn s1(p5);  
DigitalIn s2(p6);
DigitalIn s3(p7);
DigitalIn s4(p8);
DigitalIn s5(p9);
DigitalIn s6(p10);
DigitalIn s7(p12);
DigitalIn s8(p13);

//#define Kp_Roda 1
//#define Kd_Roda 1
//#define Ki_Roda 1

//============== VARIAVEIS MAIS IMPORTANTES ================//
//Separadas do resto para facilitar a alteração

float ParametroCorrecao = 5.3;
float Kp_Sensor = 0.145; //0.14;   //0.2
float Kd_Sensor = 1.64; //1.75;   //1.15
float BaseSpeed = 0.17;

//==========================================================//
//#define VMAX_PWM 100
//#define VMAX_Encoder 100

//#define TESTE_MOTOR
//#define TESTE_SENSOR

Timeout END;
//========================= VARIAVEIS GLOBAIS =========================

int Position;
int Error=0;
int Correction;
int TotalError = 0;
int LastSensor = 0;
int SET_POINT = 45;

//float Turbo = BaseSpeed;

//short LimiteFim;
//short ContaFim = 0;
//bool  LastRead = false;
//bool  Preto[15];
//int Threshold = 600;

int LastError = 0;
//int Integral_Esquerda = 0, Integral_Direita = 0;
int MAX_INTEGRAL = 50;
int MIN_INTEGRAL = -50;
float Correcao_Sensor;

//int Contador_PID_Esquerda = 1, Contador_PID_Direita = 1;
//int Esquerda_LastError = 0, Direita_LastError = 0;
float Esquerda_Esperada, Esquerda_Real, Esquerda_Corrigida;
float Direita_Esperada, Direita_Real, Direita_Corrigida;
//int Esquerda_realSpeed, Direita_realSpeed;
//int Direita_Count = 0, Direita_LastCount = 0;
//int Esquerda_Count = 0, Esquerda_LastCount = 0;
//int Esquerda_lastExpectedSpeed, Direita_lastExpectedSpeed;

/*=================================================
-------------- CODIGO DOS SENSORES ----------------
=================================================*/

//------------------ FUNCAO -----------------------

void Setup_Sensores (void){ //Define o modo de cada sensor como PullUp
    s1.mode(PullUp); 
    s2.mode(PullUp);
    s3.mode(PullUp);
    s4.mode(PullUp);
    s5.mode(PullUp);
    s6.mode(PullUp);
    s7.mode(PullUp);
    s8.mode(PullUp);
}

void Leitura_Sensores (void){ //Faz a leitura dos sensores e retorna a posição
    
    if (s4.read()==0 && s5.read()==0) {Position = 45;} //Se ler os dois do meio, está no SetPoint
    else if (s7.read()==0) {Position = 70;}    //Ele só vai ler os sensores das extremidades
    else if (s8.read()==0) {Position = 80;}    //se tiver certeza que não está no meio (ajuda o LF a passar pelos cruzamentos)
    if (s1.read()==0) {Position = 10;}
    if (s2.read()==0) {Position = 20;}
    if (s3.read()==0) {Position = 30;}
    if (s4.read()==0) {Position = 40;}
    if (s5.read()==0) {Position = 50;}
    if (s6.read()==0) {Position = 60;}
    
    //bt.printf("\n\rPosicao: %d", Position); //Print de teste
}

/*=================================================
--------------- CODIGO DOS MOTORES ----------------
=================================================*/

//------------------ FUNCOES ----------------------

void Setup_Motores(void){ pwm.period(0.01); pwm1.period(0.01); } 

void Vai_Demonio (float VelocidadeEsquerda, float VelocidadeDireita ){ //Função drive, controla a velocidade de cada roda
    
    if (VelocidadeEsquerda <= 0){
        VelocidadeEsquerda = abs(VelocidadeEsquerda);
        dir = 0;
        pwm.write(VelocidadeEsquerda);
    }
    else { dir = 1; pwm.write(VelocidadeEsquerda); }
    
    if (VelocidadeDireita <= 0){
        VelocidadeDireita = abs(VelocidadeDireita);
        dir1 = 0;
        pwm1.write(VelocidadeDireita);    
    }
    else { dir1 = 1; pwm1.write(VelocidadeDireita); }
}
    
/*void Para_Demonio (void){ dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } //Mantém o LF parado

void Volta_Pra_Linha (){ //Faz o LF voltar para o último lado em que leu a linha, caso tenha pertido (função não funcional, precisa alterar a velocidade)
    if (Position == 0){
        if (LastSensor > 0) { Esquerda_Esperada = -50; Direita_Esperada = 80; }
        else { Esquerda_Esperada = 80; Direita_Esperada = -50; }
    }
    else { LastSensor = Error; }
}
*/
void Final_Stop(){ while(1) { dir = 0; dir1 = 0; pwm.write(0); pwm1.write(0); } } //Função de parada do LF depois do percurso
/*=================================================
---------------  PID DOS SENSORES -----------------
=================================================*/

float PID_Sensores(int Position){ //Função que aplica o PID e calcula a correção
    Error = Position - SET_POINT;
    //bt.printf("\n\rErro: %d", Error);
    
    if (Error < 20 && Error > -20){
        Correcao_Sensor = Kp_Sensor * (float)Error + Kd_Sensor * ((float)Error - (float)LastError);
    }
    else{
        Correcao_Sensor = ParametroCorrecao * Kp_Sensor * (float)Error + ParametroCorrecao * Kd_Sensor * ((float)Error - (float)LastError);
    }
    
    //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)));
    //Comando para printar e verificar os dados de saida via bluetooth
    
    LastError = Error;
    
    return Correcao_Sensor;
}

/*=================================================
---------------- PID DOS MOTORES ------------------
=================================================*/

/*

void rightEncoderEvent (){
        if (B.read() == 0){
            Direita_Count++;
        }
        
        else{
            Direita_Count--;
        }
}

void leftEncoderEvent(){
    if (C.read() == 0) { Esquerda_Count++; }
    else { Esquerda_Count--; }
}

void Speed () {

    Esquerda_realSpeed = (Esquerda_Count - Esquerda_LastCount); //A realSpeed é o numero de voltas/giros por leitura. Comparar o maximo e minimo do Afro com a leitura do encoder.
    Esquerda_LastCount = Esquerda_Count;
    
    Direita_realSpeed = (Direita_Count - Direita_LastCount);
    Direita_LastCount = Direita_Count;
    //bt.printf("\n\rCount: %d ## Speed: %d",rightCount,realSpeed);
        
}

int Converte_Velocidade (int realSpeed){
    int realPWM;
    
    realPWM = (VMAX_PWM*realSpeed)/VMAX_Encoder;
    
    return realPWM;    
}

int PID_Roda (int Lado_realPWM, int Lado_VelocidadeEsperada, int Lado_LastError, int Contador_LadoMotor, int Integral_Lado_Roda, int Lado_lastExpectedSpeed){
    int proporcional, derivativo, Correcao, Velocidade_Corrigida;
    
    proporcional = Lado_realPWM - Lado_VelocidadeEsperada;
    //bt.printf("\n\r%d",proporcional);
    derivativo = proporcional - Lado_LastError;
    //bt.printf("\n\rDerivativo: %d",derivativo);
    
    Correcao = Kp_Roda*proporcional + Kd_Roda*derivativo + Ki_Roda*Integral_Lado_Roda;
    //bt.printf("\n\r%d",Correcao);
    
    Lado_LastError = proporcional;
    Integral_Lado_Roda += proporcional;

    if (Lado_lastExpectedSpeed == Lado_VelocidadeEsperada) { return Velocidade_Corrigida; }
    
    if (proporcional == 0){ Lado_LastError = 0; }
    else{
        if (Integral_Lado_Roda >= MAX_INTEGRAL) { Integral_Lado_Roda = MAX_INTEGRAL; }
        else { Integral_Lado_Roda = MIN_INTEGRAL; }        
    
    }
    Lado_lastExpectedSpeed = Lado_VelocidadeEsperada;
    
    Velocidade_Corrigida = (Lado_realPWM - Correcao);
    
    return Velocidade_Corrigida;
}

*/

/*=================================================
---------------  FUNCAO PRINCIPAL -----------------
=================================================*/

int main(void){
    
    //A.rise(&rightEncoderEvent);
    //END.attach(&Speed, 1.0);
    //C.rise(&leftEncoderEvent);
    
    //END.attach(&Speed, 1.0);
    //END1.attach(&Speed, 1.0); //TESTAR VER SE ISSO FUNCIONA
    
    Setup_Sensores(); //Faz o setup dos sensores
    Setup_Motores(); //Faz o setup dos motores
    
    //while (button.read()) { led = 1; wait(0.2); led=0; wait(0.2); }
    
    wait(2); //Delay para estabilizar
    
    pwm.write(0.00); //Pulso inicial para cada motor
    pwm1.write(0.00);
    
    END.attach(&Final_Stop, 20); //Faz o LF parar depois de 20 segundos
    
    while(1){
        //Para_Demonio();
        Leitura_Sensores(); //Lê os sensores
        
        Correction = PID_Sensores(Position); //Calcula a correção
        
        Esquerda_Esperada = BaseSpeed + (Correction * 0.01); //Aplica a correção
        Direita_Esperada = BaseSpeed - (Correction * 0.01);
        
        //bt.printf("\n\rEsquerda: %.2f | Direita: %.2f", Esquerda_Esperada, Direita_Esperada);
        //Print das velocidades que estão chegando nas rodas após o PID
        
        //Volta_Pra_Linha(); //Volta para a linha caso tenha perdido
    
        Vai_Demonio(Esquerda_Esperada, Direita_Esperada); //Drive
            
    }
}