SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Experiencias do Henrique na quinta/sexta a noite

Dependencies:   BufferedSerial

main.cpp@11:58187c7de709, 2021-05-13 (annotated)

Committer:
ppovoa
Date:
Thu May 13 16:28:09 2021 +0000
Revision:
11:58187c7de709
Parent:
10:6c8ea68e9bac
Child:
12:348038b466a3
Lidar com Matlab funcional

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabiofaria 1:dc87724abce8 1 // Coded by Luís Afonso 11-04-2019
LuisRA 0:2b691d200d6f 2 #include "mbed.h"
LuisRA 0:2b691d200d6f 3 #include "BufferedSerial.h"
LuisRA 0:2b691d200d6f 4 #include "rplidar.h"
fabiofaria 1:dc87724abce8 5 #include "Robot.h"
fabiofaria 1:dc87724abce8 6 #include "Communication.h"
ppovoa 4:256f2cbe3fdd 7 #include "Functions.h"
ppovoa 5:bc42c03f2a23 8 #include "math.h"
ppovoa 4:256f2cbe3fdd 9
ppovoa 4:256f2cbe3fdd 10 #include <stdlib.h>
ppovoa 4:256f2cbe3fdd 11 #include <stdio.h>
LuisRA 0:2b691d200d6f 12
ppovoa 5:bc42c03f2a23 13 #define PI 3.1415926535
ppovoa 5:bc42c03f2a23 14
fabiofaria 1:dc87724abce8 15 Serial pc(SERIAL_TX, SERIAL_RX);
LuisRA 0:2b691d200d6f 16 RPLidar lidar;
LuisRA 0:2b691d200d6f 17 BufferedSerial se_lidar(PA_9, PA_10);
fabiofaria 3:0a718d139ed1 18 PwmOut rplidar_motor(D3);
LuisRA 0:2b691d200d6f 19
ppovoa 8:ad8766cf2ec0 20 float MapaLog[40][40] = {0};
ppovoa 10:6c8ea68e9bac 21 float Mapa40[40][40];
henkiwan 9:76b59c5220f1 22
henkiwan 9:76b59c5220f1 23 void bresenham(float poseX, float poseY, float xf, float yf, float z);
ppovoa 8:ad8766cf2ec0 24
LuisRA 0:2b691d200d6f 25 int main()
LuisRA 0:2b691d200d6f 26 {
fabiofaria 1:dc87724abce8 27 pc.baud(115200);
fabiofaria 1:dc87724abce8 28 init_communication(&pc);
ppovoa 5:bc42c03f2a23 29
ppovoa 7:f1c122bc63c8 30 pc.printf("======================\n\r");
ppovoa 6:59fbbeaac2af 31 pc.printf("Inicio\n\r");
ppovoa 6:59fbbeaac2af 32
ppovoa 5:bc42c03f2a23 33 DigitalIn UserButton(USER_BUTTON); // Initialize Button
ppovoa 5:bc42c03f2a23 34 DigitalOut myled(LED1); // Initialize LED
ppovoa 5:bc42c03f2a23 35
ppovoa 5:bc42c03f2a23 36 //float odomX, odomY, odomTheta;
ppovoa 5:bc42c03f2a23 37 struct RPLidarMeasurement data;
ppovoa 4:256f2cbe3fdd 38
ppovoa 10:6c8ea68e9bac 39 //Inicializar Mapa das probabilidades a 0.5
ppovoa 10:6c8ea68e9bac 40 for(int i=0; i<40; i++)
ppovoa 10:6c8ea68e9bac 41 for(int j=0; j<40; j++)
ppovoa 10:6c8ea68e9bac 42 Mapa40[i][j]=0.5;
ppovoa 10:6c8ea68e9bac 43
fabiofaria 1:dc87724abce8 44 // Lidar initialization
LuisRA 0:2b691d200d6f 45 rplidar_motor.period(0.001f);
ppovoa 5:bc42c03f2a23 46 //rplidar_motor.write(0.5f);
LuisRA 0:2b691d200d6f 47 lidar.begin(se_lidar);
henkiwan 9:76b59c5220f1 48 lidar.setAngle(0, 360);
ppovoa 4:256f2cbe3fdd 49
ppovoa 10:6c8ea68e9bac 50 float pose[3] = {20, 20, 0}; // Ponto Inicial
ppovoa 10:6c8ea68e9bac 51 float LidarP[2]; // pontos na plataforma
ppovoa 10:6c8ea68e9bac 52 float LidarW[2]; // pontos no mundo
ppovoa 8:ad8766cf2ec0 53
ppovoa 8:ad8766cf2ec0 54 /*pc.printf("Inicializacao MapaLog\n\r");
ppovoa 7:f1c122bc63c8 55 for(int i = 0; i < 40; i++){
ppovoa 7:f1c122bc63c8 56 for(int j = 0; j < 40; j++){
ppovoa 5:bc42c03f2a23 57 MapaLog[i][j] = 0;
ppovoa 7:f1c122bc63c8 58 }
ppovoa 7:f1c122bc63c8 59 }*/
ppovoa 8:ad8766cf2ec0 60
ppovoa 5:bc42c03f2a23 61
ppovoa 8:ad8766cf2ec0 62 // matriz rotacao world plataforma
ppovoa 10:6c8ea68e9bac 63 float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
ppovoa 10:6c8ea68e9bac 64 {sin(pose[2]), cos(pose[2]), pose[1]},
ppovoa 4:256f2cbe3fdd 65 {0, 0, 1}};
ppovoa 11:58187c7de709 66 float dist;
ppovoa 4:256f2cbe3fdd 67
henkiwan 9:76b59c5220f1 68 setSpeeds(0, 0);
ppovoa 5:bc42c03f2a23 69
ppovoa 5:bc42c03f2a23 70 int leituras = 0;
ppovoa 5:bc42c03f2a23 71
ppovoa 5:bc42c03f2a23 72 pc.printf("waiting...\n\r");
ppovoa 5:bc42c03f2a23 73
ppovoa 5:bc42c03f2a23 74 int start = 0;
ppovoa 5:bc42c03f2a23 75 while(start != 1) {
ppovoa 5:bc42c03f2a23 76 myled=1;
ppovoa 5:bc42c03f2a23 77 if (UserButton == 0) { // Button is pressed
ppovoa 5:bc42c03f2a23 78 myled = 0;
ppovoa 5:bc42c03f2a23 79 start = 1;
ppovoa 5:bc42c03f2a23 80 rplidar_motor.write(0.5f);
ppovoa 5:bc42c03f2a23 81 }
ppovoa 5:bc42c03f2a23 82 }
ppovoa 5:bc42c03f2a23 83
ppovoa 7:f1c122bc63c8 84 lidar.startThreadScan();
ppovoa 4:256f2cbe3fdd 85
ppovoa 8:ad8766cf2ec0 86 pc.printf("Entrar no ciclo\n\r");
ppovoa 10:6c8ea68e9bac 87 while(leituras < 1000){
ppovoa 5:bc42c03f2a23 88 if(lidar.pollSensorData(&data) == 0)
ppovoa 10:6c8ea68e9bac 89 {
henkiwan 9:76b59c5220f1 90 //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
ppovoa 4:256f2cbe3fdd 91
ppovoa 6:59fbbeaac2af 92 float radians = (data.angle * static_cast<float>(PI))/180.0f;
ppovoa 4:256f2cbe3fdd 93
ppovoa 11:58187c7de709 94 dist = static_cast<float>(data.distance/10.0f); //converte de mm para cm
ppovoa 11:58187c7de709 95
ppovoa 11:58187c7de709 96 LidarP[0] = -dist*sin(radians)- 2.8f;
ppovoa 11:58187c7de709 97 LidarP[1] = -dist*cos(radians)- 1.5f;
LuisRA 0:2b691d200d6f 98
ppovoa 4:256f2cbe3fdd 99 //W_P = R_WP * p_P
ppovoa 4:256f2cbe3fdd 100 LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm
ppovoa 4:256f2cbe3fdd 101 LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
ppovoa 4:256f2cbe3fdd 102
ppovoa 11:58187c7de709 103 /*if (data.angle > 0 && data.angle < 5){
ppovoa 11:58187c7de709 104 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]);
ppovoa 11:58187c7de709 105 }*/
ppovoa 4:256f2cbe3fdd 106 // pontos onde o feixe passou
ppovoa 11:58187c7de709 107 bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, dist/5);
ppovoa 4:256f2cbe3fdd 108
ppovoa 5:bc42c03f2a23 109 leituras++;
ppovoa 5:bc42c03f2a23 110 }
ppovoa 11:58187c7de709 111 wait(0.01);
ppovoa 5:bc42c03f2a23 112 }
ppovoa 4:256f2cbe3fdd 113
ppovoa 4:256f2cbe3fdd 114 // Converter o logaritmo para o mapa 40
ppovoa 6:59fbbeaac2af 115
henkiwan 9:76b59c5220f1 116 pc.printf("\nFIM DO CICLO\n========================\n\n\r");
henkiwan 9:76b59c5220f1 117
ppovoa 8:ad8766cf2ec0 118 rplidar_motor.write(0.0f);
ppovoa 4:256f2cbe3fdd 119 for(int i=0; i<40; i++){
ppovoa 4:256f2cbe3fdd 120 for(int j=0; j<40; j++){
ppovoa 10:6c8ea68e9bac 121 //pc.printf("%0.3f ", Mapa40[i][j]);
ppovoa 10:6c8ea68e9bac 122 send_odometry(1, 2, j+1, i+1, Mapa40[j][i],10, 30); // faz prints estranhos no Putty
ppovoa 10:6c8ea68e9bac 123 wait(0.1);
ppovoa 4:256f2cbe3fdd 124 }
ppovoa 10:6c8ea68e9bac 125 //pc.printf("\n\r");
ppovoa 10:6c8ea68e9bac 126 //pc.printf("\n-----------------------------\n\r");
henkiwan 9:76b59c5220f1 127 }
henkiwan 9:76b59c5220f1 128 }
henkiwan 9:76b59c5220f1 129
henkiwan 9:76b59c5220f1 130
henkiwan 9:76b59c5220f1 131 void bresenham(float poseX, float poseY, float xf, float yf, float z){
henkiwan 9:76b59c5220f1 132 int T, E, A, B;
henkiwan 9:76b59c5220f1 133 int x = static_cast<int>(poseX);
henkiwan 9:76b59c5220f1 134 int y = static_cast<int>(poseY);
henkiwan 9:76b59c5220f1 135 int dx = static_cast<int>(abs(xf - poseX));
henkiwan 9:76b59c5220f1 136 int dy = static_cast<int>(abs(yf - poseY));
henkiwan 9:76b59c5220f1 137
henkiwan 9:76b59c5220f1 138 int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
henkiwan 9:76b59c5220f1 139 int s2 = static_cast<int>((yf - poseY)/dy);
henkiwan 9:76b59c5220f1 140
henkiwan 9:76b59c5220f1 141 int interchange = 0;
henkiwan 9:76b59c5220f1 142
henkiwan 9:76b59c5220f1 143 if (dy > dx){
henkiwan 9:76b59c5220f1 144 T = dx;
henkiwan 9:76b59c5220f1 145 dx = dy;
henkiwan 9:76b59c5220f1 146 dy = T;
henkiwan 9:76b59c5220f1 147 interchange = 1;
henkiwan 9:76b59c5220f1 148 }
henkiwan 9:76b59c5220f1 149
henkiwan 9:76b59c5220f1 150 E = 2*dy - dx;
henkiwan 9:76b59c5220f1 151 A = 2*dy;
henkiwan 9:76b59c5220f1 152 B = 2*dy - 2*dx;
henkiwan 9:76b59c5220f1 153
henkiwan 9:76b59c5220f1 154 for (int i = 0; i<dx; i++){
henkiwan 9:76b59c5220f1 155 if (E < 0){
henkiwan 9:76b59c5220f1 156 if (interchange == 1){
henkiwan 9:76b59c5220f1 157 y = y + s2;
henkiwan 9:76b59c5220f1 158 }
henkiwan 9:76b59c5220f1 159 else{
henkiwan 9:76b59c5220f1 160 x = x + s1;
henkiwan 9:76b59c5220f1 161 }
henkiwan 9:76b59c5220f1 162 E = E + A;
henkiwan 9:76b59c5220f1 163 }
henkiwan 9:76b59c5220f1 164
henkiwan 9:76b59c5220f1 165 else{
henkiwan 9:76b59c5220f1 166 y = y + s2;
henkiwan 9:76b59c5220f1 167 x = x + s1;
henkiwan 9:76b59c5220f1 168 E = E + B;
henkiwan 9:76b59c5220f1 169 }
henkiwan 9:76b59c5220f1 170
henkiwan 9:76b59c5220f1 171 if (x >= 0 && y >= 0 && x < 40 && y < 40){
henkiwan 9:76b59c5220f1 172 // Mapear mapa do Logaritmo
henkiwan 9:76b59c5220f1 173 MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
ppovoa 10:6c8ea68e9bac 174 //pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
henkiwan 9:76b59c5220f1 175 Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
henkiwan 9:76b59c5220f1 176 }
henkiwan 9:76b59c5220f1 177
henkiwan 9:76b59c5220f1 178
henkiwan 9:76b59c5220f1 179
ppovoa 4:256f2cbe3fdd 180 }
fabiofaria 1:dc87724abce8 181 }