Henrique Cardoso / Mbed OS Lidar_Rodas

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

main.cpp@10:6c8ea68e9bac, 2021-05-12 (annotated)

Committer:
ppovoa
Date:
Wed May 12 19:00:45 2021 +0000
Revision:
10:6c8ea68e9bac
Parent:
9:76b59c5220f1
Child:
11:58187c7de709
Lidar

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 {
ppovoa 6:59fbbeaac2af 27 //printf("Inicio\n\r");
fabiofaria 1:dc87724abce8 28 pc.baud(115200);
fabiofaria 1:dc87724abce8 29 init_communication(&pc);
ppovoa 5:bc42c03f2a23 30
ppovoa 7:f1c122bc63c8 31 pc.printf("======================\n\r");
ppovoa 6:59fbbeaac2af 32 pc.printf("Inicio\n\r");
ppovoa 6:59fbbeaac2af 33
ppovoa 5:bc42c03f2a23 34 DigitalIn UserButton(USER_BUTTON); // Initialize Button
ppovoa 5:bc42c03f2a23 35 DigitalOut myled(LED1); // Initialize LED
ppovoa 5:bc42c03f2a23 36
ppovoa 5:bc42c03f2a23 37 //float odomX, odomY, odomTheta;
ppovoa 5:bc42c03f2a23 38 struct RPLidarMeasurement data;
ppovoa 4:256f2cbe3fdd 39
ppovoa 10:6c8ea68e9bac 40 //Inicializar Mapa das probabilidades a 0.5
ppovoa 10:6c8ea68e9bac 41 for(int i=0; i<40; i++)
ppovoa 10:6c8ea68e9bac 42 for(int j=0; j<40; j++)
ppovoa 10:6c8ea68e9bac 43 Mapa40[i][j]=0.5;
ppovoa 10:6c8ea68e9bac 44
fabiofaria 1:dc87724abce8 45 // Lidar initialization
LuisRA 0:2b691d200d6f 46 rplidar_motor.period(0.001f);
ppovoa 5:bc42c03f2a23 47 //rplidar_motor.write(0.5f);
LuisRA 0:2b691d200d6f 48 lidar.begin(se_lidar);
henkiwan 9:76b59c5220f1 49 lidar.setAngle(0, 360);
ppovoa 4:256f2cbe3fdd 50
ppovoa 10:6c8ea68e9bac 51 float pose[3] = {20, 20, 0}; // Ponto Inicial
ppovoa 10:6c8ea68e9bac 52 float LidarP[2]; // pontos na plataforma
ppovoa 10:6c8ea68e9bac 53 float LidarW[2]; // pontos no mundo
ppovoa 8:ad8766cf2ec0 54
ppovoa 8:ad8766cf2ec0 55 /*pc.printf("Inicializacao MapaLog\n\r");
ppovoa 7:f1c122bc63c8 56 for(int i = 0; i < 40; i++){
ppovoa 7:f1c122bc63c8 57 for(int j = 0; j < 40; j++){
ppovoa 5:bc42c03f2a23 58 MapaLog[i][j] = 0;
ppovoa 7:f1c122bc63c8 59 }
ppovoa 7:f1c122bc63c8 60 }*/
ppovoa 8:ad8766cf2ec0 61
ppovoa 5:bc42c03f2a23 62
ppovoa 8:ad8766cf2ec0 63 // matriz rotacao world plataforma
ppovoa 10:6c8ea68e9bac 64 float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
ppovoa 10:6c8ea68e9bac 65 {sin(pose[2]), cos(pose[2]), pose[1]},
ppovoa 4:256f2cbe3fdd 66 {0, 0, 1}};
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 10:6c8ea68e9bac 94 LidarP[0] = -data.distance*sin(radians)- 2.8f;
ppovoa 10:6c8ea68e9bac 95 LidarP[1] = -data.distance*cos(radians)- 1.5f;
LuisRA 0:2b691d200d6f 96
ppovoa 4:256f2cbe3fdd 97 //W_P = R_WP * p_P
ppovoa 4:256f2cbe3fdd 98 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 99 LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
ppovoa 4:256f2cbe3fdd 100
ppovoa 4:256f2cbe3fdd 101 // pontos onde o feixe passou
ppovoa 10:6c8ea68e9bac 102 bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5);
ppovoa 4:256f2cbe3fdd 103
ppovoa 5:bc42c03f2a23 104 leituras++;
ppovoa 5:bc42c03f2a23 105 }
ppovoa 5:bc42c03f2a23 106 }
ppovoa 4:256f2cbe3fdd 107
ppovoa 4:256f2cbe3fdd 108 // Converter o logaritmo para o mapa 40
ppovoa 6:59fbbeaac2af 109
henkiwan 9:76b59c5220f1 110 pc.printf("\nFIM DO CICLO\n========================\n\n\r");
henkiwan 9:76b59c5220f1 111
ppovoa 8:ad8766cf2ec0 112 rplidar_motor.write(0.0f);
ppovoa 4:256f2cbe3fdd 113 for(int i=0; i<40; i++){
ppovoa 4:256f2cbe3fdd 114 for(int j=0; j<40; j++){
ppovoa 10:6c8ea68e9bac 115 //pc.printf("%0.3f ", Mapa40[i][j]);
ppovoa 10:6c8ea68e9bac 116 send_odometry(1, 2, j+1, i+1, Mapa40[j][i],10, 30); // faz prints estranhos no Putty
ppovoa 10:6c8ea68e9bac 117 wait(0.1);
ppovoa 4:256f2cbe3fdd 118 }
ppovoa 10:6c8ea68e9bac 119 //pc.printf("\n\r");
ppovoa 10:6c8ea68e9bac 120 //pc.printf("\n-----------------------------\n\r");
henkiwan 9:76b59c5220f1 121 }
henkiwan 9:76b59c5220f1 122 }
henkiwan 9:76b59c5220f1 123
henkiwan 9:76b59c5220f1 124
henkiwan 9:76b59c5220f1 125 void bresenham(float poseX, float poseY, float xf, float yf, float z){
henkiwan 9:76b59c5220f1 126 int T, E, A, B;
henkiwan 9:76b59c5220f1 127 int x = static_cast<int>(poseX);
henkiwan 9:76b59c5220f1 128 int y = static_cast<int>(poseY);
henkiwan 9:76b59c5220f1 129 int dx = static_cast<int>(abs(xf - poseX));
henkiwan 9:76b59c5220f1 130 int dy = static_cast<int>(abs(yf - poseY));
henkiwan 9:76b59c5220f1 131
henkiwan 9:76b59c5220f1 132 int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
henkiwan 9:76b59c5220f1 133 int s2 = static_cast<int>((yf - poseY)/dy);
henkiwan 9:76b59c5220f1 134
henkiwan 9:76b59c5220f1 135 int interchange = 0;
henkiwan 9:76b59c5220f1 136
henkiwan 9:76b59c5220f1 137 if (dy > dx){
henkiwan 9:76b59c5220f1 138 T = dx;
henkiwan 9:76b59c5220f1 139 dx = dy;
henkiwan 9:76b59c5220f1 140 dy = T;
henkiwan 9:76b59c5220f1 141 interchange = 1;
henkiwan 9:76b59c5220f1 142 }
henkiwan 9:76b59c5220f1 143
henkiwan 9:76b59c5220f1 144 E = 2*dy - dx;
henkiwan 9:76b59c5220f1 145 A = 2*dy;
henkiwan 9:76b59c5220f1 146 B = 2*dy - 2*dx;
henkiwan 9:76b59c5220f1 147
henkiwan 9:76b59c5220f1 148 for (int i = 0; i<dx; i++){
henkiwan 9:76b59c5220f1 149 if (E < 0){
henkiwan 9:76b59c5220f1 150 if (interchange == 1){
henkiwan 9:76b59c5220f1 151 y = y + s2;
henkiwan 9:76b59c5220f1 152 }
henkiwan 9:76b59c5220f1 153 else{
henkiwan 9:76b59c5220f1 154 x = x + s1;
henkiwan 9:76b59c5220f1 155 }
henkiwan 9:76b59c5220f1 156 E = E + A;
henkiwan 9:76b59c5220f1 157 }
henkiwan 9:76b59c5220f1 158
henkiwan 9:76b59c5220f1 159 else{
henkiwan 9:76b59c5220f1 160 y = y + s2;
henkiwan 9:76b59c5220f1 161 x = x + s1;
henkiwan 9:76b59c5220f1 162 E = E + B;
henkiwan 9:76b59c5220f1 163 }
henkiwan 9:76b59c5220f1 164
henkiwan 9:76b59c5220f1 165 if (x >= 0 && y >= 0 && x < 40 && y < 40){
henkiwan 9:76b59c5220f1 166 // Mapear mapa do Logaritmo
henkiwan 9:76b59c5220f1 167 MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
ppovoa 10:6c8ea68e9bac 168 //pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
henkiwan 9:76b59c5220f1 169 Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
henkiwan 9:76b59c5220f1 170 }
henkiwan 9:76b59c5220f1 171
henkiwan 9:76b59c5220f1 172
henkiwan 9:76b59c5220f1 173
ppovoa 4:256f2cbe3fdd 174 }
fabiofaria 1:dc87724abce8 175 }