Experiencias do Henrique na quinta/sexta a noite
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