File content as of revision 3:10da2a723362:

#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 40.50 //mm
#define diametro_esquerda 40.50 //mm
#define pi 3.1415926535897 
#define velocidade_max 220.00 //cm/seg
#define dist_minima 7.0 //cm
 
TextLCD lcd(p5, p6, p7, p8, p29, p30, TextLCD::LCD20x4); // rs, e, d4-d7
RoboClaw Placa;

MSCFileSystem  sd("sd");

Timer tempo;

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

// Variavel de leitura de distancia do sensor Sharp
float valor1 = 0, valor2 = 0, valor3 = 0, valor4 = 0;

// Leitura de dados Pendrive
float read_tempo = 0;

//Declaracao de variaveis universal RoboClaw:
char endereco = 128;
char valor = 0;
char check8 = 0;    
char buffer = 0;  
char status_encoder_e = 0; 
char status_encoder_d = 0;  
unsigned int aceleracao = 0;
int velocidade_e = 0;
int velocidade_d = 0;
unsigned int cmd_distancia_e = 0;
unsigned int cmd_distancia_d = 0;
int distancia_e = 0;
int distancia_d = 0;
int temp_distancia_e = 0;
int temp_distancia_d = 0;
unsigned int Kp = 65536;
unsigned int Ki = 32768;
unsigned int Kd = 16384;
unsigned int QPPS = 800;
unsigned long long resposta_encoder = 0;
unsigned short le_buffer = 0;
char buffer_M1 = 0, buffer_M2 = 0;
char usando_na_thread = 0;
char usando_na_main = 1;


/*
//Rotina Interrupcao Thread -> Controle de Velocidade
void thread_interrompe(void const *argument) 
{
    char parar = 0;
    
    while (true) 
    {
        valor1 = Placa.sharp(1);
        valor2 = Placa.sharp(2);
        valor3 = Placa.sharp(3);
        valor4 = Placa.sharp(4);

        if(((valor1 <= dist_minima) && (valor1 >=0)) || ((valor2 <= dist_minima) && (valor2 >=0)) || ((valor3 <= dist_minima) && (valor3 >=0)) || ((valor4 <= dist_minima) && (valor4 >=0)))
        {
            endereco = 128;
            parar = 0;
            Placa.comando8(endereco, parar);
            lcd.cls();
            lcd.printf("1/%4.2f 2/%4.2f\n3/%4.2f 4/%4.2f" , valor1, valor2, valor3, valor4);
            wait(2);
        }      
        osDelay(0.1);
    }
}
osThreadDef(thread_interrompe, osPriorityNormal, DEFAULT_STACK_SIZE);   */ 

/*
//Rotina Interrupcao Thread -> Atualiza vetor de tempo
void vetor_atualiza(void const *argument) 
{
    while (true) 
    {
        if(usando_na_main != 1)
        {
            usando_na_thread = 1;
            endereco = 128;
            le_buffer = Placa.comando47(endereco);
            buffer_M1 = ((le_buffer>>8) & 0xFF);
            buffer_M2 = (le_buffer & 0xFF);
            usando_na_thread = 0;
        }                
        osDelay(0.1);
    }
}
osThreadDef(vetor_atualiza, osPriorityNormal, DEFAULT_STACK_SIZE);*/


