SRA_HenriquePovoa
/
Lidar_Rodas
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: BufferedSerial
Revision 9:76b59c5220f1, committed 2021-05-11
- Comitter:
- henkiwan
- Date:
- Tue May 11 17:53:17 2021 +0000
- Parent:
- 8:ad8766cf2ec0
- Child:
- 10:6c8ea68e9bac
- Commit message:
- yah; ; temos de ver o log;
Changed in this revision
--- a/Communication.cpp Tue May 11 15:05:49 2021 +0000 +++ b/Communication.cpp Tue May 11 17:53:17 2021 +0000 @@ -2,7 +2,7 @@ #include "mbed.h" #include "MessageBuilder.h" -const char max_len = 30; +//const char max_len = 30; Serial *serial_object; MessageBuilder bin_msg;
--- a/Functions.cpp Tue May 11 15:05:49 2021 +0000
+++ b/Functions.cpp Tue May 11 17:53:17 2021 +0000
@@ -47,7 +47,7 @@
//if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
//L = L0;
- if ((z < z_max) && (abs(r-z_max) < alfa/2.0))
+ if ((z < z_max) && (abs(r-z) < alfa/2.0))
L = Locc;
else if (r <= z)
L = Lfree;
@@ -58,53 +58,3 @@
}
-void bresenham(float poseX, float poseY, float xf, float yf, float z){
- int T, E, A, B;
- int x = static_cast<int>(poseX);
- int y = static_cast<int>(poseY);
- int dx = static_cast<int>(abs(xf - poseX));
- int dy = static_cast<int>(abs(yf - poseY));
-
- int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
- int s2 = static_cast<int>((yf - poseY)/dy);
-
- int interchange = 0;
-
- if (dy > dx){
- T = dx;
- dx = dy;
- dy = T;
- interchange = 1;
- }
-
- E = 2*dy - dx;
- A = 2*dy;
- B = 2*dy - 2*dx;
-
- for (int i = 0; i<dx; i++){
- if (E < 0){
- if (interchange == 1){
- y = y + s2;
- }
- else{
- x = x + s1;
- }
- E = E + A;
- }
-
- else{
- y = y + s2;
- x = x + s1;
- E = E + B;
- }
-
- 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);
- Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
- }
-
-
-
- }
-}
--- a/Functions.h Tue May 11 15:05:49 2021 +0000 +++ b/Functions.h Tue May 11 17:53:17 2021 +0000 @@ -8,4 +8,4 @@ void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[]); void JanelaAtivaVFF(float pose[], float Mapa_40[40][40], float poseFinal[], float FCRepul, float FCAtracao, float RectSize, float *ForcaResult); float Algorith_Inverse(float xi, float yi, float xt, float yt, float z); -void bresenham(float xi, float yi, float xf, float yf, float z); +//void bresenham(float xi, float yi, float xf, float yf, float z);
--- a/main.cpp Tue May 11 15:05:49 2021 +0000
+++ b/main.cpp Tue May 11 17:53:17 2021 +0000
@@ -18,7 +18,9 @@
PwmOut rplidar_motor(D3);
float MapaLog[40][40] = {0};
-float Mapa40[40][40];
+float Mapa40[40][40] = {0.5};
+
+void bresenham(float poseX, float poseY, float xf, float yf, float z);
int main()
{
@@ -39,9 +41,9 @@
rplidar_motor.period(0.001f);
//rplidar_motor.write(0.5f);
lidar.begin(se_lidar);
- lidar.setAngle(0,360);
+ lidar.setAngle(0, 360);
- int pose[3] = {20,20}; // Ponto Inicial
+ int pose[3] = {20, 20}; // Ponto Inicial
float p_angulo = 0;
int LidarP[2]; // pontos na plataforma
int LidarW[2]; // pontos no mundo
@@ -53,14 +55,13 @@
}
}*/
- float Mapa40[40][40];
// 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]},
{0, 0, 1}};
- setSpeeds(0,0);
+ setSpeeds(0, 0);
int leituras = 0;
@@ -86,7 +87,7 @@
break;
}*/
- pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
+ //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;
@@ -106,12 +107,73 @@
// Converter o logaritmo para o mapa 40
+ pc.printf("\nFIM DO CICLO\n========================\n\n\r");
+
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]);
+ pc.printf("%f|", Mapa40[i][j]);
//send_map(Mapa40[j][i]); // envia linha em linha (j i)
- //send_odometry(1, 2, Mapa40[j][i], 4, 10,10, 30); // faz prints estranhos no Putty
+ //send_odometry(1, 2, Mapa40[j][i], j, i,10, 30); // faz prints estranhos no Putty
}
+ 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);
+ int y = static_cast<int>(poseY);
+ int dx = static_cast<int>(abs(xf - poseX));
+ int dy = static_cast<int>(abs(yf - poseY));
+
+ int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
+ int s2 = static_cast<int>((yf - poseY)/dy);
+
+ int interchange = 0;
+
+ if (dy > dx){
+ T = dx;
+ dx = dy;
+ dy = T;
+ interchange = 1;
+ }
+
+ E = 2*dy - dx;
+ A = 2*dy;
+ B = 2*dy - 2*dx;
+
+ for (int i = 0; i<dx; i++){
+ if (E < 0){
+ if (interchange == 1){
+ y = y + s2;
+ }
+ else{
+ x = x + s1;
+ }
+ E = E + A;
+ }
+
+ else{
+ y = y + s2;
+ x = x + s1;
+ E = E + B;
+ }
+
+ 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])));
+ Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
+ }
+
+
+
}
}
\ No newline at end of file