Revision 6:df6b8b2468d8, committed 2021-05-18

Comitter:

xaficz

Date:

Tue May 18 00:04:27 2021 +0000

Parent:

5:25bd866ef068

Child:

7:5fa6f21eb739

Commit message:

gbhuj

Changed in this revision

--- a/Robot.cpp	Mon May 17 15:03:21 2021 +0000
+++ b/Robot.cpp	Tue May 18 00:04:27 2021 +0000
@@ -1,6 +1,15 @@
 #include "Robot.h"
 #include "mbed.h"
 
+
+#include "BufferedSerial.h"
+#include "rplidar.h"
+
+#include "Communication.h"
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
 I2C i2c(I2C_SDA, I2C_SCL );
 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
 
@@ -61,21 +70,3 @@
     countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
     countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
 }
-
-//Funcao para criar um mapa simples com um quadrado no meio
-void read_map(int Map_Matrix[80][80]){
-    for(int i = 0; i < 80; i++){
-        for(int j = 0; j < 80; j++){
-            if(i == 0 || j==0 || i==79 || j==79){
-                Map_Matrix[i][j] = 1;  
-            }     
-            else{
-                Map_Matrix[i][j] = 0;
-                if(i >= 10 && j>=10 && i<=69 && j<=69){
-                    Map_Matrix[i][j] = 1;
-                }
-            }              
-        }
-    } 
-}
-

--- a/Robot.h	Mon May 17 15:03:21 2021 +0000
+++ b/Robot.h	Tue May 18 00:04:27 2021 +0000
@@ -49,6 +49,9 @@
 void VFH(float x, float y, float phi, float x_final, float y_final, float Log_Map[80][80], float Map[80][80], float Limiar_Histograma, float Limiar_Vale, float kv, float ki, float ks, float L, float r, float w_Max);
 
 void read_map(int Map_Matrix[80][80]);
+int distancia_prevista (int x, int y, int x_celula, int y_celula, float Angle, int Map_Matrix[80][80]);
+
+
 int *y_bresenham(int x1, int y1, int x2, int y2);
 int *x_bresenham(int x1, int y1, int x2, int y2);
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/funcs.cpp	Tue May 18 00:04:27 2021 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "rplidar.h"
+#include "Robot.h"
+#include "Communication.h"
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#define pi 3.14159265359
+
+
+    int x_c;
+    int y_c;
+    int next_x;
+    int next_y;
+    float auxx;
+    float auxy;
+    double dist_odometria;
+    
+    
+    //Funcao para criar um mapa simples com um quadrado no meio
+void read_map(int Map_Matrix[80][80]){
+    for(int i = 0; i < 80; i++){
+        for(int j = 0; j < 80; j++){
+            if(i == 0 || j==0 || i==79 || j==79){
+                Map_Matrix[i][j] = 1;  
+            }     
+            else{
+                Map_Matrix[i][j] = 0;
+                if((i >= 2 && j>=2 && i<=10 && j<=10) || (i >=69 && j>=69 && i<=77 && j<=77) || (i >=69 && j>=69 && i<=10 && j<=10) || (i >= 2 && j>=2 && i<=77 && j<=77) || (i >= 2 && j>=2 && i<=10 && j<=10)){
+                    Map_Matrix[i][j] = 1;
+                }
+            }              
+        }
+    } 
+}
+
+    
+int distancia_prevista (int x , int y, int x_celula, int y_celula, float Angle, int Map_Matrix[80][80]){
+    
+    // x_celula --> posição odometria x
+    // y_celula --> posição odometria y
+    // Distance --> Leitura do lidar
+    
+    int find = 0;
+    next_x = x;
+    next_y = y;
+    
+    x_c = x_celula;
+    y_c = y_celula;
+    
+    //:::::::::::: calcular estimativa ::::::::::::::::::
+    while(find == 0)
+     
+        if(Map_Matrix[x_c][y_c] == 1){
+            find = 1;
+            }
+        else
+            next_x = next_x + 5*cos(Angle); // meti ele a andar para a frente de 5 em 5 cm 
+            next_y = next_y + 5*sin(Angle); // isto pode causar algum erro
+        
+            auxx=next_x/5;
+            auxy= next_y/5;
+            x_c = ceil(auxx)-1;
+            y_c = ceil(auxy) - 1;
+        
+        
+        
+          dist_odometria = ((next_x-x)*(next_x-x)) + ((next_y-y)*(next_y-y));
+          dist_odometria = sqrt(dist_odometria);
+    
+    return(dist_odometria);
+    }// fim do while 
+    
+    
+    // usei o next_x_cel com erro de que ele faz de 5 em 5 cm
+    // mas tb posso meter de 1 em 1 ou ate menos
+

