Gonçalo Lopes
VFH com Lidar
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);
-}
-
+