Diff: main.cpp

Revision:

3:10da2a723362

Parent:

2:f1f6b33cd7bd

diff -r f1f6b33cd7bd -r 10da2a723362 main.cpp
--- 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); 
+    } 
+    */          
 }