main.cpp

00001 #include "mbed.h"
00002 #include "BufferedSerial.h"
00003 #include "rplidar.h"
00004 #include "Robot.h "
00005 #include "Communication.h"
00006 #include "Functions.h"
00007 #include <math.h>
00008 #include <stdio.h>
00009 #include <stdlib.h>
00010 
00011 #define PI 3.141592654
00012 
00013 Serial pc(SERIAL_TX, SERIAL_RX);
00014 RPLidar lidar;
00015 BufferedSerial se_lidar(PA_9, PA_10);
00016 PwmOut rplidar_motor(D3);
00017 
00018 void bresenham(float poseX, float poseY, float xf, float yf, float z);
00019 
00020 float MapaLog[40][40] = {0};
00021 float Mapa40[40][40];
00022 
00023 int main() {
00024     pc.baud(115200);
00025     init_communication(&pc);
00026     
00027     DigitalIn UserButton(USER_BUTTON); // Initialize Button
00028     DigitalOut myled(LED1); // Initialize LED
00029     
00030     // PARAMETROS CONSTANTES
00031     float T = 0.5;
00032     float wheelsRadius = 3.5;
00033     float wheelsDistance = 13.5;
00034     
00035     // Variaveis
00036     float k_v = 15; // 15
00037     float k_i = 0.3; //0.1
00038     float k_s = 11; //20
00039     float v_erro = 15;
00040     
00041     int   DPontos = 2; //distância de erro
00042     
00043     float RectSize = 4.0;
00044     float FCRepul = -1000; // Forca de Repulsao
00045     float FCAtracao = 5; // Forca de Atracao
00046     
00047     float pose[3] = {30, 15, 0}; // Ponto Inicial
00048     float pose_f[3] = {60,100,0}; // Ponto Objetivo
00049     
00050     float errot = 0, erro = 0, erro_old = -1;
00051     float Forcas[4] = {0, 0, 0, 0};
00052     
00053     float vRobot, wRobot, Norma, angForca, phi, w[2], ForcaResult[2], PIntermedio[2];
00054     
00055     float LidarP[2]; // pontos na plataforma
00056     float LidarW[2]; // pontos no mundo
00057     
00058     // matriz rotacao world plataforma
00059     float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
00060                         {sin(pose[2]), cos(pose[2]), pose[1]},
00061                         {0, 0, 1}};
00062     
00063     struct RPLidarMeasurement data;
00064     
00065     //Inicializar Mapa das probabilidades a 0.5
00066     for(int i=0; i<40; i++)
00067         for(int j=0; j<40; j++)
00068             Mapa40[i][j]=0.5;
00069     
00070     int leituras = 0; long dist;
00071     
00072     // Lidar initialization
00073     rplidar_motor.period(0.001f);
00074     //rplidar_motor.write(0.5f);
00075     lidar.begin(se_lidar);
00076     lidar.setAngle(0, 360);
00077             
00078     
00079     setSpeeds(0, 0);
00080     
00081     pc.printf("waiting...\n\r");
00082 
00083     int start = 0;
00084     while(start != 1) {
00085         myled=1;
00086         if (UserButton == 0) { // Button is pressed
00087             myled = 0;
00088             start = 1;
00089             rplidar_motor.write(0.5f);
00090         }
00091     }
00092     
00093     lidar.startThreadScan();
00094     
00095     while(leituras < 1000){
00096         if(lidar.pollSensorData(&data) == 0)
00097         {           
00098             //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
00099             
00100             float radians = (data.angle * static_cast<float>(PI))/180.0f;
00101             
00102             dist = data.distance/10.0f;
00103             
00104             LidarP[0] = -dist*sin(radians)- 2.8f;
00105             LidarP[1] = -dist*cos(radians)- 1.5f;
00106 
00107             //W_P = R_WP * p_P
00108             LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm
00109             LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
00110             
00111             // pontos onde o feixe passou
00112             bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5);
00113             
00114             leituras++;
00115         }
00116     }
00117     
00118     for(int f = 0; f < 40; f++){
00119         for(int g = 0; g < 40; g++){
00120             if(Mapa40[f][g] < 0.65f)
00121                 pc.printf("0 ");
00122             else
00123                 pc.printf("1 ");
00124         }
00125         pc.printf("nova linha \n\r");
00126     }
00127     
00128     start = 0;
00129     while(start != 1) {
00130         myled=1;
00131         if (UserButton == 0) { // Button is pressed
00132             myled = 0;
00133             start = 1;
00134             rplidar_motor.write(0.5f);
00135         }
00136     }
00137     
00138     pc.printf("Running\n\r");
00139 
00140     while(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 5.0f) {
00141           
00142         getCountsAndReset(); // Call getCountsAndReset() to read the values of encoders
00143                              // and reset them. The values become available on variables
00144                              // "countsLeft" and "countsRight".
00145         
00146         //if(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 13){
00147             
00148             pc.printf("x; %f, y: %f\n\r", pose[0], pose[1]);
00149             
00150             JanelaAtivaVFF(pose, Mapa40, pose_f, FCRepul, FCAtracao, RectSize, Forcas);
00151             
00152             ForcaResult[0] = Forcas[0]+Forcas[2]; // componente de x
00153             ForcaResult[1] = Forcas[1]+Forcas[3]; // componente de y
00154     
00155             Norma = sqrt(pow(ForcaResult[0],2) + pow(ForcaResult[1],2));
00156             ForcaResult[0] = ForcaResult[0]/Norma; // Normalização da forca resultante entre a forca de repulsão e a forca de atracão
00157             ForcaResult[1] = ForcaResult[1]/Norma;
00158     
00159             // Angulo da forca resultante
00160             angForca = atan2(ForcaResult[1], ForcaResult[0]);
00161         //}
00162         //else
00163             //angForca = atan2(pose_f[1] - pose[1], pose_f[0] - pose[0]);
00164         
00165         
00166         PIntermedio[0] = pose[0] + v_erro * cos(angForca);
00167         PIntermedio[1] = pose[1] + v_erro * sin(angForca);
00168         
00169         erro = sqrt(pow(PIntermedio[1]-pose[1], 2)+ pow(PIntermedio[0]-pose[0],2)) - DPontos;
00170         
00171         if(erro_old == -1){
00172             erro_old = erro;    
00173         }
00174         
00175         errot = errot + (erro + erro_old) * T/2.0f;
00176         
00177         //Velocidade do Robo
00178         vRobot = k_v * erro + k_i * errot;
00179         
00180         erro_old = erro;
00181         
00182         phi = atan2((PIntermedio[1]-pose[1]), (PIntermedio[0]-pose[0])); // Angulo entre o ponto intermedio e o robo
00183         
00184         wRobot = k_s * atan2(sin(phi-pose[2]), cos(phi-pose[2])); //velocidade angular do robo;
00185               
00186         // Velocidades das rodas
00187         velRobot2velWheels(vRobot,wRobot,wheelsRadius,wheelsDistance,w);
00188         
00189         if (w[0] > 200)
00190             w[0]=200;
00191             
00192         if (w[1] > 200)
00193             w[1]=200;
00194         
00195         setSpeeds(w[0], w[1]);
00196         
00197         nextPose(countsLeft, countsRight, wheelsRadius, wheelsDistance, pose);
00198         
00199         leituras = 0;
00200         
00201         while(leituras < 5){
00202             if(lidar.pollSensorData(&data) == 0)
00203             {           
00204                 //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
00205                 
00206                 float radians = (data.angle * static_cast<float>(PI))/180.0f;
00207                 
00208                 LidarP[0] = -data.distance*sin(radians)- 2.8f;
00209                 LidarP[1] = -data.distance*cos(radians)- 1.5f;
00210     
00211                 //W_P = R_WP * p_P
00212                 LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm
00213                 LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
00214                 
00215                 // pontos onde o feixe passou
00216                 bresenham(pose[0]/5.0f, pose[1]/5.0f, LidarW[0]/5.0f, LidarW[1]/5.0f, data.distance/5.0f);
00217                 
00218                 leituras++;
00219             }
00220         }
00221             
00222         wait(T); // Delay of 0.5 seconds.
00223     }
00224     myled=1;
00225     setSpeeds(0, 0);
00226     rplidar_motor.write(0.0f);
00227 }
00228 
00229 void bresenham(float poseX, float poseY, float xf, float yf, float z){
00230     int T, E, A, B;
00231     
00232     int x = static_cast<int>(poseX);
00233     int y = static_cast<int>(poseY);
00234     int dx = abs(static_cast<int>(xf - poseX));
00235     int dy = abs(static_cast<int>(yf - poseY));
00236     
00237     int s1;
00238     int s2;
00239     // substitui o sign() do matlab
00240     if(static_cast<int>(xf - poseX) > 0)
00241         s1 = 1;
00242     else if (static_cast<int>(xf - poseX) == 0)
00243         s1 = 0;
00244     else
00245         s1 = -1;
00246         
00247     if(static_cast<int>(yf - poseY) > 0)
00248         s2 = 1;
00249     else if (static_cast<int>(yf - poseY) == 0)
00250         s2 = 0;
00251     else
00252         s2 = -1;
00253  
00254     
00255     int interchange = 0;
00256     
00257     if (dy > dx){
00258         T = dx;
00259         dx = dy;
00260         dy = T;
00261         interchange = 1;
00262     }
00263     
00264     E = 2*dy - dx;
00265     A = 2*dy;
00266     B = 2*dy - 2*dx;
00267     
00268     for (int i = 0; i<dx; i++){
00269         if (E < 0){
00270             if (interchange == 1){
00271                 y = y + s2;
00272             }
00273             else{
00274                 x = x + s1;
00275             }
00276             E = E + A;
00277         }
00278         
00279         else{
00280             y = y + s2;
00281             x = x + s1;
00282             E = E + B;
00283         }
00284         
00285         if (x >= 0 && y >= 0 && x < 40 && y < 40){
00286             // Mapear mapa do Logaritmo
00287             MapaLog[x][y] = MapaLog[x][y] + Algorithm_Inverse(poseX, poseY, x, y, z);
00288             //pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
00289             Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
00290         }
00291         
00292         
00293                   
00294     }
00295 }