--- a/main.cpp	Mon May 17 15:03:21 2021 +0000
+++ b/main.cpp	Tue May 18 00:04:27 2021 +0000
@@ -38,27 +38,40 @@
    // double k = 8;                       //Ganho angular
     
     //Variaveis de posicao
-    double x = 300;                     //x inicial
-    double y = 300;                     //y inicial
+    double x = 60;                     //x inicial cm
+    double y = 60;                     //y inicial
     double phi = 0.01;                  //phi inicial
     
     
+    // Coordenadas
+    int x_Celula;       // Coordenada X Relativa à Celula Ativa (Sistema de Coordenadas baseado em Células de Aresta 5 cm)
+    int y_Celula;       // Coordenada Y Relativa à Celula Ativa (Sistema de Coordenadas baseado em Células de Aresta 5 cm)
+    
+    
+    float Distance;         // Distância Devolvida pela Leitura do LIDAR
+    float Angle;            // Angulo Devolvida pela Leitura do LIDAR
+    
+    
     //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
+    double L = 15.0;                     //distancia entre rodas 
+    double r = 3.5;                      //raio da roda
     
     //Variaveis de comando as rodas
     double V_left = 20;                      //velocidade da roda esquerda    
     double V_right = 25;                     //velocidade da roda direita 
 
     //Auxiliares
-    int ciclos = 0;
+    int Ciclos = 0;
     int Leituras = 0;
+    int AUX = 0;
+    
+    
+    double dist_odometria;
     
     //Funcao de obtencao do Mapa    
     read_map(Map_Matrix);              
@@ -67,16 +80,14 @@
   while(1){
         
         //Este while é para fazer uma paragem de 10 em 10 ciclos para dar tempo para o lidar executar leituras
-        while(ciclos < 10)
+        while(AUX == 1){
         
         //Manda andar 
         setSpeeds(V_left,V_right);
         
-        //conta como mais um ciclo
-        ciclos = ciclos+1;
         
-
     //:::::::::::::: ODOMETRIA :::::::::::::::
+    
         //Obter as contagens dos encoders
         getCountsAndReset();
 
@@ -98,7 +109,67 @@
         
         pc.printf("x_odometria = %f, y_odometria = %f\n", x, y);
         
-    } // fim do while de 10 ciclos
+    
+        //Celula da Odometria
+            x_Celula = ceil(x/5) - 1;
+            y_Celula = ceil(y/5) - 1;
+    
+        pc.printf("x_Celula = %f, y_Celula = %f\n", x_Celula, y_Celula);
+    
+    
+    //============================================
+    
+    
+    
+    
+                    //Distancia suposta
+                    dist_odometria = distancia_prevista(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
+    
+    
+    
+    
+    
+    
+    
+    //==========================================
+        //conta como mais um ciclo
+        Ciclos = Ciclos+1;
+        
+     
+        
+    //::::::::::::::IF de LEITURAS ::::::::::::::::::::::::::: 
+        if(Ciclos == 10){
+            while(Leituras<30) // faz 30 leituras parado
+            
+                if(lidar.pollSensorData(&data)== 0) {
+            
+                    Distance = (data.distance)/10; // aqui este 10 é para cm TENHO QUE VER     
+                    Angle = pi*(data.angle)/180;
+                    Angle = atan2(sin(Angle),cos(Angle));   //em rad
+                    
+                    //Distancia suposta
+                    dist_odometria = distancia_prevista(x, y, x_celula, y_celula, Angle,Map_Matrix[80][80]){ 
+            
+            
+                Leituras = Leituras + 1; // incrementa as leituras
+                }
+                
+                
+        Ciclos = 0;    
+            } // fim do numero de leituras
+            
+            
+        } //fim de if leituras
+    //======================================
+        
+        
+        
+    } // fim do while
+    
+    
+    
+    
+
     
     // reset de ciclos
     //