File content as of revision 2:f1f6b33cd7bd:

#include "mbed.h"
#include "RoboClaw.h"
#include "TextLCD.h"
#include "MSCFileSystem.h"
#include "cmsis_os.h"

#define tempo_period 50 //Periodo do PWM
#define diametro_direita 42.74 //mm
#define diametro_esquerda 42.67 //mm
#define pi 3.1415926535897
#define velocidade_max 220.00 //cm/seg
#define tam_vetor 64
 
TextLCD lcd(p5, p6, p7, p8, p29, p30, TextLCD::LCD20x4); // rs, e, d4-d7
RoboClaw Placa(void);
MSCFileSystem  sd("sd");


//Serial Robo(p28, p27); // tx, rx

AnalogIn sensor1(p15); 
AnalogIn sensor2(p16); 
AnalogIn sensor3(p17); 
AnalogIn sensor4(p19); 

DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);


//Declaracao de variaveis universal RoboClaw:
char endereco = 128;
char comando = 0;
char valor = 0;
char check8 = 0;    
char buffer = 0;  
unsigned int aceleracao = 0;
int velocidade = 0;
int velocidade_e = 0;
int velocidade_d = 0;
unsigned int cmd_distancia = 0;
unsigned int cmd_distancia_e = 0;
unsigned int cmd_distancia_d = 0;
unsigned int distancia_e = 0;
unsigned int distancia_d = 0;
unsigned int Kp = 65536;
unsigned int Ki = 32768;
unsigned int Kd = 16384;
unsigned int QPPS = 800;


//Funcoes Universais
double distancia(int y);



/*
//Rotina Interrupcao Thread -> Controle de Velocidade
void thread_interrompe(void const *argument) 
{
    while (true) 
    {
        
        
        osDelay(0.001);
    }
}
osThreadDef(thread_interrompe, osPriorityNormal, DEFAULT_STACK_SIZE);

//Rotina Interrupcao Thread -> Atualiza vetor de tempo
void vetor_atualiza(void const *argument) 
{
    while (true) 
    {
    
        osDelay(1);
    }
}
osThreadDef(vetor_atualiza, osPriorityNormal, DEFAULT_STACK_SIZE);
          */

//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
int main() 
{
    //char resp = 0, string[40], imprimir = 0;
    //int i = 0;
    //unsigned int aux = 0, dist = 0;
    //float valor1 = 0, valor2 = 0, valor3 = 0, valor4 = 0;
    
    //DIR *d;
    //struct dirent *p;
    
    //Robo.baud(38400);
    //Robo.attach(&callback);
   
    // Comeca Thread
    //osThreadCreate(osThread(thread_interrompe), NULL);
    //osThreadCreate(osThread(vetor_atualiza), NULL);
    
    

    endereco = 128;  
    aceleracao = 0;
    velocidade_e = 150;
    velocidade_d = 150;
    cmd_distancia_e = 140;
    cmd_distancia_d = 140;
    buffer = 0;

    Placa.comando46 (endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_d, cmd_distancia_e, buffer);
}



// Funcoes auxiliares:
double distancia(int y)
{
    double resultado = 0, x = 0;
    
    if(y == 1)
        x = 3.3*sensor1;
    else
        if(y == 2)
            x = 3.3*sensor2;
        else
            if(y == 3)
                x = 3.3*sensor3;
            else
                if(y == 4)
                    x = 3.3*sensor4;
                else
                    x = 0;
    
    // Grafico 2: y = 2,266x6 - 25,396x5 + 114,73x4 - 268,77x3 + 349,89x2 - 251,81x + 92,04
    
    resultado = 2.266*x*x*x*x*x*x;
    resultado = resultado - 25.396*x*x*x*x*x;
    resultado = resultado + 114.73*x*x*x*x;
    resultado = resultado - 268.77*x*x*x;
    resultado = resultado + 349.89*x*x;
    resultado = resultado - 251.81*x;
    resultado = resultado + 92.04;
    
    
    if( (resultado >= 40) || (resultado < 0))
        resultado = -1;
     
    return (resultado);
}