SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Experiencias do Henrique na quinta/sexta a noite

Dependencies:   BufferedSerial

main.cpp@5:bc42c03f2a23, 2021-05-07 (annotated)

Committer:
ppovoa
Date:
Fri May 07 14:58:46 2021 +0000
Revision:
5:bc42c03f2a23
Parent:
4:256f2cbe3fdd
Child:
6:59fbbeaac2af
alteracoes no Bresenham.

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
LuisRA 0:2b691d200d6f 20 int main()
LuisRA 0:2b691d200d6f 21 {
ppovoa 4:256f2cbe3fdd 22
fabiofaria 1:dc87724abce8 23 pc.baud(115200);
fabiofaria 1:dc87724abce8 24 init_communication(&pc);
ppovoa 5:bc42c03f2a23 25
ppovoa 5:bc42c03f2a23 26 DigitalIn UserButton(USER_BUTTON); // Initialize Button
ppovoa 5:bc42c03f2a23 27 DigitalOut myled(LED1); // Initialize LED
ppovoa 5:bc42c03f2a23 28
ppovoa 5:bc42c03f2a23 29 //float odomX, odomY, odomTheta;
ppovoa 5:bc42c03f2a23 30 struct RPLidarMeasurement data;
ppovoa 4:256f2cbe3fdd 31
fabiofaria 1:dc87724abce8 32 // Lidar initialization
LuisRA 0:2b691d200d6f 33 rplidar_motor.period(0.001f);
ppovoa 5:bc42c03f2a23 34 //rplidar_motor.write(0.5f);
LuisRA 0:2b691d200d6f 35 lidar.begin(se_lidar);
LuisRA 0:2b691d200d6f 36 lidar.setAngle(0,360);
ppovoa 4:256f2cbe3fdd 37
ppovoa 4:256f2cbe3fdd 38 float pose[3] = {20,20,0}; // Ponto Inicial
ppovoa 4:256f2cbe3fdd 39 float LidarP[2]; // pontos na plataforma
ppovoa 4:256f2cbe3fdd 40 float LidarW[2]; // pontos no mundo
ppovoa 5:bc42c03f2a23 41 float MapaLog[40][40];
ppovoa 5:bc42c03f2a23 42 for(int i = 0; i < 40; i++)
ppovoa 5:bc42c03f2a23 43 for(int j = 0; j < 40; j++)
ppovoa 5:bc42c03f2a23 44 MapaLog[i][j] = 0;
ppovoa 5:bc42c03f2a23 45
ppovoa 4:256f2cbe3fdd 46 float Mapa40[40][40];
ppovoa 5:bc42c03f2a23 47
ppovoa 5:bc42c03f2a23 48 // matriz rotacao world plataforma
ppovoa 5:bc42c03f2a23 49 float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
ppovoa 4:256f2cbe3fdd 50 {sin(pose[2]), cos(pose[2]), pose[1]},
ppovoa 4:256f2cbe3fdd 51 {0, 0, 1}};
ppovoa 4:256f2cbe3fdd 52
ppovoa 5:bc42c03f2a23 53 setSpeeds(0,0);
ppovoa 5:bc42c03f2a23 54
ppovoa 5:bc42c03f2a23 55 int leituras = 0;
ppovoa 5:bc42c03f2a23 56
ppovoa 5:bc42c03f2a23 57 pc.printf("waiting...\n\r");
ppovoa 5:bc42c03f2a23 58
ppovoa 5:bc42c03f2a23 59 int start = 0;
ppovoa 5:bc42c03f2a23 60 while(start != 1) {
ppovoa 5:bc42c03f2a23 61 myled=1;
ppovoa 5:bc42c03f2a23 62 if (UserButton == 0) { // Button is pressed
ppovoa 5:bc42c03f2a23 63 myled = 0;
ppovoa 5:bc42c03f2a23 64 start = 1;
ppovoa 5:bc42c03f2a23 65 rplidar_motor.write(0.5f);
ppovoa 5:bc42c03f2a23 66 }
ppovoa 5:bc42c03f2a23 67 }
ppovoa 5:bc42c03f2a23 68
ppovoa 5:bc42c03f2a23 69 lidar.startThreadScan();
ppovoa 4:256f2cbe3fdd 70
ppovoa 4:256f2cbe3fdd 71 pc.printf("Program started.\n\r");
ppovoa 4:256f2cbe3fdd 72
ppovoa 5:bc42c03f2a23 73 while(1){
ppovoa 5:bc42c03f2a23 74 if(lidar.pollSensorData(&data) == 0)
ppovoa 5:bc42c03f2a23 75 {
ppovoa 5:bc42c03f2a23 76 if(leituras == 100){
ppovoa 5:bc42c03f2a23 77 break;
ppovoa 5:bc42c03f2a23 78 }
ppovoa 5:bc42c03f2a23 79 pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
ppovoa 4:256f2cbe3fdd 80
ppovoa 5:bc42c03f2a23 81 float radians = (data.angle * PI)/180.0;
ppovoa 4:256f2cbe3fdd 82
ppovoa 5:bc42c03f2a23 83 LidarP[0] = -data.distance*cos(radians)- 2.8f;
ppovoa 5:bc42c03f2a23 84 LidarP[1] = -data.distance*sin(radians)- 1.5f;
LuisRA 0:2b691d200d6f 85
ppovoa 4:256f2cbe3fdd 86 //W_P = R_WP * p_P
ppovoa 4:256f2cbe3fdd 87 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 88 LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
ppovoa 4:256f2cbe3fdd 89
ppovoa 4:256f2cbe3fdd 90 // pontos onde o feixe passou
ppovoa 5:bc42c03f2a23 91 bresenham(pose[0], pose[1], LidarW[0], LidarW[1], MapaLog, data.distance);
ppovoa 4:256f2cbe3fdd 92
ppovoa 5:bc42c03f2a23 93 leituras++;
ppovoa 5:bc42c03f2a23 94 }
ppovoa 5:bc42c03f2a23 95 }
ppovoa 4:256f2cbe3fdd 96
ppovoa 4:256f2cbe3fdd 97 // Converter o logaritmo para o mapa 40
ppovoa 4:256f2cbe3fdd 98 for(int i=0; i<40; i++){
ppovoa 4:256f2cbe3fdd 99 for(int j=0; j<40; j++){
ppovoa 4:256f2cbe3fdd 100 Mapa40[j][i] = 1 - 1/(1+exp(MapaLog[j][i]));
ppovoa 4:256f2cbe3fdd 101 //printf("%.2f\n", 1 - 1/(1+exp(MapaLog[i][j])));
ppovoa 5:bc42c03f2a23 102 send_map(Mapa40[j][i]); // envia linha em linha (j i)
ppovoa 4:256f2cbe3fdd 103 }
ppovoa 4:256f2cbe3fdd 104 }
ppovoa 4:256f2cbe3fdd 105
ppovoa 4:256f2cbe3fdd 106 /*
LuisRA 0:2b691d200d6f 107 while(1) {
LuisRA 0:2b691d200d6f 108 // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
LuisRA 0:2b691d200d6f 109 if(lidar.pollSensorData(&data) == 0)
LuisRA 0:2b691d200d6f 110 {
ppovoa 4:256f2cbe3fdd 111 //if (data.angle > 0 and data.angle < 15)
ppovoa 4:256f2cbe3fdd 112 pc.printf("%f\t%f\t%d\t%c\n\r", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
ppovoa 4:256f2cbe3fdd 113 send_odometry(1, 2, countsLeft, countsRight, odomX, odomY, odomTheta);
LuisRA 0:2b691d200d6f 114 }
fabiofaria 3:0a718d139ed1 115 wait(0.01);
ppovoa 4:256f2cbe3fdd 116 }*/
fabiofaria 1:dc87724abce8 117 }