Diff: main.cpp

Revision:

4:53ac11e9e8b9

Parent:

3:0a718d139ed1

Child:

5:25bd866ef068

diff -r 0a718d139ed1 -r 53ac11e9e8b9 main.cpp
--- a/main.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/main.cpp	Mon May 03 15:22:53 2021 +0000
@@ -1,38 +1,62 @@
-// Coded by Luís Afonso 11-04-2019
 #include "mbed.h"
 #include "BufferedSerial.h"
 #include "rplidar.h"
 #include "Robot.h"
 #include "Communication.h"
-
-Serial pc(SERIAL_TX, SERIAL_RX);
-RPLidar lidar;
-BufferedSerial se_lidar(PA_9, PA_10);
-PwmOut rplidar_motor(D3);
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
 
-int main()
-{
-    float odomX, odomY, odomTheta;
-    struct RPLidarMeasurement data;
+#define pi 3.14159265359
     
-    pc.baud(115200);
-    init_communication(&pc);
+// ****** Inicialização de Variáveis ****** //
 
-    // Lidar initialization
-    rplidar_motor.period(0.001f);
-    lidar.begin(se_lidar);
-    lidar.setAngle(0,360);
-
-    pc.printf("Program started.\n");
-        
-    lidar.startThreadScan();
+// Carateristicas da Plataforma
+float r = 3.5;
+float L = 15;
+    
+// Velocidade Máxima das Rodas
+float w_Max = 150;
+    
+// Posição Inicial e Final
+float x_Inicial = 15;
+float y_Inicial = 15;
+float phi_Inicial = 0;
+float x_Final = 100;
+float y_Final = 75;
+    
+// Ganhos de Controlo
+float kv = 9;
+float ki = 0.15;
+float ks = 18;
     
-    while(1) {
-        // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
-        if(lidar.pollSensorData(&data) == 0)
-        {
-            pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+// Alpha e Limiares
+float Limiar_Histograma = 3;
+float Limiar_Vale = 0.5;    
+
+// Matrizes de Mapeamento
+float Map[80][80];
+float Log_Map[80][80];
+
+int main(){
+    
+    //:::::::::::::::::Inicializaçao dos Mapas a zero:::::::::::: 
+    
+    for(int i = 0; i < 80; i++){
+        for(int j = 0; j < 80; j++){
+                Map[i][j] = 0; 
         }
-       wait(0.01); 
-    }
+    }     
+    
+    for(int i = 0; i < 80; i++){
+        for(int j = 0; j < 80; j++){
+                Log_Map[i][j] = 0;  
+        }
+    }     
+            
+
+    //:::::::::::::::::::::Função VFH::::::::::::::::::::..
+    
+    VFH(x_Inicial, y_Inicial, phi_Inicial, x_Final, y_Final, Log_Map, Map, Limiar_Histograma, Limiar_Vale, kv, ki, ks, L, r, w_Max);
+
 }
\ No newline at end of file