SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Experiencias do Henrique na quinta/sexta a noite

Dependencies:   BufferedSerial

main.cpp@7:f1c122bc63c8, 2021-05-10 (annotated)

Committer:
ppovoa
Date:
Mon May 10 15:23:24 2021 +0000
Revision:
7:f1c122bc63c8
Parent:
6:59fbbeaac2af
Child:
8:ad8766cf2ec0
10/05/2021

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