Henrique Cardoso
/
Lidar_Rodas
Experiencias do Henrique na quinta a noite
Revision 12:348038b466a3, committed 2021-05-14
- Comitter:
- henkiwan
- Date:
- Fri May 14 05:52:55 2021 +0000
- Parent:
- 11:58187c7de709
- Commit message:
- Experiencias que fiz de noite
Changed in this revision
--- a/Functions.cpp Thu May 13 16:28:09 2021 +0000 +++ b/Functions.cpp Fri May 14 05:52:55 2021 +0000 @@ -1,6 +1,5 @@ + #include <math.h> -#include <stdio.h> -#include "Functions.h" void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2]) { @@ -14,16 +13,16 @@ // Deslocamentos float d_l, d_r, desl, delta_ang, delta_x, delta_y; - d_l = 2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f ); - d_r = 2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f ); + d_l = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f )); + d_r = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f )); - desl = (d_l+d_r)/2.0f; + desl = (d_l+d_r)/2; delta_ang = (d_r-d_l)/wheelsDistance; - delta_x = desl * cos(pose[2]+delta_ang/2.0f); - delta_y = desl * sin(pose[2]+delta_ang/2.0f); + delta_x = desl * cos(pose[2]+delta_ang/2); + delta_y = desl * sin(pose[2]+delta_ang/2); pose[0] = pose[0] + delta_x; @@ -31,8 +30,7 @@ pose[2] = pose[2] + delta_ang; } - -float Algorith_Inverse(float xi, float yi, float xt, float yt, float z){ +float Algorithm_Inverse(float xi, float yi, float xt, float yt, float z){ float z_max = 200; // 2 m float alfa = 6.0f/5.0f; @@ -56,5 +54,4 @@ return L; -} - +} \ No newline at end of file
--- a/Functions.h Thu May 13 16:28:09 2021 +0000 +++ b/Functions.h Fri May 14 05:52:55 2021 +0000 @@ -1,11 +1,7 @@ + #include <math.h> #include "mbed.h" -extern float MapaLog[40][40], Mapa40[40][40]; - -//float MapaLog[40][40]; - -void velRobot2velWheels(float vRobot, float wRobot, float wheelsRadius, float wheelsDistance, float w[2]); +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 Algorith_Inverse(float xi, float yi, float xt, float yt, float z); -//void bresenham(float xi, float yi, float xf, float yf, float z); +float Algorithm_Inverse(float xi, float yi, float xt, float yt, float z); \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/JanelaAtivaVFF.cpp Fri May 14 05:52:55 2021 +0000 @@ -0,0 +1,85 @@ +#include <math.h> +#include <stdio.h> +#include <stdlib.h> + +void JanelaAtivaVFF(float pose[], float Mapa_40[40][40], float poseFinal[], float FCRepul, float FCAtracao, float RectSize, float *ForcaResult) +{ + float x0, y0; + int cima, baixo, esq, dir; + + x0 = static_cast<float>(ceil(pose[0]/5.0f)); + y0 = static_cast<float>(ceil(pose[1]/5.0f)); // para corresponder ao Y do plot (de baixo para cima) + + // condições caso a janela ativa esteja fora da matriz 40 por 40 + // se estiver fora entao fica no limite da matriz + cima = (int)(y0-floor(RectSize/2.0f)); + if(cima < 0) + cima = 0; + + baixo = (int)(y0+floor(RectSize/2.0f)); + if(baixo > 39) + baixo = 39; + + esq = (int)(x0-floor(RectSize/2.0f)); + if(esq < 0) + esq = 0; + + dir = (int)(x0+floor(RectSize/2.0f)); + if(dir > 39) + dir = 39; + + // Posicao do robo real (cm) + x0 = pose[0]; + y0 = pose[1]; + + // 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 xt, yt, dist; + + // confirmar (x_max - x_min) + /*for (int i = 0; i <= (baixo-cima); i++){ + for (int j = 0; j <= (dir-esq); j++){ + Janela[i][j] = Mapa_40[cima+i][esq+j]; + } + } + */ + 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"); + // Posicao do pixel real (cm) + //printf("%f\n\r", Mapa_40[i][j]); + xt = (5.0f*(static_cast<float>(j)))-2.5f; + yt = (5.0f*(static_cast<float>(i)))-2.5f; + + // 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); + } + } + + //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[0] = FCAtracao * cos(FA_angle); + fa[1] = FCAtracao * sin(FA_angle); + + ForcaResult[0] = fa[0]; + ForcaResult[1] = fa[1]; + ForcaResult[2] = fr[0]; + ForcaResult[3] = fr[1]; + +} \ No newline at end of file
--- a/main.cpp Thu May 13 16:28:09 2021 +0000 +++ b/main.cpp Fri May 14 05:52:55 2021 +0000 @@ -1,39 +1,65 @@ -// Coded by Luís Afonso 11-04-2019 #include "mbed.h" #include "BufferedSerial.h" #include "rplidar.h" #include "Robot.h" #include "Communication.h" #include "Functions.h" -#include "math.h" - +#include <math.h> +#include <stdio.h> #include <stdlib.h> -#include <stdio.h> -#define PI 3.1415926535 +#define PI 3.141592654 Serial pc(SERIAL_TX, SERIAL_RX); RPLidar lidar; BufferedSerial se_lidar(PA_9, PA_10); PwmOut rplidar_motor(D3); +void bresenham(float poseX, float poseY, float xf, float yf, float z); + float MapaLog[40][40] = {0}; float Mapa40[40][40]; -void bresenham(float poseX, float poseY, float xf, float yf, float z); - -int main() -{ +int main() { pc.baud(115200); init_communication(&pc); - pc.printf("======================\n\r"); - pc.printf("Inicio\n\r"); - DigitalIn UserButton(USER_BUTTON); // Initialize Button DigitalOut myled(LED1); // Initialize LED - //float odomX, odomY, odomTheta; + // PARAMETROS CONSTANTES + float T = 0.5; + 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 v_erro = 15; + + int DPontos = 2; //distância de erro + + float RectSize = 4.0; + float FCRepul = -1000; // Forca de Repulsao + float FCAtracao = 5; // Forca de Atracao + + float pose[3] = {30, 15, 0}; // Ponto Inicial + float pose_f[3] = {60,100,0}; // Ponto Objetivo + + float errot = 0, erro = 0, erro_old = -1; + float Forcas[4] = {0, 0, 0, 0}; + + float vRobot, wRobot, Norma, angForca, phi, w[2], ForcaResult[2], PIntermedio[2]; + + float LidarP[2]; // pontos na plataforma + float LidarW[2]; // pontos no mundo + + // matriz rotacao world plataforma + float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]}, + {sin(pose[2]), cos(pose[2]), pose[1]}, + {0, 0, 1}}; + struct RPLidarMeasurement data; //Inicializar Mapa das probabilidades a 0.5 @@ -41,36 +67,19 @@ for(int j=0; j<40; j++) Mapa40[i][j]=0.5; + int leituras = 0; long dist; + // Lidar initialization rplidar_motor.period(0.001f); //rplidar_motor.write(0.5f); lidar.begin(se_lidar); lidar.setAngle(0, 360); - - 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++){ - for(int j = 0; j < 40; j++){ - MapaLog[i][j] = 0; - } - }*/ - - - // matriz rotacao world plataforma - float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]}, - {sin(pose[2]), cos(pose[2]), pose[1]}, - {0, 0, 1}}; - float dist; + setSpeeds(0, 0); - int leituras = 0; - pc.printf("waiting...\n\r"); - + int start = 0; while(start != 1) { myled=1; @@ -83,7 +92,6 @@ lidar.startThreadScan(); - pc.printf("Entrar no ciclo\n\r"); while(leituras < 1000){ if(lidar.pollSensorData(&data) == 0) { @@ -91,7 +99,7 @@ float radians = (data.angle * static_cast<float>(PI))/180.0f; - dist = static_cast<float>(data.distance/10.0f); //converte de mm para cm + dist = data.distance/10.0f; LidarP[0] = -dist*sin(radians)- 2.8f; LidarP[1] = -dist*cos(radians)- 1.5f; @@ -100,43 +108,149 @@ 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, dist/5); + bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5); 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 "); + } + pc.printf("nova linha \n\r"); + } + + start = 0; + while(start != 1) { + myled=1; + if (UserButton == 0) { // Button is pressed + myled = 0; + start = 1; + rplidar_motor.write(0.5f); + } } - // Converter o logaritmo para o mapa 40 + 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; - pc.printf("\nFIM DO CICLO\n========================\n\n\r"); + // 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); - 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 - wait(0.1); - } - //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); int y = static_cast<int>(poseY); - int dx = static_cast<int>(abs(xf - poseX)); - int dy = static_cast<int>(abs(yf - poseY)); + int dx = abs(static_cast<int>(xf - poseX)); + int dy = abs(static_cast<int>(yf - poseY)); - int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab - int s2 = static_cast<int>((yf - poseY)/dy); + 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) + s1 = 0; + else + s1 = -1; + + if(static_cast<int>(yf - poseY) > 0) + s2 = 1; + else if (static_cast<int>(yf - poseY) == 0) + s2 = 0; + else + s2 = -1; + int interchange = 0; @@ -170,7 +284,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); + MapaLog[x][y] = MapaLog[x][y] + Algorithm_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])); }