Experiencias do Henrique na quinta/sexta a noite
Dependencies: BufferedSerial
Revision 10:6c8ea68e9bac, committed 2021-05-12
- Comitter:
- ppovoa
- Date:
- Wed May 12 19:00:45 2021 +0000
- Parent:
- 9:76b59c5220f1
- Child:
- 11:58187c7de709
- Commit message:
- Lidar
Changed in this revision
Functions.cpp | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Functions.cpp Tue May 11 17:53:17 2021 +0000 +++ b/Functions.cpp Wed May 12 19:00:45 2021 +0000 @@ -35,7 +35,7 @@ float Algorith_Inverse(float xi, float yi, float xt, float yt, float z){ float z_max = 200; // 2 m - float alfa = 5; // 5 cm + float alfa = 10; //float beta = 1; // 1 grau float L0 = 0.0; float Locc = 0.65;
--- a/main.cpp Tue May 11 17:53:17 2021 +0000 +++ b/main.cpp Wed May 12 19:00:45 2021 +0000 @@ -18,7 +18,7 @@ PwmOut rplidar_motor(D3); float MapaLog[40][40] = {0}; -float Mapa40[40][40] = {0.5}; +float Mapa40[40][40]; void bresenham(float poseX, float poseY, float xf, float yf, float z); @@ -37,16 +37,20 @@ //float odomX, odomY, odomTheta; struct RPLidarMeasurement data; + //Inicializar Mapa das probabilidades a 0.5 + for(int i=0; i<40; i++) + for(int j=0; j<40; j++) + Mapa40[i][j]=0.5; + // Lidar initialization rplidar_motor.period(0.001f); //rplidar_motor.write(0.5f); lidar.begin(se_lidar); lidar.setAngle(0, 360); - int pose[3] = {20, 20}; // Ponto Inicial - float p_angulo = 0; - int LidarP[2]; // pontos na plataforma - int LidarW[2]; // pontos no mundo + float pose[3] = {20, 20, 0}; // Ponto Inicial + float LidarP[2]; // pontos na plataforma + float LidarW[2]; // pontos no mundo /*pc.printf("Inicializacao MapaLog\n\r"); for(int i = 0; i < 40; i++){ @@ -57,8 +61,8 @@ // matriz rotacao world plataforma - float R_WP[3][3]= {{cos(p_angulo), -sin(p_angulo), pose[0]}, - {sin(p_angulo), cos(p_angulo), pose[1]}, + float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]}, + {sin(pose[2]), cos(pose[2]), pose[1]}, {0, 0, 1}}; setSpeeds(0, 0); @@ -80,26 +84,22 @@ lidar.startThreadScan(); pc.printf("Entrar no ciclo\n\r"); - while(leituras < 500){ + while(leituras < 1000){ if(lidar.pollSensorData(&data) == 0) - { - /*if (UserButton == 0) { // Button is pressed - break; - }*/ - + { //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement. float radians = (data.angle * static_cast<float>(PI))/180.0f; - LidarP[0] = -data.distance*cos(radians)- 2.8f; - LidarP[1] = -data.distance*sin(radians)- 1.5f; + LidarP[0] = -data.distance*sin(radians)- 2.8f; + LidarP[1] = -data.distance*cos(radians)- 1.5f; //W_P = R_WP * p_P LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2]; // pontos onde o feixe passou - bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance); + bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5); leituras++; } @@ -112,20 +112,16 @@ rplidar_motor.write(0.0f); for(int i=0; i<40; i++){ for(int j=0; j<40; j++){ - pc.printf("%f|", Mapa40[i][j]); - //send_map(Mapa40[j][i]); // envia linha em linha (j i) - //send_odometry(1, 2, Mapa40[j][i], j, i,10, 30); // faz prints estranhos no Putty + //pc.printf("%0.3f ", Mapa40[i][j]); + send_odometry(1, 2, j+1, i+1, Mapa40[j][i],10, 30); // faz prints estranhos no Putty + wait(0.1); } - pc.printf("\n-----------------------------\n\r"); + //pc.printf("\n\r"); + //pc.printf("\n-----------------------------\n\r"); } } - - - - - void bresenham(float poseX, float poseY, float xf, float yf, float z){ int T, E, A, B; int x = static_cast<int>(poseX); @@ -169,7 +165,7 @@ if (x >= 0 && y >= 0 && x < 40 && y < 40){ // Mapear mapa do Logaritmo MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z); - pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y]))); + //pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y]))); Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y])); }