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