Diff: main.cpp

Revision:

5:25bd866ef068

Parent:

4:53ac11e9e8b9

Child:

6:df6b8b2468d8

--- a/main.cpp	Mon May 03 15:22:53 2021 +0000
+++ b/main.cpp	Mon May 17 15:03:21 2021 +0000
@@ -8,55 +8,102 @@
 #include <math.h>
 
 #define pi 3.14159265359
-    
-// ****** Inicialização de Variáveis ****** //
+
+    Serial pc(SERIAL_TX, SERIAL_RX);
+    DigitalIn Button_VFH(USER_BUTTON);
+
+    RPLidar lidar;
+    BufferedSerial se_lidar(PA_9, PA_10);
+    PwmOut rplidar_motor(D3);
+
+struct RPLidarMeasurement data;
 
-// Carateristicas da Plataforma
-float r = 3.5;
-float L = 15;
+int main() {
     
-// Velocidade Máxima das Rodas
-float w_Max = 150;
+    pc.baud(115200);
+    init_communication(&pc);
+
+    rplidar_motor.period(0.01f);
+    //rplidar_motor.write(1.0f);
+    lidar.begin(se_lidar);
+    lidar.setAngle(0,360);
+    lidar.startThreadScan();
+    rplidar_motor.write(0.7f);
+    
     
-// 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;
+    //Matrizes de mapeamento
+    int Map_Matrix[80][80];             //Matriz do mapa
+    
+    //Ganhos
+   // double k = 8;                       //Ganho angular
+    
+    //Variaveis de posicao
+    double x = 300;                     //x inicial
+    double y = 300;                     //y inicial
+    double phi = 0.01;                  //phi inicial
+    
     
-// Ganhos de Controlo
-float kv = 9;
-float ki = 0.15;
-float ks = 18;
+    //Variaveis de estimacao de posicao
+    double var_l;                       //contagens do encoder esquerdas
+    double var_r;                       //contagens do encoder direitas
+    double var_d;                       //variacao linear entre iteracoes    
+    double var_phi;                     //variacao angular entre iteracoes
+
+    double L = 150;                     //distancia entre rodas 
+    double r = 35;                      //raio da roda
     
-// Alpha e Limiares
-float Limiar_Histograma = 3;
-float Limiar_Vale = 0.5;    
+    //Variaveis de comando as rodas
+    double V_left = 20;                      //velocidade da roda esquerda    
+    double V_right = 25;                     //velocidade da roda direita 
 
-// Matrizes de Mapeamento
-float Map[80][80];
-float Log_Map[80][80];
-
-int main(){
+    //Auxiliares
+    int ciclos = 0;
+    int Leituras = 0;
+    
+    //Funcao de obtencao do Mapa    
+    read_map(Map_Matrix);              
+    
     
-    //:::::::::::::::::Inicializaçao dos Mapas a zero:::::::::::: 
-    
-    for(int i = 0; i < 80; i++){
-        for(int j = 0; j < 80; j++){
-                Map[i][j] = 0; 
+  while(1){
+        
+        //Este while é para fazer uma paragem de 10 em 10 ciclos para dar tempo para o lidar executar leituras
+        while(ciclos < 10)
+        
+        //Manda andar 
+        setSpeeds(V_left,V_right);
+        
+        //conta como mais um ciclo
+        ciclos = ciclos+1;
+        
+
+    //:::::::::::::: ODOMETRIA :::::::::::::::
+        //Obter as contagens dos encoders
+        getCountsAndReset();
+
+        //Estimacao de pose
+        var_l = (2*pi*r*countsLeft/1440);
+        var_r = (2*pi*r*countsRight/1440);
+        var_d = (var_l+var_r)/2;
+        var_phi= (var_r-var_l)/L;
+
+        if(var_phi == 0){
+            x = x + var_d*cos(phi);
+            y = y + var_d*sin(phi);
+        } 
+        else{
+            x = x + (var_d)*((sin(phi/2)/(phi/2))*cos(phi + var_phi/2));
+            y = y + (var_d)*((sin(phi/2)/(phi/2))*sin(phi + var_phi/2));
+            phi = phi + var_phi;
         }
-    }     
+        
+        pc.printf("x_odometria = %f, y_odometria = %f\n", x, y);
+        
+    } // fim do while de 10 ciclos
     
-    for(int i = 0; i < 80; i++){
-        for(int j = 0; j < 80; j++){
-                Log_Map[i][j] = 0;  
-        }
-    }     
-            
+    // reset de ciclos
+    // 
 
-    //:::::::::::::::::::::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
+    return(0);
+}      
+