Program version 1
Dependencies: FatFileSystemSD MSCFileSystem TextLCD mbed-rtos mbed
Fork of USBFlashDiskTest by
Diff: main.cpp
- Revision:
- 3:10da2a723362
- Parent:
- 2:f1f6b33cd7bd
--- a/main.cpp Sun Sep 16 15:13:32 2012 +0000 +++ b/main.cpp Wed Nov 21 17:20:27 2012 +0000 @@ -5,147 +5,277 @@ #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 diametro_direita 40.50 //mm +#define diametro_esquerda 40.50 //mm +#define pi 3.1415926535897 #define velocidade_max 220.00 //cm/seg -#define tam_vetor 64 +#define dist_minima 7.0 //cm TextLCD lcd(p5, p6, p7, p8, p29, p30, TextLCD::LCD20x4); // rs, e, d4-d7 -RoboClaw Placa(void); +RoboClaw Placa; + MSCFileSystem sd("sd"); - -//Serial Robo(p28, p27); // tx, rx - -AnalogIn sensor1(p15); -AnalogIn sensor2(p16); -AnalogIn sensor3(p17); -AnalogIn sensor4(p19); +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 comando = 0; 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 = 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; +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; - - -//Funcoes Universais -double distancia(int y); - +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) { - - - osDelay(0.001); + 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); +osThreadDef(thread_interrompe, osPriorityNormal, DEFAULT_STACK_SIZE); */ +/* //Rotina Interrupcao Thread -> Atualiza vetor de tempo void vetor_atualiza(void const *argument) { while (true) { - - osDelay(1); + 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); - */ +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); +{ + + 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 = 0; - velocidade_e = 150; - velocidade_d = 150; - cmd_distancia_e = 140; - cmd_distancia_d = 140; + 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); - Placa.comando46 (endereco, aceleracao, velocidade_d, cmd_distancia_d, velocidade_d, 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); -// Funcoes auxiliares: -double distancia(int y) -{ - double resultado = 0, x = 0; + 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); - 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; + 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); - // Grafico 2: y = 2,266x6 - 25,396x5 + 114,73x4 - 268,77x3 + 349,89x2 - 251,81x + 92,04 + 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); - 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; + 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); - - if( (resultado >= 40) || (resultado < 0)) - resultado = -1; - - return (resultado); + 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); + } + */ }