Revision 4:213afe3d5c4b, committed 2021-05-03

Comitter:

xaficz

Date:

Mon May 03 15:45:29 2021 +0000

Parent:

3:0a718d139ed1

Commit message:

3_VFH

Changed in this revision

--- a/Robot.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/Robot.cpp	Mon May 03 15:45:29 2021 +0000
@@ -60,4 +60,6 @@
     i2c.read( addr8bit, read_buffer, 4);
     countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
     countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
-}
\ No newline at end of file
+}
+
+    

--- a/Robot.h	Wed Apr 24 16:00:07 2019 +0000
+++ b/Robot.h	Mon May 03 15:45:29 2021 +0000
@@ -46,4 +46,8 @@
  *  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);
+
+int *y_bresenham(int x1, int y1, int x2, int y2);
+int *x_bresenham(int x1, int y1, int x2, int y2);
 #endif /* ROBOT_H_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/funcs.cpp	Mon May 03 15:45:29 2021 +0000
@@ -0,0 +1,229 @@
+#include "mbed.h"
+#include "math.h"
+#include <stdio.h>
+#include "funcs.h"
+
+static const double pi = 3.14159265358;
+
+Serial pc1(SERIAL_TX, SERIAL_RX);
+
+
+//Funcao para obter as matrizes de Seccoes e Amplitudes
+void get_Sector_Matrix(int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15]){
+    double division_angle = 2*pi/24;
+    double Aux_Angle_Matrix[15][15];
+    double a = sqrt((double)2)*7;
+    double b = 1;
+    int i;
+    
+    //Atribui angulos aos varios pontos da matriz
+    for(int x = 0; x < 15; x++){
+        for(int y = 0; y < 15; y++){
+            Aux_Angle_Matrix[x][y] = atan2((double)(y-7),(double)(x-7));
+        }
+    }
+    
+    //Colocar a matriz de seccoes toda a 0
+    for(int x = 0; x < 15; x++){
+        for(int y = 0; y < 15; y++){
+            Sector_Matrix_Sections[x][y] = 0;
+        }
+    }    
+    
+    //Atribuir o indice da seccao em funcao dos angulos obtidos anteriormente
+    for(int x = 0; x < 15; x++){
+        for(int y = 0; y < 15; y++){
+            i = 0;
+            for(double ca = division_angle-pi; ca < pi; ca = ca+division_angle){ 
+                if(Sector_Matrix_Sections[x][y] == 0 && (Aux_Angle_Matrix[x][y] <= ca || Aux_Angle_Matrix[x][y] == pi) && Aux_Angle_Matrix[x][y] >= ca-division_angle){
+                    Sector_Matrix_Sections[x][y] = i;      
+                }
+                i = i+1;
+            }
+        }
+    }
+    
+    //Atribuir a amplitude a cada elemento da matriz
+    for(int x = 0; x < 15; x++){
+        for(int y = 0; y < 15; y++){
+            Sector_Matrix_Abs[x][y] = a-b*sqrt((double)((7-y)*(7-y) + (7-x)*(7-x)));
+        }
+    }
+}
+
+
+//Funcao para obter a matriz das redondezas do robo 
+void get_Surroundings_Matrix(int Surroundings_Matrix[15][15], double actual_position_x, double actual_position_y, int Map_Matrix[80][80]){
+    int map_position_x = floor(actual_position_x/50);
+    int map_position_y = floor(actual_position_y/50);
+    
+    int i = 0;
+    int j = 0;
+    for(int x = map_position_x-8; x < map_position_x+8; x++){
+        for(int y = map_position_y-8; y < map_position_y+8; y++){
+            if(x < 0 || x > 79 || y < 0 || y > 79){
+                Surroundings_Matrix[i][j] = 1;
+            }    
+            else{
+                Surroundings_Matrix[i][j] = Map_Matrix[x][y];
+            }
+            j = j + 1;
+        }
+        i = i + 1;
+        j = 0;
+    }
+}
+
+//Funcao auxiliar de get_orientation_angle para encontrar os indices do histograma
+void find_index(int given_index, int l, int out_index[9]){
+    int n = 0;
+    for(int i = given_index-l; i <= given_index+l; i++){
+        if(i < 0){
+            out_index[n] = 22 + i;
+            n++;
+        }
+        else{   
+            if(i > 22){
+                out_index[n] = i - 23;
+                n++;
+            }
+            else{
+                out_index[n] = i;
+                n++;
+            }    
+        }
+    }
+}
+
+//Funcao para obter o angulo de orientacao seguinte 
+double get_orientation_angle(double actual_position_x, double actual_position_y, double end_position_x, double end_position_y, int Surroundings_Matrix[15][15], int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15], double threshold){
+    double Sector_Sum_Vector[23] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
+    double Sector_Sum_Vector_Filtered[23];
+    double auxiliar_angle[23];
+    double angle_difference_end[23];
+    double section_angle = 2*pi/23;
+    double end_angle;
+    int index = 0;
+    int l = 4;
+    int out_index[(2*l)+1];
+    int ind;
+    
+    double Surroundings_Matrix_Aux[15][15];
+    
+    //Atribuimos os valores de amplitudes aos pontos da matriz das redondezas
+    for(int x = 0; x < 15; x++){
+        for(int y = 0; y < 15; y++){
+            Surroundings_Matrix_Aux[x][y] = Surroundings_Matrix[x][y]*Sector_Matrix_Abs[x][y];
+        }
+    }    
+
+    //Criamos o vetor do histograma das seccoes
+    for(int x = 0; x < 15; x++){  
+        for(int y = 0; y < 15; y++){   
+            index = Sector_Matrix_Sections[x][y];
+            Sector_Sum_Vector[index] = Sector_Sum_Vector[index] + Surroundings_Matrix_Aux[x][y];
+        }
+    } 
+    
+    //Filtramos o histograma
+    for(int i = 0; i < 23; i++){  
+        find_index(i,l,out_index);
+        
+        for(int c = 0; c < l; c++){
+            ind = out_index[c];
+            Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (c+1)*Sector_Sum_Vector[ind];
+        }
+        
+        Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (l+1)*Sector_Sum_Vector[l];
+
+        for(int c = 0; c < l; c++){
+            ind = out_index[c+l+1];
+            Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i] + (l-c)*Sector_Sum_Vector[ind];
+        }
+            
+        Sector_Sum_Vector_Filtered[i] = Sector_Sum_Vector_Filtered[i]/(2*l+1);
+    }
+
+    
+    for(int i = 0; i < 23; i++){
+        if(Sector_Sum_Vector_Filtered[i] < threshold){
+            auxiliar_angle[i] = i*section_angle - pi;
+        }
+        else{
+            auxiliar_angle[i] = 999;
+        }    
+    }
+    
+    end_angle = atan2((double)(end_position_y-actual_position_y),(double)(end_position_x-actual_position_x));
+    
+    //Escolhemos a secccao disponivel cujo angulo e mais proximo do angulo final
+    for(int i = 0; i < 23; i++){
+        angle_difference_end[i] = sqrt((double)(auxiliar_angle[i] - end_angle)*(auxiliar_angle[i] - end_angle));  
+    }    
+    
+    ind = 0;
+    for(int i = 0; i < 23; i++){
+        if(angle_difference_end[i] < angle_difference_end[ind]){
+            ind = i;
+        }      
+    } 
+    
+    if(auxiliar_angle[ind] == 999) auxiliar_angle[ind] = 0;  // caso de erro andar em frente
+          
+    return(auxiliar_angle[ind]);
+}
+
+void update_map(double Log_Map_Matrix[80][80], int Aux_Matrix[80][80], int Map_Matrix[80][80], double distance, double theta, double actual_position_x, double actual_position_y){
+    double inc = 1;
+    int x, y;
+    actual_position_x = floor(actual_position_x/50); // saber a celula
+    actual_position_y = floor(actual_position_y/50);
+    double ang = theta;
+        for(double r = inc; r < (distance-inc); r += inc){
+            x = actual_position_x+floor(r*cos(ang));
+            y = actual_position_y+floor(r*sin(ang));
+            if(x >= 0 && y >= 0 && x < 80 && y < 80){
+                if(Aux_Matrix[x][y] != 1){
+                    Log_Map_Matrix[x][y] = Log_Map_Matrix[x][y] - 0.65;    
+                    Aux_Matrix[x][y] = 1;
+                    if((1-(1/(1+exp(Log_Map_Matrix[x][y])))) > 0.5){
+                        Map_Matrix[x][y] = 1;
+                    }
+                    else{
+                        Map_Matrix[y][x] = 0;
+                    }
+                }
+            }    
+        }
+        
+        x = actual_position_x+floor(distance*cos(ang));
+        y = actual_position_y+floor(distance*sin(ang));
+        if(x >= 0 && y >= 0 && x < 80 && y < 80){
+            if(Aux_Matrix[x][y] != 1){
+                Log_Map_Matrix[x][y] = Log_Map_Matrix[x][y] + 0.65;
+                Aux_Matrix[x][y] = 1;
+                if((1-(1/(1+exp(Log_Map_Matrix[x][y])))) > 0.5){
+                    Map_Matrix[x][y] = 1;
+                }
+                else{
+                    Map_Matrix[y][x] = 0;
+                }
+            }
+        }    
+    
+    //for(double ang=(theta-alpha); ang <= (theta+alpha); ang += alpha){
+        for(double r = 0; r < (distance-inc); r += inc){
+            x = actual_position_x+floor(r*cos(ang));
+            y = actual_position_y+floor(r*sin(ang));
+            if(x >= 0 && y >= 0 && x < 80 && y < 80){    
+                Aux_Matrix[x][y] = 0;
+            }    
+        }
+        
+        x = actual_position_x+floor(distance*cos(theta));
+        y = actual_position_y+floor(distance*sin(theta));
+        if(x >= 0 && y >= 0 && x < 80 && y < 80){
+            Aux_Matrix[x][y] = 0;
+        }   
+    }
+    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/funcs.h	Mon May 03 15:45:29 2021 +0000
@@ -0,0 +1,10 @@
+
+void get_Sector_Matrix(int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15]);
+
+void get_Surroundings_Matrix(int Surroundings_Matrix[15][15], double actual_position_x, double actual_position_y, int Map_Matrix[80][80]);
+
+void find_index(int given_index, int l, int out_index[23]);
+
+double get_orientation_angle(double actual_position_x, double actual_position_y, double end_position_x, double end_position_y, int Surroundings_Matrix[15][15], int Sector_Matrix_Sections[15][15], double Sector_Matrix_Abs[15][15], double threshold);
+
+void update_map(double Log_Map_Matrix[80][80], int Aux_Matrix[80][80], int Map_Matrix[80][80], double distance, double theta, double actual_position_x, double actual_position_y);
\ No newline at end of file

