Gonçalo Lopes
VFH com Lidar
Revision 5:25bd866ef068, committed 2021-05-17
- Comitter:
- xaficz
- Date:
- Mon May 17 15:03:21 2021 +0000
- Parent:
- 4:53ac11e9e8b9
- Child:
- 6:df6b8b2468d8
- Commit message:
- 4_real
Changed in this revision
--- a/Robot.cpp Mon May 03 15:22:53 2021 +0000
+++ b/Robot.cpp Mon May 17 15:03:21 2021 +0000
@@ -60,4 +60,22 @@
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
+}
+
+//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 03 15:22:53 2021 +0000 +++ b/Robot.h Mon May 17 15:03:21 2021 +0000 @@ -48,7 +48,7 @@ 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 *y_bresenham(int x1, int y1, int x2, int y2); int *x_bresenham(int x1, int y1, int x2, int y2);
--- a/VFH.cpp Mon May 03 15:22:53 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,567 +0,0 @@
-#include "mbed.h"
-#include "Robot.h"
-#include "math.h"
-#include "BufferedSerial.h"
-#include "Communication.h"
-#include "rplidar.h"
-
-#define pi 3.14159265359
-
- Serial pc_Point(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;
-
-// ****** Inicialização de Variáveis ****** //
-
-float Distance; // Distância Devolvida pela Leitura do LIDAR
-float Angle; // Angulo Devolvida pela Leitura do LIDAR
-
-int *Celulas_x; // Coordenada x das Células Devolvidas Pelas Funções Bresenham
-int *Celulas_y; // Coordenada y das Células Devolvidas Pelas Funções Bresenham
-int size; // Número de Células Devolvidas Pelas Funções Bresenham
-int size_aux; // Variável Auxiliar Assossiada às Funções Bresenham
-int x_aux; // Variável Auxiliar Assossiada às Funções Bresenham
-int y_aux; // Variável Auxiliar Assossiada às Funções Bresenham
-float l_free = -0.65;
-float l_occ = 0.65;
-int Leituras = 0; // Variável que Funciona como Contador
-
-float Delta_Dr; // Variação/Deslocamento da Roda Direita
-float Delta_Dl; // Variação/Deslocamento da Roda Direita
-float Delta_D; // Variação/Deslocamento da Plataforma
-float Delta_Theta; // Variação da Orientação da Plataforma
-
-// Distâncias
-float d = 4; // Distância Constante de Seguimento entre o Ponto Atual e o Ponto Objetivo (Ponto Atual + Força Resultante)
-float d_Atr; // Distância entre o Ponto Atual e o Ponto Final
-
-// 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 x_Cartesiano; // Coordenada X Relativa à Celula Ativa (Sistema de Coordenadas Mundo)
-float y_Cartesiano; // Coordenada Y Relativa à Celula Ativa (Sistema de Coordenadas Mundo)
-float x_obj; // Coordenada X Relativa ao Ponto Objetivo
-float y_obj; // Coordenada Y Relativa ao Ponto Objetivo
-float x_Objeto; // Coordenada X Relativa ao Objeto Detetado pelo LIDAR
-float y_Objeto; // Coordenada Y Relativa ao Objeto Detetado pelo LIDAR
-float Mundo_X_Sensor; // Coordenada X Relativa ao Objeto Detetado pelo LIDAR após as Devidas Transformações
-float Mundo_Y_Sensor; // Coordenada X Relativa ao Objeto Detetado pelo LIDAR após as Devidas Transformações
-float x_Celula_Objeto; // Coordenada X Relativa ao Objeto Detetado pelo LIDAR (Sistema de Coordenadas baseado em Células de Aresta 5 cm)
-float y_Celula_Objeto; // Coordenada Y Relativa ao Objeto Detetado pelo LIDAR (Sistema de Coordenadas baseado em Células de Aresta 5 cm)
-
-// Histograma
-int Alpha = 10;
-int Bins = 36; // 360/Alpha
-float Beta;
-int K;
-float d_Celula;
-float m;
-float a = 1;
-float b = a/35.3553; // sqrt(1250) = 35.3553
-float h[36]; // h[Bins]
-
-// Histograma Suavizado
-int j_Aux;
-float h_Aux;
-float h_Suavizado[36]; // h_Suavizado[Bins]
-
-// Vales
-int Ocupado[36]; // Ocupado[Bins]
-int N_Vales;
-int Cnt;
-int Vale[12]; // Vale[Bins/3]
-
-// Direção da Força Atrativa
-float F_Atr;
-float F_Atr_Aux;
-int K_Atr;
-float Theta;
-float Vale_Dif[12]; // Vale_Dif[Bins/3]
-float Menor_Dist;
-int Indice;
-
-// Erro
-float e; // Erro de Seguimento (Diferença entre a distância do ponto atual ao ponto objetivo e a distância constante de seguimento d)
-float erro = 0; // Somatório dos Erros de Seguimento de Cada Iteração
-
-// Controladores
-float C_Proporcional; // Controlador Proporcional de Velocidade Linear
-float C_Integral; // Controlador Integral de Velocidade Linear
-
-// Controlo de Orientação da Plataforma
-float Orientation; // Ângulo do Ponto Objetivo Relativamente à Plataforma
-float Dif_Entre_Ang; // Variável Auxiliar que define que o Valor do Ângulo de Orientação se encontra entre [-pi/2,pi/2]
-float v; // Velocidade Linear da Plataforma
-float w; // Velocidade Angular da Plataforma
-
-// Velocidades das Rodas
-float w_Right; // Velocidade Aplicada na Roda Direita
-float w_Left; // Velocidade Aplicada na Roda Esquerda
-
-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)
-{
- pc_Point.baud(115200);
- init_communication(&pc_Point);
-
- 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);
-
- h[1]=100;
- pc_Point.printf("%f\n",h[1]);
-
-
- for (int i = 0; i < Bins; i++) {
- h[i] = 0;
- h_Suavizado[i] = 0;
- Ocupado[i] = 0;
- pc_Point.printf("%f\n",h[i]);
- }
-
- for (int i = 0; i < Bins/3; i++) {
- Vale[i] = 0;
- Vale_Dif[i] = 0;
- }
-
-//::::::::::::::::::::Inicialização::::::::::::::::::::::::::::::::
-pc_Point.printf("FUNFA\n");
-
- // while(Button_VFH == 1) { }
- wait(1);
-pc_Point.printf("FUNFA1\n");
-
- while(Leituras < 250) {
- if(lidar.pollSensorData(&data)== 0) {
-pc_Point.printf("Leitura\n");
-
- Distance = (data.distance)/10;
- Angle = pi*(data.angle)/180;
-
- Angle = atan2(sin(Angle),cos(Angle));
-
-
- //:::::::::::Localização do Objeto::::::::
-
- if(Distance >= 10 && Distance <= 200) {
-
- pc_Point.printf("%f _____ %f\n", Angle, Distance);
-
- x_Objeto = Distance*cos(Angle);
- y_Objeto = Distance*sin(Angle);
-
- Mundo_X_Sensor = x + 5*cos(phi) - sin(phi) - y_Objeto*cos(phi) - x_Objeto*sin(phi);
- Mundo_Y_Sensor = y + cos(phi) + 5*sin(phi) + x_Objeto*cos(phi) - y_Objeto*sin(phi);
-
- x_Celula_Objeto = ceil(Mundo_X_Sensor/5) - 1;
- y_Celula_Objeto = ceil(Mundo_Y_Sensor/5) - 1;
-
- // ****** Aplicação do Algoritmo de Bresenham ****** //
-
- if(x_Celula_Objeto >= 0 && x_Celula_Objeto <= 79 && y_Celula_Objeto >= 0 && y_Celula_Objeto <= 79) {
-
- Celulas_x = x_bresenham(x_Celula, y_Celula, x_Celula_Objeto, y_Celula_Objeto);
- Celulas_y = y_bresenham(x_Celula, y_Celula, x_Celula_Objeto, y_Celula_Objeto);
-
- size = Celulas_x[79];
-
- for(int i=0; i < (size-1); i++) {
- x_aux = Celulas_x[i];
- y_aux = Celulas_y[i];
- Log_Map[x_aux][y_aux] = Log_Map[x_aux][y_aux] + l_free;
- Map[x_aux][y_aux] = 1 - (1/(1+exp(Log_Map[x_aux][y_aux])));
- }
-
- x_aux = x_Celula_Objeto;
- y_aux = y_Celula_Objeto;
-
- Log_Map[x_aux][y_aux] = Log_Map[x_aux][y_aux] + l_occ;
- Map[x_aux][y_aux] = 1 - (1/(1+exp(Log_Map[x_aux][y_aux])));
-
- Leituras += 1;
- pc_Point.printf("%d\n",Leituras);
- pc_Point.printf("%d\n",Map[40][40]);
- pc_Point.printf("%d\n",Log_Map[40][40]);
-
- }
- }
- }
- }
-
- for(int k=0;k<80;k++){
- for(int j=0;j<80;j++){
- if(Map[k][j]<0){
- pc_Point.printf("0");
- }
- else {pc_Point.printf("0");
- }
-
- }
- pc_Point.printf("\n");
- }
-
-
- //::::::::::Andanças:::::::::::::
-
- d_Atr = sqrt(pow(x_final-x,2) + pow(y_final-y,2));
-
- pc_Point.printf("Distancia - %f\n",d_Atr);
-
- while(d_Atr > 3) {
- if(lidar.pollSensorData(&data)== 0) {
-
- pc_Point.printf("Distancia - %f\n",d_Atr);
-
- // Odometria
-
- getCountsAndReset();
-
- Delta_Dr = (2*pi*r*countsRight)/1440;
- Delta_Dl = (2*pi*r*countsLeft)/1440;
-
- Delta_D = (Delta_Dr+Delta_Dl)/2;
- Delta_Theta = (Delta_Dr-Delta_Dl)/L;
-
- if(Delta_Theta==0) {
- x = x + Delta_D*cos(phi);
- y = y + Delta_D*sin(phi);
- phi = phi;
- } else {
- x = x + Delta_D*(sin(Delta_Theta/2)/(Delta_Theta/2))*cos(phi+(Delta_Theta/2));
- y = y + Delta_D*(sin(Delta_Theta/2)/(Delta_Theta/2))*sin(phi+(Delta_Theta/2));
- phi = phi+Delta_Theta;
- }
-
- //Condição de Paragem
- d_Atr = sqrt(pow(x_final-x,2) + pow(y_final-y,2));
-
- // if(d_Atr < 2.5) {
- // break;
- // }
-
- //Celula da Odometria
-
- x_Celula = ceil(x/5) - 1;
- y_Celula = ceil(y/5) - 1;
-
- // ****** Leitura dos Dados do LIDAR ****** //
-
- if(lidar.pollSensorData(&data)== 0) {
-
- Distance = (data.distance)/10;
- Angle = pi*(data.angle)/180;
-
- Angle = atan2(sin(Angle),cos(Angle));
-
- // ****** Localização do Objeto ****** //
-
- if(Distance >= 10 && Distance <= 200) {
-
- // pc_Point.printf("%f %f\n", Angle, Distance);
-
- x_Objeto = Distance*cos(Angle);
- y_Objeto = Distance*sin(Angle);
-
- Mundo_X_Sensor = x + 4*cos(phi) - sin(phi) - y_Objeto*cos(phi) - x_Objeto*sin(phi);
- Mundo_Y_Sensor = y + cos(phi) + 4*sin(phi) + x_Objeto*cos(phi) - y_Objeto*sin(phi);
-
- x_Celula_Objeto = ceil(Mundo_X_Sensor/5) - 1;
- y_Celula_Objeto = ceil(Mundo_Y_Sensor/5) - 1;
-
- // ****** Aplicação do Algoritmo de Bresenham ****** //
-
- if(x_Celula_Objeto >= 0 && x_Celula_Objeto <= 79 && y_Celula_Objeto >= 0 && y_Celula_Objeto <= 79) {
-
- Celulas_x = x_bresenham(x_Celula, y_Celula, x_Celula_Objeto, y_Celula_Objeto);
- Celulas_y = y_bresenham(x_Celula, y_Celula, x_Celula_Objeto, y_Celula_Objeto);
-
- size = Celulas_x[79];
-
- for(int i=0; i < (size-1); i++) {
- x_aux = Celulas_x[i];
- y_aux = Celulas_y[i];
- Log_Map[x_aux][y_aux] = Log_Map[x_aux][y_aux] + l_free;
- Map[x_aux][y_aux] = 1 - (1/(1+exp(Log_Map[x_aux][y_aux])));
- }
-
- x_aux = x_Celula_Objeto;
- y_aux = y_Celula_Objeto;
-
- Log_Map[x_aux][y_aux] = Log_Map[x_aux][y_aux] + l_occ;
- Map[x_aux][y_aux] = 1 - (1/(1+exp(Log_Map[x_aux][y_aux])));
-
- }
- }
- }
-
- for(int k=0;k<80;k++){
- for(int j=0;j<80;j++){
- if(Map[k][j]<0){
- pc_Point.printf("0");
- }
- else {pc_Point.printf("1");
- }
-
- }
- pc_Point.printf("\n");
- }
-
-
- //::::::::::::::Histograma::::::::::::::::::::::::::
-
- // zona ativa11 * 11 celulas)
- for (int i = x_Celula - 5; i <= x_Celula + 5; i++) {
- for (int j = y_Celula - 5; j <= y_Celula + 5; j++) {
- // Verificação se a Região Ativa se Encontra Englobada no Ambiente da Plataforma (4m * 4m)
- if (i >= 0 && i < 80 && j >= 0 && j < 80) {
-
- x_Cartesiano = (i-1)*5 + 2.5;
- y_Cartesiano = (j-1)*5 + 2.5;
-
- // Ângulo
- Beta = atan2(y_Cartesiano-y,x_Cartesiano-x);
- Beta = atan2(sin(Beta),cos(Beta));
- Beta = Beta * 180 / pi;
- if (Beta<0) {
- Beta = Beta + 360;
- }
-
- // Setor K Associado a Cada Célula
- K = ceil(Beta/Alpha);
-
- // Cálculo da Força de Repulsão Induzida pelas Células Ativas Englobadas pela Região Ativa
- if(Map[i][j] != 0) {
-
- // Distância entre a Celula Ativa e o Robot
- d_Celula = sqrt(pow(x_Cartesiano-x,2) + pow(y_Cartesiano-y,2));
-
- // Amplitude
- m = Map[i][j] * (a-b*d_Celula);
-
- // Medida de Densidade de Obstáculos
- h[K] = h[K] + m;
- }
- }
- }
- }
-
- //::::::::::::::::::::Suavizaçao do histograma::::::::::::::::::
-
- // Após a Extração das Medidas de Densidade MPO é Gerado um Histograma
- // Suavizado Mediante a Aplicação de Filtragem de Fase Nula
-
- for (int i = 0; i < Bins; i++) {
- for (int j = (i-Limiar_Histograma); j <= (i+Limiar_Histograma); j++) {
-
- if(j == (i-Limiar_Histograma) || j == (i+Limiar_Histograma)) {
- if(j < 0) {
- j_Aux = Bins+j;
- h_Aux = h[j_Aux];
- } else if(j == 0) {
- j_Aux = Bins;
- h_Aux = h[j_Aux];
- } else if(j > Bins) {
- j_Aux = j - Bins;
- h_Aux = h[j_Aux];
- } else {
- h_Aux = h[j];
- }
- }
-
- else if(j == i) {
- h_Aux = Limiar_Histograma*h[j];
- }
-
- else {
- if(j < 0) {
- j_Aux = Bins+j;
- h_Aux = 2*h[j_Aux];
- } else if(j == 0) {
- j_Aux = Bins;
- h_Aux = 2*h[j_Aux];
- } else if(j > Bins) {
- j_Aux = j - Bins;
- h_Aux = 2*h[j_Aux];
- } else {
- h_Aux = 2*h[j];
- }
- }
-
- h_Suavizado[i] = h_Suavizado[i] + h_Aux;
- }
-
- h_Suavizado[i] = h_Suavizado[i]/(2*Limiar_Histograma+1);
- }
-
- // ****** Vales ****** //
-
- // Definir se o Setor se encontra ocupado:
- // 0 -> Livre
- // 1 -> Ocupado
- for(int i = 0; i < Bins; i++) {
- if(h_Suavizado[i] < Limiar_Vale) {
- Ocupado[i]=0;
- } else {
- Ocupado[i]=1;
- }
- }
-
- // Numero de Vales
- N_Vales = 0;
-
- Cnt = 0;
- while(Cnt < Bins) {
- if(Cnt == Bins-2) { // Se tiver na penultima bin -> vou ver até à bin 1
- if(Ocupado[Cnt]==0 && Ocupado[Cnt+1]==0 && Ocupado[1]==0) {
- N_Vales = N_Vales + 1;
- Vale[N_Vales] = Cnt+1;
- Cnt=Cnt+3;
- } else {
- Cnt = Cnt+1;
- }
- } else if(Cnt == Bins-1) { // Se tiver na ultima bin -> vou ver até à bin 2
- if(Ocupado[Cnt]==0 && Ocupado[1]==0 && Ocupado[2]==0) {
- N_Vales = N_Vales + 1;
- Vale[N_Vales] = 1;
- Cnt=Cnt+3;
- } else {
- Cnt = Cnt+1;
- }
- } else if(Ocupado[Cnt]==0 && Ocupado[Cnt+1]==0 && Ocupado[Cnt+2]==0) {
- N_Vales = N_Vales + 1;
- Vale[N_Vales] = Cnt+1;
- Cnt=Cnt+3;
- } else {
- Cnt = Cnt+1;
- }
- }
-
- // ****** Direção da Força Atrativa ****** //
-
- F_Atr = atan2(y_final-y,x_final-x);
- F_Atr = atan2(sin(F_Atr),cos(F_Atr));
-
- F_Atr_Aux = F_Atr * 180 / pi;
- if (F_Atr_Aux<0) {
- F_Atr_Aux = F_Atr_Aux + 360;
- }
-
- K_Atr = ceil(F_Atr_Aux/Alpha);
-
- if(Ocupado[K_Atr] == 0) {
- Theta = F_Atr;
- } else {
- for (int i=0; i<N_Vales; i++) {
- Vale_Dif[i] = abs(K_Atr - Vale[i]);
- }
-
- Menor_Dist = Vale_Dif[1];
- for (int i=0; i<N_Vales; i++) {
- if(Vale_Dif[i]<Menor_Dist) {
- Menor_Dist = Vale_Dif[i];
- Indice = i;
- } else {
- Indice = 1;
- }
- }
-
- Theta = Alpha*Vale[Indice] - 0.5*Alpha;
- Theta = Theta * pi/180;
- }
-
- // ****** Definição do Ponto Objetivo ****** //
-
- x_obj = x + 5*cos(Theta);
- y_obj = y + 5*sin(Theta);
-
- // ****** Erro ****** //
-
- e = sqrt(pow(x_obj-x,2) + pow(y_obj-y,2)) - d;
-
- // ****** Lei de Controlo ****** //
-
- // Controlador Proporcional de Velocidade Linear
- C_Proporcional = kv*e;
-
- // Controlador Integral de Velocidade Linear
- erro = erro + e;
- C_Integral = ki*erro;
-
- // Velocidade Linear da Plataforma
- v = C_Proporcional + C_Integral;
-
- // Ângulo do Ponto Objetivo Relativamente à Plataforma
- Orientation = atan2(y_obj-y,x_obj-x);
- Orientation = atan2(sin(Orientation),cos(Orientation));
- Dif_Entre_Ang = atan2(sin(Orientation-phi),cos(Orientation-phi));
-
- // Velocidade Angular -> Controlador Proporcional de Velocidade Linear
- w = ks*Dif_Entre_Ang;
-
- // Aplicação da Velocidade nas Rodas
- w_Right = (v+(L/2)*w)/r;
- w_Left = (v-(L/2)*w)/r;
-
- //::::::::::::::::::Reset
- for (int i = 0; i < Bins; i++) {
- h[i] = 0;
- h_Suavizado[i] = 0;
- Ocupado[i] = 0;
- }
-
- for (int i = 0; i < Bins/3; i++) {
- Vale[i] = 0;
- Vale_Dif[i] = 0;
- }
-
- // ****** Limitador de Velocidade ****** //
-
- // Velocidade da Roda de valor mais baixo é alterada de forma proporcional
- if(w_Right > w_Max) {
-
- w_Left = (w_Left*w_Max)/w_Right;
- w_Right = w_Max;
-
- }
- if(w_Left > w_Max) {
-
- w_Right = (w_Right*w_Max)/w_Left;
- w_Left = w_Max;
-
- }
-
- // Uma vez que para valores de velocidade das rodas menor que 20 a plataforma
- // deixa de se movimentar, quando a plataforma registar valores abaixo desse
- // o ciclo, e consequentemente o movimento da plataforma, é abortado
- if(w_Right < 20 && w_Left < 20) {
-
- break;
-
- }
-
- // ****** Aplicação de Velocidade nas Rodas ****** //
-
- setSpeeds(w_Left,w_Right);
-
- // ****** Emergency Button ****** //
-
- if (Button_VFH == 0) {
-
- break;
-
- }
-
- }
-
- }
-
- // ****** Saída do Ciclo -> Paragem da Plataforma ****** //
- setSpeeds(0,0);
-}
--- 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);
+}
+