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 13:20e124fba426, committed 2021-05-15
- Comitter:
- henkiwan
- Date:
- Sat May 15 03:17:01 2021 +0000
- Parent:
- 12:348038b466a3
- Commit message:
- Que merda;
Changed in this revision
--- a/Communication.cpp Fri May 14 05:52:55 2021 +0000
+++ b/Communication.cpp Sat May 15 03:17:01 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;
@@ -32,13 +32,4 @@
bin_msg.add(theta);
write_bytes(bin_msg.message, bin_msg.length());
-}
-
-void send_map(float Mapa)
-{
- bin_msg.reset();
- bin_msg.add('O');
- bin_msg.add(Mapa);
-
- write_bytes(bin_msg.message, bin_msg.length());
}
\ No newline at end of file
--- a/Communication.h Fri May 14 05:52:55 2021 +0000 +++ b/Communication.h Sat May 15 03:17:01 2021 +0000 @@ -6,6 +6,5 @@ void init_communication(Serial *serial_in); void write_bytes(char *ptr, unsigned char len); void send_odometry(int value1, int value2, int ticks_left, int ticks_right, float x, float y, float theta); -void send_map(float Mapa); #endif
--- a/Functions.cpp Fri May 14 05:52:55 2021 +0000
+++ b/Functions.cpp Sat May 15 03:17:01 2021 +0000
@@ -30,10 +30,10 @@
pose[2] = pose[2] + delta_ang;
}
-float Algorithm_Inverse(float xi, float yi, float xt, float yt, float z){
+float Algorith_Inverse(float xi, float yi, float xt, float yt, float z){
- float z_max = 200; // 2 m
- float alfa = 6.0f/5.0f;
+ float z_max = 200.0f/5.0f; // 2 m
+ float alfa = 20.0f/5.0f;
//float beta = 1; // 1 grau
float L0 = 0.0;
float Locc = 0.65;
--- a/Functions.h Fri May 14 05:52:55 2021 +0000 +++ b/Functions.h Sat May 15 03:17:01 2021 +0000 @@ -4,4 +4,4 @@ void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2]); 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 Algorithm_Inverse(float xi, float yi, float xt, float yt, float z); \ No newline at end of file +float Algorith_Inverse(float xi, float yi, float xt, float yt, float z); \ No newline at end of file
--- a/JanelaAtivaVFF.cpp Fri May 14 05:52:55 2021 +0000
+++ b/JanelaAtivaVFF.cpp Sat May 15 03:17:01 2021 +0000
@@ -35,9 +35,9 @@
// Inicialização de variaveis
float fr[2] = {0, 0};
float fa[2] = {0, 0};
- float FR_magn = 0;
- float FR_angle = 0;
- float FA_angle = 0;
+ //float FR_magn = 0;
+ //float FR_angle = 0;
+ //float FA_angle = 0;
float xt, yt, dist;
@@ -50,8 +50,7 @@
*/
for (int i = cima; i <= baixo; i++){
for (int j = esq; j <= dir; j++){
- //if(Mapa_40[i][j] >= 0.65)
- //printf("SUPERIOR!!!!! \n\r");
+ //if(Mapa_40[i][j]==1){
// Posicao do pixel real (cm)
//printf("%f\n\r", Mapa_40[i][j]);
xt = (5.0f*(static_cast<float>(j)))-2.5f;
@@ -59,23 +58,25 @@
// Distância do Robo ao Pixel
dist = sqrt(pow(yt-y0,2) + pow(xt-x0,2));
-
- // Forca de Repulsão
- FR_angle = atan2((yt-y0), (xt-x0));
- FR_magn = Mapa_40[i][j]*FCRepul/pow(dist,2);
- //}
- fr[0] = fr[0] + FR_magn*cos(FR_angle);
- fr[1] = fr[1] + FR_magn*sin(FR_angle);
+
+ // só as celulas acima de 0.6 que exercem uma forca repulsiva
+ if (Mapa_40[i][j] > 0.6f){
+ fr[0] = fr[0] + Mapa_40[i][j]*FCRepul*(xt-x0)/pow(dist,3);
+ fr[1] = fr[1] + Mapa_40[i][j]*FCRepul*(yt-y0)/pow(dist,3);
+ }
+ //}
}
}
//printf("Cima: %d, Baixo: %d, Esq= %d, Dir= %d\n\r", cima, baixo, esq, dir);
//printf("Mapa40[28][0]= %f, Mapa_40[28][0]==1: %d\n\r", Mapa_40[28][0], Mapa_40[28][0]==1);
- FA_angle = atan2((poseFinal[1]-y0), (poseFinal[0]-x0));
+ //FA_angle = atan2((poseFinal[1]-y0), (poseFinal[0]-x0));
- fa[0] = FCAtracao * cos(FA_angle);
- fa[1] = FCAtracao * sin(FA_angle);
+ dist = sqrt(pow(poseFinal[1]-y0,2) + pow(poseFinal[0]-x0,2));
+
+ fa[0] = FCAtracao * ((poseFinal[0]-x0)/dist);
+ fa[1] = FCAtracao * ((poseFinal[1]-y0)/dist);
ForcaResult[0] = fa[0];
ForcaResult[1] = fa[1];
--- a/main.cpp Fri May 14 05:52:55 2021 +0000
+++ b/main.cpp Sat May 15 03:17:01 2021 +0000
@@ -27,25 +27,25 @@
DigitalIn UserButton(USER_BUTTON); // Initialize Button
DigitalOut myled(LED1); // Initialize LED
- // PARAMETROS CONSTANTES
- float T = 0.5;
+ // PARAMETROS CONSTANSTES
+ float T = 0.01;
float wheelsRadius = 3.5;
float wheelsDistance = 13.5;
// Variaveis
- float k_v = 15; // 15
- float k_i = 0.3; //0.1
- float k_s = 11; //20
+ float k_v = 20; // 15
+ float k_i = 0.2; //0.1
+ float k_s = 8; //20
float v_erro = 15;
- int DPontos = 2; //distância de erro
+ int DPontos = 3; //distância de erro
- float RectSize = 4.0;
- float FCRepul = -1000; // Forca de Repulsao
- float FCAtracao = 5; // Forca de Atracao
+ float RectSize = 16.0;
+ float FCRepul = -8000; // Forca de Repulsao
+ float FCAtracao = 0.02; // Forca de Atracao
- float pose[3] = {30, 15, 0}; // Ponto Inicial
- float pose_f[3] = {60,100,0}; // Ponto Objetivo
+ float pose[3] = {30, 30, 0}; // Ponto Inicial
+ float pose_f[3]; // = {130,130,0}; // Ponto Objetivo (definido mais em baixo)
float errot = 0, erro = 0, erro_old = -1;
float Forcas[4] = {0, 0, 0, 0};
@@ -67,10 +67,12 @@
for(int j=0; j<40; j++)
Mapa40[i][j]=0.5;
- int leituras = 0; long dist;
+ int leituras = 0;
+ float dist;
+ float r;
// Lidar initialization
- rplidar_motor.period(0.001f);
+ rplidar_motor.period(0.01f);
//rplidar_motor.write(0.5f);
lidar.begin(se_lidar);
lidar.setAngle(0, 360);
@@ -78,7 +80,7 @@
setSpeeds(0, 0);
- pc.printf("waiting...\n\r");
+ pc.printf("wating...\n\r");
int start = 0;
while(start != 1) {
@@ -92,14 +94,14 @@
lidar.startThreadScan();
- while(leituras < 1000){
+ while(leituras < 1500){
if(lidar.pollSensorData(&data) == 0)
{
//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;
- dist = data.distance/10.0f;
+ dist = static_cast<float>(data.distance/10.0f); //converte de mm para cm
LidarP[0] = -dist*sin(radians)- 2.8f;
LidarP[1] = -dist*cos(radians)- 1.5f;
@@ -108,21 +110,26 @@
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];
+ /*if (data.angle > 0 && data.angle < 5){
+ pc.printf("Pose[0]: %f Pose[1]: %f Dist: %f Ang: %f LidarP[0]: %f LidarP[1]: %f LidarW[0]: %f LidarW[1]: %f\n\r", pose[0], pose[1], data.distance, data.angle, LidarP[0], LidarP[1], LidarW[0], LidarW[1]);
+ }*/
// pontos onde o feixe passou
- bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5);
+ bresenham(pose[0]/5.0f, pose[1]/5.0f, LidarW[0]/5.0f, LidarW[1]/5.0f, dist/5.0f);
leituras++;
}
+ wait(0.01);
}
- for(int f = 0; f < 40; f++){
- for(int g = 0; g < 40; g++){
- if(Mapa40[f][g] < 0.65f)
- pc.printf("0 ");
- else
- pc.printf("1 ");
+ //Código para enviar para o matlab
+ /*rplidar_motor.write(0.0f);
+ for(int i=0; i<40; i++){
+ for(int j=0; j<40; j++){
+ //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
}
- pc.printf("nova linha \n\r");
+ //pc.printf("\n\r");
+ //pc.printf("\n-----------------------------\n\r");
}
start = 0;
@@ -131,126 +138,197 @@
if (UserButton == 0) { // Button is pressed
myled = 0;
start = 1;
- rplidar_motor.write(0.5f);
+ }
+ }*/
+
+ pc.printf("Running\n\r");
+
+ for(int i=0; i<1; i++){
+
+ // Pontos ao pe da secretaria do prof
+
+ // Primeiro Ponto
+ if (i==0){
+ pose_f[0] = 160.0f;
+ pose_f[1] = 160.0f;
+ }
+
+ // Segundo Ponto
+ /*if (i==1){
+ pose_f[0] = 30;
+ pose_f[1] = 30;
+ }*/
+
+ /*// Terceiro Ponto
+ if (i==2){
+ pose_f[0] = 30;
+ pose_f[1] = 30;
+ }*/
+
+
+ while(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 8.0f) {
+
+ getCountsAndReset(); // Call getCountsAndReset() to read the values of encoders
+ // and reset them. The values become available on variables
+ // "countsLeft" and "countsRight".
+
+ //if(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 13){
+
+ JanelaAtivaVFF(pose, Mapa40, pose_f, FCRepul, FCAtracao, RectSize, Forcas);
+
+ ForcaResult[0] = Forcas[0]+Forcas[2]; // componente de x
+ ForcaResult[1] = Forcas[1]+Forcas[3]; // componente de y
+
+ Norma = sqrt(pow(ForcaResult[0],2) + pow(ForcaResult[1],2));
+ ForcaResult[0] = ForcaResult[0]/Norma; // Normalização da forca resultante entre a forca de repulsão e a forca de atracão
+ ForcaResult[1] = ForcaResult[1]/Norma;
+
+ // Angulo da forca resultante
+ angForca = atan2(ForcaResult[1], ForcaResult[0]);
+
+ //pc.printf("FR[0]: %f FR[1]: %f Ang: %f\n\r", ForcaResult[0], ForcaResult[1], angForca);
+ //}
+ //else
+ //angForca = atan2(pose_f[1] - pose[1], pose_f[0] - pose[0]);
+
+
+ PIntermedio[0] = pose[0] + v_erro * cos(angForca);
+ PIntermedio[1] = pose[1] + v_erro * sin(angForca);
+
+ erro = sqrt(pow(PIntermedio[1]-pose[1], 2)+ pow(PIntermedio[0]-pose[0],2)) - DPontos;
+
+ if(erro_old == -1){
+ erro_old = erro;
+ }
+
+ errot = errot + (erro + erro_old) * T/2.0f;
+
+ //Velocidade do Robo
+ vRobot = k_v * erro + k_i * errot;
+
+ erro_old = erro;
+
+ phi = atan2((PIntermedio[1]-pose[1]), (PIntermedio[0]-pose[0])); // Angulo entre o ponto intermedio e o robo
+
+ wRobot = k_s * atan2(sin(phi-pose[2]), cos(phi-pose[2])); //velocidade angular do robo;
+
+ // Velocidades das rodas
+ velRobot2velWheels(vRobot,wRobot,wheelsRadius,wheelsDistance,w);
+
+ // Racio da Velocidade
+ r = w[0]/w[1];
+
+ // nao permite velocidades menores que 50 e maiores que 100 mantendo o racio
+ if(w[0] < 50 || w[1] < 50){
+ if (r > 1){
+ w[1] = 50;
+ w[0] = 50*r;
+ }
+ else if(r < 1){
+ w[0] = 50;
+ w[1] = 50/r;
+ }
+ else{
+ w[0] = 50;
+ w[1] = 50;
+ }
+ }
+
+ if (w[0] > 100 || w[1] > 100){
+ if (r > 1){
+ w[0] = 100;
+ w[1] = 100/r;
+ }
+ else if(r < 1){
+ w[1] = 100;
+ w[0] = 100*r;
+ }
+ else{
+ w[0] = 100;
+ w[1] = 100;
+ }
+ }
+
+
+ /* (w[0] > 200)
+ w[0] = 200;
+
+ if (w[1] > 200)
+ w[1] = 200;*/
+
+ setSpeeds(w[0], w[1]);
+
+ nextPose(countsLeft, countsRight, wheelsRadius, wheelsDistance, pose);
+
+ leituras = 0;
+
+ while(leituras < 10){
+ if(lidar.pollSensorData(&data) == 0)
+ {
+ //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*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/5);
+
+ leituras++;
+ }
+ }
+
+ wait(T); // Delay of 0.01 seconds.
}
}
- pc.printf("Running\n\r");
-
- while(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 5.0f) {
-
- getCountsAndReset(); // Call getCountsAndReset() to read the values of encoders
- // and reset them. The values become available on variables
- // "countsLeft" and "countsRight".
-
- //if(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 13){
-
- pc.printf("x; %f, y: %f\n\r", pose[0], pose[1]);
-
- JanelaAtivaVFF(pose, Mapa40, pose_f, FCRepul, FCAtracao, RectSize, Forcas);
-
- ForcaResult[0] = Forcas[0]+Forcas[2]; // componente de x
- ForcaResult[1] = Forcas[1]+Forcas[3]; // componente de y
-
- Norma = sqrt(pow(ForcaResult[0],2) + pow(ForcaResult[1],2));
- ForcaResult[0] = ForcaResult[0]/Norma; // Normalização da forca resultante entre a forca de repulsão e a forca de atracão
- ForcaResult[1] = ForcaResult[1]/Norma;
-
- // Angulo da forca resultante
- angForca = atan2(ForcaResult[1], ForcaResult[0]);
- //}
- //else
- //angForca = atan2(pose_f[1] - pose[1], pose_f[0] - pose[0]);
-
-
- PIntermedio[0] = pose[0] + v_erro * cos(angForca);
- PIntermedio[1] = pose[1] + v_erro * sin(angForca);
-
- erro = sqrt(pow(PIntermedio[1]-pose[1], 2)+ pow(PIntermedio[0]-pose[0],2)) - DPontos;
-
- if(erro_old == -1){
- erro_old = erro;
- }
-
- errot = errot + (erro + erro_old) * T/2.0f;
-
- //Velocidade do Robo
- vRobot = k_v * erro + k_i * errot;
-
- erro_old = erro;
-
- phi = atan2((PIntermedio[1]-pose[1]), (PIntermedio[0]-pose[0])); // Angulo entre o ponto intermedio e o robo
-
- wRobot = k_s * atan2(sin(phi-pose[2]), cos(phi-pose[2])); //velocidade angular do robo;
-
- // Velocidades das rodas
- velRobot2velWheels(vRobot,wRobot,wheelsRadius,wheelsDistance,w);
-
- if (w[0] > 200)
- w[0]=200;
-
- if (w[1] > 200)
- w[1]=200;
-
- setSpeeds(w[0], w[1]);
-
- nextPose(countsLeft, countsRight, wheelsRadius, wheelsDistance, pose);
-
- leituras = 0;
-
- while(leituras < 5){
- if(lidar.pollSensorData(&data) == 0)
- {
- //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*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.0f, pose[1]/5.0f, LidarW[0]/5.0f, LidarW[1]/5.0f, data.distance/5.0f);
-
- leituras++;
- }
- }
-
- wait(T); // Delay of 0.5 seconds.
- }
myled=1;
setSpeeds(0, 0);
rplidar_motor.write(0.0f);
+
+ /*// Fica a espera do botao
+ start = 0;
+ while(start != 1) {
+ myled=1;
+ if (UserButton == 0) { // Button is pressed
+ myled = 0;
+ start = 1;
+ }
+ }
+
+ //Código para enviar para o matlab
+ for(int i=0; i<40; i++){
+ for(int j=0; j<40; j++){
+ send_odometry(1, 2, j+1, i+1, Mapa40[j][i],10, 30); // faz prints estranhos no Putty
+ }
+ }*/
+
+
}
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 = abs(static_cast<int>(xf - poseX));
int dy = abs(static_cast<int>(yf - poseY));
- int s1;
- int s2;
- // substitui o sign() do matlab
- if(static_cast<int>(xf - poseX) > 0)
- s1 = 1;
- else if (static_cast<int>(xf - poseX) == 0)
+ int s1, s2;
+
+ if (dx == 0)
s1 = 0;
else
- s1 = -1;
+ s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
- if(static_cast<int>(yf - poseY) > 0)
- s2 = 1;
- else if (static_cast<int>(yf - poseY) == 0)
+ if (dy == 0)
s2 = 0;
else
- s2 = -1;
-
+ s2 = static_cast<int>((yf - poseY)/dy);
int interchange = 0;
@@ -284,12 +362,10 @@
if (x >= 0 && y >= 0 && x < 40 && y < 40){
// Mapear mapa do Logaritmo
- MapaLog[x][y] = MapaLog[x][y] + Algorithm_Inverse(poseX, poseY, x, y, z);
+ MapaLog[y][x] = MapaLog[y][x] + 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]));
+ Mapa40[y][x] = 1 - 1/(1+exp(MapaLog[y][x]));
}
-
-
}
}
\ No newline at end of file