--- a/main.cpp	Wed Apr 24 16:00:07 2019 +0000
+++ b/main.cpp	Mon May 03 15:45:29 2021 +0000
@@ -1,38 +1,145 @@
-// Coded by Luís Afonso 11-04-2019
+
 #include "mbed.h"
 #include "BufferedSerial.h"
 #include "rplidar.h"
 #include "Robot.h"
 #include "Communication.h"
+#include "funcs.h"
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 RPLidar lidar;
 BufferedSerial se_lidar(PA_9, PA_10);
 PwmOut rplidar_motor(D3);
+DigitalIn Button(USER_BUTTON);
 
+static const double pi = 3.14159265358;
+
+
+//Matrizes de mapeamento
+int Map_Matrix[80][80] = {{0}};     //Matriz do mapa
+double Log_Map_Matrix[80][80] = {{0}}; //Matriz de Logs
+int Aux_Matrix[80][80] = {{0}};
+int Sector_Matrix_Sections[15][15]; //Matriz com os indices das seccoes
+double Sector_Matrix_Abs[15][15];   //Matriz com as amplitudes 
+int Surroundings_Matrix[15][15];    //Matriz das redondezas do bot
+double angle;    
+    
+//Ganhos
+double k = 12;                       //ganho angular
+    
+//Variaveis de posicao
+double x = 100;                     //x inicial
+double y = 2000;                     //y inicial
+double phi = 0.01;                  //phi inicial
+double end_position_x = 1900;        //x final
+double end_position_y = 2000;       //y final
+double next_phi;                    //phi seguinte
+    
+//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
+    
+//Variaveis de comando as rodas
+double w;                           //velociade angular do robo
+double w_left;                      //velocidade roda esquerda
+double w_right;                     //velocidade roda direita
+double v = 2100;                    //velocidade linear do robo   
+double dist;
+    
 int main()
 {
-    float odomX, odomY, odomTheta;
+    while (Button == 1) {}
+    wait(1);
+    
+    //float odomX, odomY, odomTheta;
     struct RPLidarMeasurement data;
     
     pc.baud(115200);
     init_communication(&pc);
 
     // Lidar initialization
+    rplidar_motor.write(1.0f);
     rplidar_motor.period(0.001f);
     lidar.begin(se_lidar);
     lidar.setAngle(0,360);
 
-    pc.printf("Program started.\n");
-        
     lidar.startThreadScan();
+
+    //--------------------------------------------------------------------------
     
-    while(1) {
+    //Espera que o botao seja premido
+    while (Button == 1) {}
+    wait(1);
+    
+    //Funcao para obter a matriz com os indices das seccoes assim como as amplitudes
+    get_Sector_Matrix(Sector_Matrix_Sections, Sector_Matrix_Abs);
+    
+    //Distancia ao ponto objetivo
+    dist = sqrt((double)pow(end_position_x-x,2) + pow(end_position_y-y,2));
+    
+    while(dist > 50) {
         // 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.
+            //pc.printf("%f;%f;%d;%c;\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+        
+        
+        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;
         }
-       wait(0.01); 
+        //Print coordenadas habituais
+        //pc.printf("x_atual = %f, y_atual = %f\n",x,y);
+        //Distancia ao ponto objetivo
+        dist = sqrt((double)pow(end_position_x-x,2) + pow(end_position_y-y,2));
+        //pc.printf("\ndist = %f\n", dist);
+        
+        //Conversao dos angulos em radianos
+        angle = (2*pi/360)*(data.angle); //
+        angle = atan2(sin(angle + pi/2 + phi),cos(angle + pi/2 + phi));
+        update_map(Log_Map_Matrix, Aux_Matrix, Map_Matrix, data.distance/50, angle, x, y); //Funcao para atualizar o mapa
+        
+        //Funcao para obter o mapa das redondezas do robo
+        get_Surroundings_Matrix(Surroundings_Matrix, x, y, Map_Matrix);
+        
+        for(int n = 0; n < 15; n++){
+            for(int m = 0; m < 15; m++){
+                pc.printf("%d,",Surroundings_Matrix[n][m]);
+            }
+            pc.printf("\n");
+        }
+        
+        //Funcao para obter a direcao que o robo deve tomar em funcao da sua posicao atual e das suas redondezas    
+        next_phi = get_orientation_angle(x, y, end_position_x, end_position_y, Surroundings_Matrix, Sector_Matrix_Sections, Sector_Matrix_Abs, 31);
+        //pc.printf("\n\n next phi = %f\n\n", next_phi);
+        
+        //Controlo proporcional da direcao
+        w = k*atan2(sin(next_phi-phi),cos(next_phi-phi));
+        
+        //Enviar os comandos de velocidades para as rodas
+        w_left=(v-(L/2)*w)/r;
+        w_right=(v+(L/2)*w)/r;
+        setSpeeds(w_left,w_right); 
+        }   
     }
-}
\ No newline at end of file
+    setSpeeds(0,0);
+    
+    return(0);
+    }
\ No newline at end of file