SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Experiencias do Henrique na quinta/sexta a noite

Dependencies:   BufferedSerial

main.cpp@6:59fbbeaac2af, 2021-05-07 (annotated)

Committer:
ppovoa
Date:
Fri May 07 17:19:07 2021 +0000
Revision:
6:59fbbeaac2af
Parent:
5:bc42c03f2a23
Child:
7:f1c122bc63c8
6yzh

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