File content as of revision 0:9efe13b5d868:

#include "mbed.h"
#include "LineSensor.h"
#include "bluetooth.h"

#define DEBUG true

int comprimentoDoRobo = 20 ; //Comprimento do seguidor em mm

AnalogIn AsaDireitaInterno (p16);
AnalogIn AsaDireitaExterno (p15);
AnalogIn AsaEsquerdaInterno (p17);
AnalogIn AsaEsquerdaExterno (p18);
AnalogIn MultiplexadorIn (p19);
DigitalIn SensorCentral (p25);

DigitalOut Multiplexador0 (p12);
DigitalOut Multiplexador1 (p13);
DigitalOut Multiplexador2 (p14);

PwmOut Buzzer (p8);

float leituraNaoCalibrada;
float leituraMaxima [12] = {0,0,0,0,0,0,0,0,0,0,0,0};
float leituraMinima [12] = {1,1,1,1,1,1,1,1,1,1,1,1};
float leituraCalibrada [8];
float parametroDeLeituraAceitavel = 0.7;

float posicaoDosSensores [8] = {-37.5,-27.5,-17.5,-7.5,7.5,17.5,27.5,37.5};

int tabelaMultiplexador0 [8] = {0,1,0,1,0,1,0,1};
int tabelaMultiplexador1 [8] = {0,0,1,1,0,0,1,1};
int tabelaMultiplexador2 [8] = {0,0,0,0,1,1,1,1};


void AlocaLeitura (int posicaoSensor,float leitura)   //Adiciona a leitura nao calibrada aos vetores de leitura maxima ou minima
{
    if (leitura > leituraMaxima[posicaoSensor])
        {
            leituraMaxima[posicaoSensor] = leitura;
        }
        else if (leitura < leituraMinima[posicaoSensor])
        {
            leituraMinima[posicaoSensor] = leitura;
        }
}

void CalibragemDosSensores()    //Registra as leituras minimas e maximas feitas por cada sensor
                                //para funcionar precisa rodar a função continuamente e passar os sensores por cima da linha branca e do fundo preto da pista
{
    for(int i = 0;i<8;i++){     //Calibrando sensores frontais
        //Ajustando o multiplexador
        Multiplexador0 = tabelaMultiplexador0[i];
        Multiplexador1 = tabelaMultiplexador1[i];
        Multiplexador2 = tabelaMultiplexador2[i];
        
        //Lendo o sensor a adicionando na leitura
        leituraNaoCalibrada = MultiplexadorIn;
        
        //Salvando as leituras máximas e minimas
        AlocaLeitura(i,leituraNaoCalibrada);
    }
    //Calibragem das asas
    leituraNaoCalibrada = AsaEsquerdaExterno;
    AlocaLeitura(8, leituraNaoCalibrada);

    leituraNaoCalibrada = AsaEsquerdaInterno;
    AlocaLeitura(9, leituraNaoCalibrada);

    leituraNaoCalibrada = AsaDireitaInterno;
    AlocaLeitura(10, leituraNaoCalibrada);

    leituraNaoCalibrada = AsaDireitaExterno;
    AlocaLeitura(11, leituraNaoCalibrada);

}

float CalibragemDeLeitura (int numeroDoSensor,float leitura) // Calibra cada leitura individualmente baseado nas leituras maximas e minimas ja cadastradas de cada sensor
{
    return (leitura - leituraMinima[numeroDoSensor])/(leituraMaxima[numeroDoSensor]-leituraMinima[numeroDoSensor]) ;
}

void LerSensoresFrontais () //Faz a leitura dos sensores frontais e salva no vetor leituraCalibrada
{
    int contador = 1;

    for(int i = 0;i < 8;i++){

        Multiplexador0 = tabelaMultiplexador0[i];
        Multiplexador1 = tabelaMultiplexador1[i];
        Multiplexador2 = tabelaMultiplexador2[i];

        leituraNaoCalibrada = MultiplexadorIn;

        leituraCalibrada[i] = CalibragemDeLeitura(i,leituraNaoCalibrada);
    }


}

float CalculaErro () //Retorna o erro em graus para o calculo do PID
{
    float distancia = 0;

    for(int i = 0;i < 8;i++){//Calcula a media ponderada das leituras pra achar a distancia da linha para o setpoint
    
        distancia += ( leituraCalibrada[i] + posicaoDosSensores [i] )/ leituraCalibrada [i];
    }
    return atan(distancia/comprimentoDoRobo);
}

bool LerAsaDireita ()   //Retorna True se os sensores da asa direita lerem a linha
{
    leituraNaoCalibrada = AsaDireitaExterno;
    float leituraExterna = CalibragemDeLeitura(11,leituraNaoCalibrada);

    leituraNaoCalibrada = AsaDireitaInterno;
    float leituraInterna = CalibragemDeLeitura(10,leituraNaoCalibrada);

    if((leituraExterna > parametroDeLeituraAceitavel) || (leituraInterna > parametroDeLeituraAceitavel)){
        return true;
    }
    return false ;
}

bool LerAsaEsquerda ()   //Retorna True se os sensores da asa esquerda lerem a linha
{
    leituraNaoCalibrada = AsaEsquerdaExterno;
    float leituraExterna = CalibragemDeLeitura(8,leituraNaoCalibrada);

    leituraNaoCalibrada = AsaEsquerdaInterno;
    float leituraInterna = CalibragemDeLeitura(9,leituraNaoCalibrada);

    if((leituraExterna > parametroDeLeituraAceitavel) || (leituraInterna > parametroDeLeituraAceitavel)){
        return true;
    }
    return false ;
}

bool LerTurbo()
{
    return SensorCentral.read();
}