Revision 7:5fa6f21eb739, committed 2021-05-24

Comitter:

xaficz

Date:

Mon May 24 15:32:13 2021 +0000

Parent:

6:df6b8b2468d8

Commit message:

4__zenaga

Changed in this revision

--- a/Robot.h	Tue May 18 00:04:27 2021 +0000
+++ b/Robot.h	Mon May 24 15:32:13 2021 +0000
@@ -46,11 +46,13 @@
  *  not have to worry about the count overflowing. */
 void getCountsAndReset();
 
-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 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 distancia_prevista (int x, int y, int x_celula, int y_celula, float Angle, int Map_Matrix[80][80]);
+
+//int DistanciaPrevBresh (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);

--- a/funcs.cpp	Tue May 18 00:04:27 2021 +0000
+++ b/funcs.cpp	Mon May 24 15:32:13 2021 +0000
@@ -10,14 +10,18 @@
 #define pi 3.14159265359
 
 
+    int *x_cel;
+    int *y_cel;
     int x_c;
     int y_c;
+    
     int next_x;
     int next_y;
     float auxx;
     float auxy;
-    double dist_odometria;
-    
+    int dist_odometria;
+    int x_end;
+    int y_end;
     
     //Funcao para criar um mapa simples com um quadrado no meio
 void read_map(int Map_Matrix[80][80]){
@@ -36,8 +40,8 @@
     } 
 }
 
-    
-int distancia_prevista (int x , int y, int x_celula, int y_celula, float Angle, int Map_Matrix[80][80]){
+/*    
+void distancia_prevista (int x , int y, int x_celula, int y_celula, float Angle, int Map_Matrix[80][80],float dist_odometria){
     
     // x_celula --> posição odometria x
     // y_celula --> posição odometria y
@@ -66,14 +70,51 @@
             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);
+    //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
 
+*/
+/*
+int DistanciaPrevBresh(int x, int y, int x_celula, int y_celula, float angle, int Map_Matrix[80][80] ){
+    
+    if(angle < pi/2 && angle > 3*pi/2) 
+    x_end = 80;
+    else
+    x_end = 0;
+    
+    if(angle < 0 && angle < pi) 
+    y_end = 80;
+    else
+    y_end = 0;
+    
+    
+int find = 0;
+    
+    x_cel = x_bresenham(x_celula, y_celula, x_end, y_end);
+    y_cel = y_bresenham(x_celula, y_celula, x_end, y_end);
+    
+    
+    while(find == 0){
+        int i = 0;
+        if(Map_Matrix[x_cel[i]][y_cel[i]] == 1){
+            find = 1;
+            }
+        else{
+           i=i+1;
+            }
+    }
+        
+    dist_odometria = ((x_c-x_celula)*(x_c-x_celula)) + ((y_c-y_celula)*(y_c-y_celula));
+    dist_odometria = sqrt(dist_odometria);
+              
+    return(dist_odometria);
+    }
+    
+*/
\ No newline at end of file

--- a/main.cpp	Tue May 18 00:04:27 2021 +0000
+++ b/main.cpp	Mon May 24 15:32:13 2021 +0000
@@ -10,27 +10,20 @@
 #define pi 3.14159265359
 
     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;
-
-int main() {
     
-    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);
+    //DigitalIn Button_VFH(USER_BUTTON);
     
     
+//=======================================
+        //Para o matlab
+        
+    //init_communication(Serial *serial_in)
+        
+//=======================================
+
     //Matrizes de mapeamento
     int Map_Matrix[80][80];             //Matriz do mapa
     
@@ -62,25 +55,49 @@
     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 
+    double V_left = 45;                      //velocidade da roda esquerda    
+    double V_right = 52;                     //velocidade da roda direita 
 
+    
+    
+    
+int main() {
+    
+    struct RPLidarMeasurement data;
+    
+    
+    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);
+    
     //Auxiliares
     int Ciclos = 0;
     int Leituras = 0;
-    int AUX = 0;
+    int AUX = 1;
+    int i = 0;
+    int fin = 0;
+    int x_end;
+    int y_end;
+    int *x_cel;
+    int *y_cel;
+    float auxx;
+    float auxy;
     
-    
-    double dist_odometria;
+    int dist_odometria;
     
     //Funcao de obtencao do Mapa    
     read_map(Map_Matrix);              
     
-    
   while(1){
         
         //Este while é para fazer uma paragem de 10 em 10 ciclos para dar tempo para o lidar executar leituras
-        while(AUX == 1){
+        //while(ciclos< 10){
         
         //Manda andar 
         setSpeeds(V_left,V_right);
@@ -97,15 +114,11 @@
         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));
+
+            x = x + (var_d)*cos(phi + var_phi/2);
+            y = y + (var_d)*sin(phi + var_phi/2);
             phi = phi + var_phi;
-        }
+    
         
         pc.printf("x_odometria = %f, y_odometria = %f\n", x, y);
         
