SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Experiencias do Henrique na quinta/sexta a noite

Dependencies:   BufferedSerial

main.cpp@8:ad8766cf2ec0, 2021-05-11 (annotated)

Committer:
ppovoa
Date:
Tue May 11 15:05:49 2021 +0000
Revision:
8:ad8766cf2ec0
Parent:
7:f1c122bc63c8
Child:
9:76b59c5220f1
SRA - erro no sendodometry

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