Diff: main.cpp

Revision:

2:f1f6b33cd7bd

Parent:

0:22a5db2c7926

Child:

3:10da2a723362

diff -r 1f7a5d9bf5f7 -r f1f6b33cd7bd main.cpp
--- a/main.cpp	Mon Jul 23 09:59:38 2012 +0000
+++ b/main.cpp	Sun Sep 16 15:13:32 2012 +0000
@@ -1,39 +1,151 @@
 #include "mbed.h"
+#include "RoboClaw.h"
+#include "TextLCD.h"
 #include "MSCFileSystem.h"
+#include "cmsis_os.h"
 
-MSCFileSystem sd("sd");
+#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);
 
-int main() {
-
-    printf("This is version 1.0 of the USB host test\n");
-
-    FILE *fp = fopen("/sd/test.txt","w");
-    printf("Create filehandle\n");
-
-    led1= 1;
 
-    if (fp == NULL) {                 //make sure it's been opened
-        printf("its bad :-(\n");
-        led2 = 1;
-        exit(1);
-    }
-    printf("its good :-)\n");
+//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;
 
-    led3 = 1;
 
-    fprintf(fp,"Hello world!\n");
-    printf("Write to the file\n");
+//Funcoes Universais
+double distancia(int y);
+
 
 
-    fclose(fp);
-    printf("Close the handle\n");
+/*
+//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);
+    
+    
 
-    led4 = 1;
-    printf("\n");
+    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);
+}