//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////
int main() 
{ 

    FILE *fp = fopen("/sd/dad.txt","w");
    if ( fp == NULL )
    {
        error("Could not open file for write\n");
        led2 = 1;
    }
   
    usando_na_main = 1;
    // Comeca Thread
    //osThreadCreate(osThread(thread_interrompe), NULL);
    //osThreadCreate(osThread(vetor_atualiza), NULL);
    
    wait(1);
                                
    //Minimo volts main battery: 6V
    endereco = 128;
    valor = 0;
    Placa.comando2(endereco, valor);

    //Máximo volts main battery: 9V
    endereco = 128;
    valor = 47;
    Placa.comando3(endereco, valor); 
    
    // Zera o encoder
    endereco = 128;
    Placa.comando20(endereco);
    
    // Configura o motor M1 = Direita
    endereco = 128;
    Kp = 1000000;       
    Ki = 500000;
    Kd = 250000;
    QPPS = 880;  // 840+ 10%
    Placa.comando28(endereco, Kd, Kp, Ki,QPPS);

    // Configura o motor M2 = Esquerda
    endereco = 128;
    Kp = 1000000;       
    Ki = 500000;
    Kd = 250000;
    QPPS = 880;  // 840+ 10%
    Placa.comando29(endereco, Kd, Kp, Ki,QPPS); 


    endereco = 128;  
    aceleracao = 50;
    velocidade_e = 50;
    velocidade_d = 50;
    cmd_distancia_e = 179;
    cmd_distancia_d = 179;
    buffer = 1;
    //Placa.comando37(endereco, velocidade_d, velocidade_e);
    Placa.comando46(endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_e, cmd_distancia_e, buffer);

    endereco = 128;  
    aceleracao = 125;
    velocidade_e = 0;
    velocidade_d = 0;
    cmd_distancia_e = 10;
    cmd_distancia_d = 10;
    buffer = 0;
    Placa.comando46(endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_e, cmd_distancia_e, buffer);

    // Monitoramento no Pendrive
    endereco = 128;
    resposta_encoder = Placa.comando18(endereco);
    velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
    resposta_encoder = Placa.comando19(endereco);
    velocidade_e = int (resposta_encoder & 0xFFFFFFFF);


    while((velocidade_d==0) || (velocidade_e==0))
    {
        resposta_encoder = Placa.comando18(endereco);
        velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_d = char ((resposta_encoder>>32) & 0xFF);

        resposta_encoder = Placa.comando19(endereco);
        velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
    }
    
    tempo.start();
    
    endereco = 128;
    resposta_encoder = Placa.comando18(endereco);
    velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
    resposta_encoder = Placa.comando19(endereco);
    velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
    
    while((velocidade_d!=0) || (velocidade_e!=0))
    {
        led4 != led4;
        endereco = 128;
        resposta_encoder = Placa.comando16(endereco);
        distancia_d = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
    
        endereco = 128;
        resposta_encoder = Placa.comando17(endereco);
        distancia_e = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
            
        resposta_encoder = Placa.comando18(endereco);
        velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_d = char ((resposta_encoder>>32) & 0xFF);

        resposta_encoder = Placa.comando19(endereco);
        velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
        
        read_tempo = tempo.read_ms();
        
        fprintf(fp, "%f\t" , read_tempo);
        fprintf(fp, "%d\t" , velocidade_e);
        fprintf(fp, "%d\t\t" , distancia_e);
        
        fprintf(fp, "%f\t" , read_tempo);
        fprintf(fp, "%d\t" , velocidade_d);
        fprintf(fp, "%d-" , distancia_d);
        
        wait_us(800);   
    }
        
    fclose(fp);
    
    led4 = 1;


    /*    
    while(1)
    {
        valor1 = Placa.sharp(1);
        valor2 = Placa.sharp(2);
        valor3 = Placa.sharp(3);
        valor4 = Placa.sharp(4);

        led3 = !led3;
                
        lcd.cls();
        lcd.printf("1/%4.2f 2/%4.2f\n3/%4.2f 4/%4.2f" , valor1, valor2, valor3, valor4);
        wait(0.1);

        
        endereco = 128;
        resposta_encoder = Placa.comando16(endereco);
        distancia_d = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_d = char ((resposta_encoder>>32) & 0xFF);
    
        endereco = 128;
        resposta_encoder = Placa.comando17(endereco);
        distancia_e = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
            
        resposta_encoder = Placa.comando18(endereco);
        velocidade_d = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_d = char ((resposta_encoder>>32) & 0xFF);

        resposta_encoder = Placa.comando19(endereco);
        velocidade_e = int (resposta_encoder & 0xFFFFFFFF);
        status_encoder_e = char ((resposta_encoder>>32) & 0xFF);
    
        lcd.cls(); 
        lcd.printf("DD:%i*DE:%i" , distancia_d, distancia_e);
        lcd.printf("\nVD:%i*VE:%i" , velocidade_d, velocidade_e);    
        wait(0.2); 
    } 
    */          
}