File content as of revision 2:a68b0082faac:

#include "mbed.h"
#include "motors.h"
//#include "PID.h"

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

//------------------ FUNCOES ----------------------
#define velocidadeBase 0.18

PwmOut rodaEsquerda(p25); // Potencias da roda e direções
DigitalOut direcaoEsquerda (p26);
PwmOut rodaDireita(p24);
DigitalOut direcaoDireita (p23);
AnalogIn batMeasure (p22);


void Setup_Motores(void)//Funcao de setup dos motores, ajustando o periodo para inicializar o pwm
{ 
rodaEsquerda.period(0.01); 
rodaDireita.period(0.01); 
}

void ControleMotores (float VelocidadeEsquerda, float VelocidadeDireita ){ //Função drive, controla a velocidade de cada roda
    
    if (VelocidadeEsquerda <= 0){
        VelocidadeEsquerda = abs(VelocidadeEsquerda);//Utiliza o modulo da velocidade, pois esta nao pode ser negativa
        direcaoEsquerda = 0;//Muda a direcao da roda.
        rodaEsquerda.write(VelocidadeEsquerda);
    }
    else { direcaoEsquerda = 1; rodaEsquerda.write(VelocidadeEsquerda); }//Mantem a direcao da roda.
    
    if (VelocidadeDireita <= 0){
        VelocidadeDireita = abs(VelocidadeDireita);
        direcaoDireita = 0;
        rodaDireita.write(VelocidadeDireita);    
    }
    else { direcaoDireita = 1; rodaDireita.write(VelocidadeDireita); }
}

void Acelerando(float velocidadeMax)//Acelera o seguidor de linha
{
    int bandeira = 0;
    float novaVelocidade = velocidadeBase;
    while(bandeira == 0)
    {
        novaVelocidade = novaVelocidade + 0.01;
        wait(0.001);//Deve ser testado se e necessario esse wait
        if (novaVelocidade <= velocidadeMax)
        {
            bandeira = 1;
        }
    }
}

void Desacelerando(float velocidadeMin)//Desacelera o seguidor de linha
{
    int bandeira = 0;
    float novaVelocidade = velocidadeBase;
    while(bandeira == 0)
    {
        novaVelocidade = novaVelocidade - 0.01;
        wait(0.001);//Deve ser testado se e necessario esse wait
        if (novaVelocidade <= velocidadeMin)
        {
            bandeira = 1;
        }
    }
}

void VelocidadeBateria() // Funcao de setup da velocidade base dos motores
{
    float leituraBateria = batMeasure; //Lendo a carga da bateria em um numero de 0 a 1
    float tensaoBateria = 12.6 * leituraBateria; // Carga atual da bateria em Volts
    float novaVelocidade = velocidadeBase * 8.4/tensaoBateria;// Calculando a nova velocidade de acordo com a medida de carga atual da bateria
    return;
}

/*float ajustaVelocidade(float velocidadeEsperada, float velocidadeReal)
{
    Correcao_roda=PID_roda();
    velocidadeEsperada=velocidadeReal + Correcao_roda
    return velocidadeEsperada;
}*/

void ParadaFinal()//Para o seguidor de linha
{ 
while(1) 
{ 
 rodaEsquerda.write(0); 
 rodaDireita.write(0);
 }
 } //Função de parada do LF depois do percurso 
    
/*void Para_Demonio (void){ direcaoEsquerda = 0; direcaoDireita = 0; rodaEsquerda.write(0); rodaDireita.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; }
}
*/