SRA_HenriquePovoa / 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@9:76b59c5220f1, 2021-05-11 (annotated)

Committer:
henkiwan
Date:
Tue May 11 17:53:17 2021 +0000
Revision:
9:76b59c5220f1
Parent:
8:ad8766cf2ec0
Child:
10:6c8ea68e9bac
yah; ; temos de ver o log;

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};
henkiwan 9:76b59c5220f1 21 float Mapa40[40][40] = {0.5};
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
fabiofaria 1:dc87724abce8 40 // Lidar initialization
LuisRA 0:2b691d200d6f 41 rplidar_motor.period(0.001f);
ppovoa 5:bc42c03f2a23 42 //rplidar_motor.write(0.5f);
LuisRA 0:2b691d200d6f 43 lidar.begin(se_lidar);
henkiwan 9:76b59c5220f1 44 lidar.setAngle(0, 360);
ppovoa 4:256f2cbe3fdd 45
henkiwan 9:76b59c5220f1 46 int pose[3] = {20, 20}; // Ponto Inicial
ppovoa 7:f1c122bc63c8 47 float p_angulo = 0;
ppovoa 7:f1c122bc63c8 48 int LidarP[2]; // pontos na plataforma
ppovoa 7:f1c122bc63c8 49 int LidarW[2]; // pontos no mundo
ppovoa 8:ad8766cf2ec0 50
ppovoa 8:ad8766cf2ec0 51 /*pc.printf("Inicializacao MapaLog\n\r");
ppovoa 7:f1c122bc63c8 52 for(int i = 0; i < 40; i++){
ppovoa 7:f1c122bc63c8 53 for(int j = 0; j < 40; j++){
ppovoa 5:bc42c03f2a23 54 MapaLog[i][j] = 0;
ppovoa 7:f1c122bc63c8 55 }
ppovoa 7:f1c122bc63c8 56 }*/
ppovoa 8:ad8766cf2ec0 57
ppovoa 5:bc42c03f2a23 58
ppovoa 8:ad8766cf2ec0 59 // matriz rotacao world plataforma
ppovoa 7:f1c122bc63c8 60 float R_WP[3][3]= {{cos(p_angulo), -sin(p_angulo), pose[0]},
ppovoa 7:f1c122bc63c8 61 {sin(p_angulo), cos(p_angulo), pose[1]},
ppovoa 4:256f2cbe3fdd 62 {0, 0, 1}};
ppovoa 4:256f2cbe3fdd 63
henkiwan 9:76b59c5220f1 64 setSpeeds(0, 0);
ppovoa 5:bc42c03f2a23 65
ppovoa 5:bc42c03f2a23 66 int leituras = 0;
ppovoa 5:bc42c03f2a23 67
ppovoa 5:bc42c03f2a23 68 pc.printf("waiting...\n\r");
ppovoa 5:bc42c03f2a23 69
ppovoa 5:bc42c03f2a23 70 int start = 0;
ppovoa 5:bc42c03f2a23 71 while(start != 1) {
ppovoa 5:bc42c03f2a23 72 myled=1;
ppovoa 5:bc42c03f2a23 73 if (UserButton == 0) { // Button is pressed
ppovoa 5:bc42c03f2a23 74 myled = 0;
ppovoa 5:bc42c03f2a23 75 start = 1;
ppovoa 5:bc42c03f2a23 76 rplidar_motor.write(0.5f);
ppovoa 5:bc42c03f2a23 77 }
ppovoa 5:bc42c03f2a23 78 }
ppovoa 5:bc42c03f2a23 79
ppovoa 7:f1c122bc63c8 80 lidar.startThreadScan();
ppovoa 4:256f2cbe3fdd 81
ppovoa 8:ad8766cf2ec0 82 pc.printf("Entrar no ciclo\n\r");
ppovoa 8:ad8766cf2ec0 83 while(leituras < 500){
ppovoa 5:bc42c03f2a23 84 if(lidar.pollSensorData(&data) == 0)
ppovoa 8:ad8766cf2ec0 85 {
ppovoa 8:ad8766cf2ec0 86 /*if (UserButton == 0) { // Button is pressed
ppovoa 5:bc42c03f2a23 87 break;
ppovoa 8:ad8766cf2ec0 88 }*/
ppovoa 8:ad8766cf2ec0 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 5:bc42c03f2a23 94 LidarP[0] = -data.distance*cos(radians)- 2.8f;
ppovoa 5:bc42c03f2a23 95 LidarP[1] = -data.distance*sin(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 8:ad8766cf2ec0 102 bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance);
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++){
henkiwan 9:76b59c5220f1 115 pc.printf("%f|", Mapa40[i][j]);
ppovoa 6:59fbbeaac2af 116 //send_map(Mapa40[j][i]); // envia linha em linha (j i)
henkiwan 9:76b59c5220f1 117 //send_odometry(1, 2, Mapa40[j][i], j, i,10, 30); // faz prints estranhos no Putty
ppovoa 4:256f2cbe3fdd 118 }
henkiwan 9:76b59c5220f1 119 pc.printf("\n-----------------------------\n\r");
henkiwan 9:76b59c5220f1 120 }
henkiwan 9:76b59c5220f1 121 }
henkiwan 9:76b59c5220f1 122
henkiwan 9:76b59c5220f1 123
henkiwan 9:76b59c5220f1 124
henkiwan 9:76b59c5220f1 125
henkiwan 9:76b59c5220f1 126
henkiwan 9:76b59c5220f1 127
henkiwan 9:76b59c5220f1 128
henkiwan 9:76b59c5220f1 129 void bresenham(float poseX, float poseY, float xf, float yf, float z){
henkiwan 9:76b59c5220f1 130 int T, E, A, B;
henkiwan 9:76b59c5220f1 131 int x = static_cast<int>(poseX);
henkiwan 9:76b59c5220f1 132 int y = static_cast<int>(poseY);
henkiwan 9:76b59c5220f1 133 int dx = static_cast<int>(abs(xf - poseX));
henkiwan 9:76b59c5220f1 134 int dy = static_cast<int>(abs(yf - poseY));
henkiwan 9:76b59c5220f1 135
henkiwan 9:76b59c5220f1 136 int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
henkiwan 9:76b59c5220f1 137 int s2 = static_cast<int>((yf - poseY)/dy);
henkiwan 9:76b59c5220f1 138
henkiwan 9:76b59c5220f1 139 int interchange = 0;
henkiwan 9:76b59c5220f1 140
henkiwan 9:76b59c5220f1 141 if (dy > dx){
henkiwan 9:76b59c5220f1 142 T = dx;
henkiwan 9:76b59c5220f1 143 dx = dy;
henkiwan 9:76b59c5220f1 144 dy = T;
henkiwan 9:76b59c5220f1 145 interchange = 1;
henkiwan 9:76b59c5220f1 146 }
henkiwan 9:76b59c5220f1 147
henkiwan 9:76b59c5220f1 148 E = 2*dy - dx;
henkiwan 9:76b59c5220f1 149 A = 2*dy;
henkiwan 9:76b59c5220f1 150 B = 2*dy - 2*dx;
henkiwan 9:76b59c5220f1 151
henkiwan 9:76b59c5220f1 152 for (int i = 0; i<dx; i++){
henkiwan 9:76b59c5220f1 153 if (E < 0){
henkiwan 9:76b59c5220f1 154 if (interchange == 1){
henkiwan 9:76b59c5220f1 155 y = y + s2;
henkiwan 9:76b59c5220f1 156 }
henkiwan 9:76b59c5220f1 157 else{
henkiwan 9:76b59c5220f1 158 x = x + s1;
henkiwan 9:76b59c5220f1 159 }
henkiwan 9:76b59c5220f1 160 E = E + A;
henkiwan 9:76b59c5220f1 161 }
henkiwan 9:76b59c5220f1 162
henkiwan 9:76b59c5220f1 163 else{
henkiwan 9:76b59c5220f1 164 y = y + s2;
henkiwan 9:76b59c5220f1 165 x = x + s1;
henkiwan 9:76b59c5220f1 166 E = E + B;
henkiwan 9:76b59c5220f1 167 }
henkiwan 9:76b59c5220f1 168
henkiwan 9:76b59c5220f1 169 if (x >= 0 && y >= 0 && x < 40 && y < 40){
henkiwan 9:76b59c5220f1 170 // Mapear mapa do Logaritmo
henkiwan 9:76b59c5220f1 171 MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
henkiwan 9:76b59c5220f1 172 pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
henkiwan 9:76b59c5220f1 173 Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
henkiwan 9:76b59c5220f1 174 }
henkiwan 9:76b59c5220f1 175
henkiwan 9:76b59c5220f1 176
henkiwan 9:76b59c5220f1 177
ppovoa 4:256f2cbe3fdd 178 }
fabiofaria 1:dc87724abce8 179 }