@@ -114,23 +127,63 @@
             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);
+        pc.printf("x_Celula = %d, y_Celula = %d\n", x_Celula, y_Celula);
     
+    //===========================================
+        //Para o matlab
+        
+        //send_odometry(value1,value2,ticks_left,ticks_right,x,y,theta)
     
     //============================================
     
-    
-    
+        if(lidar.pollSensorData(&data)== 0) {
+            
+                    //Distancia Lida
+                    Distance = (data.distance)/10; // aqui este 10 é para cm TENHO QUE VER     
+                    
+                    
+                    //Angulo da leitura
+                    Angle = pi*(data.angle)/180;
+                    Angle = atan2(sin(Angle),cos(Angle));   //em rad
+                    
+            pc.printf("Distancia = %f,-----------, Angle= %f\n", Distance,Angle);
     
                     //Distancia suposta
-                    dist_odometria = distancia_prevista(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
+                   // dist_odometria = distancia_prevista(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
+                   // dist_odometria = DistanciaPrevBresh(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
+                   
+                   
+    if(Angle < pi/2 && Angle > 3*pi/2) {
+    x_end = 80;}
+    else{
+    x_end = 0;}
     
+    if(Angle < 0 && Angle < pi) {
+    y_end = 80;}
+    else{
+    y_end = 0;}
+    
+    x_cel = x_bresenham(x_Celula, y_Celula, x_end, y_end);
+    y_cel = y_bresenham(x_Celula, y_Celula, x_end, y_end);
     
     
-    
+    while(fin == 0){
+        if(Map_Matrix[x_cel[i]][y_cel[i]] == 1){
+            fin = 1;
+            }
+        else{
+           i=i+1;
+            }
+    }
     
+    auxx = ((x_cel[i]-x_Celula)*(x_cel[i]-x_Celula));
+    auxy = ((y_cel[i]-y_Celula)*(y_cel[i]-y_Celula));
+    dist_odometria = sqrt( auxx + auxy );
     
+    pc.printf("DistanciaPREVISTA = %f\n", dist_odometria);
     
+ }   
+                    
     //==========================================
         //conta como mais um ciclo
         Ciclos = Ciclos+1;
@@ -148,13 +201,13 @@
                     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]){ 
+                   // dist_odometria = distancia_prevista(x, y, x_celula, y_celula, Angle,Map_Matrix[80][80]){ 
             
             
                 Leituras = Leituras + 1; // incrementa as leituras
                 }
                 
-                
+        // reset de ciclos       
         Ciclos = 0;    
             } // fim do numero de leituras
             
@@ -163,18 +216,11 @@
     //======================================
         
         
-        
+         return(0);
     } // fim do while
     
-    
-    
-    
-
+    //return(0);
     
-    // reset de ciclos
-    // 
+//}      
 
-    
-    return(0);
-}      
-        
+