Revision 12:348038b466a3, committed 2021-05-14

Comitter:

henkiwan

Date:

Fri May 14 05:52:55 2021 +0000

Parent:

11:58187c7de709

Child:

13:20e124fba426

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]));
         }