Gonçalo Lopes / Mbed OS 3

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@5:25bd866ef068, 2021-05-17 (annotated)

Committer:
xaficz
Date:
Mon May 17 15:03:21 2021 +0000
Revision:
5:25bd866ef068
Parent:
4:53ac11e9e8b9
Child:
6:df6b8b2468d8
4_real

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LuisRA 0:2b691d200d6f 1 #include "mbed.h"
LuisRA 0:2b691d200d6f 2 #include "BufferedSerial.h"
LuisRA 0:2b691d200d6f 3 #include "rplidar.h"
fabiofaria 1:dc87724abce8 4 #include "Robot.h"
fabiofaria 1:dc87724abce8 5 #include "Communication.h"
xaficz 4:53ac11e9e8b9 6 #include <string.h>
xaficz 4:53ac11e9e8b9 7 #include <stdio.h>
xaficz 4:53ac11e9e8b9 8 #include <math.h>
LuisRA 0:2b691d200d6f 9
xaficz 4:53ac11e9e8b9 10 #define pi 3.14159265359
xaficz 5:25bd866ef068 11
xaficz 5:25bd866ef068 12 Serial pc(SERIAL_TX, SERIAL_RX);
xaficz 5:25bd866ef068 13 DigitalIn Button_VFH(USER_BUTTON);
xaficz 5:25bd866ef068 14
xaficz 5:25bd866ef068 15 RPLidar lidar;
xaficz 5:25bd866ef068 16 BufferedSerial se_lidar(PA_9, PA_10);
xaficz 5:25bd866ef068 17 PwmOut rplidar_motor(D3);
xaficz 5:25bd866ef068 18
xaficz 5:25bd866ef068 19 struct RPLidarMeasurement data;
LuisRA 0:2b691d200d6f 20
xaficz 5:25bd866ef068 21 int main() {
xaficz 4:53ac11e9e8b9 22
xaficz 5:25bd866ef068 23 pc.baud(115200);
xaficz 5:25bd866ef068 24 init_communication(&pc);
xaficz 5:25bd866ef068 25
xaficz 5:25bd866ef068 26 rplidar_motor.period(0.01f);
xaficz 5:25bd866ef068 27 //rplidar_motor.write(1.0f);
xaficz 5:25bd866ef068 28 lidar.begin(se_lidar);
xaficz 5:25bd866ef068 29 lidar.setAngle(0,360);
xaficz 5:25bd866ef068 30 lidar.startThreadScan();
xaficz 5:25bd866ef068 31 rplidar_motor.write(0.7f);
xaficz 5:25bd866ef068 32
xaficz 4:53ac11e9e8b9 33
xaficz 5:25bd866ef068 34 //Matrizes de mapeamento
xaficz 5:25bd866ef068 35 int Map_Matrix[80][80]; //Matriz do mapa
xaficz 5:25bd866ef068 36
xaficz 5:25bd866ef068 37 //Ganhos
xaficz 5:25bd866ef068 38 // double k = 8; //Ganho angular
xaficz 5:25bd866ef068 39
xaficz 5:25bd866ef068 40 //Variaveis de posicao
xaficz 5:25bd866ef068 41 double x = 300; //x inicial
xaficz 5:25bd866ef068 42 double y = 300; //y inicial
xaficz 5:25bd866ef068 43 double phi = 0.01; //phi inicial
xaficz 5:25bd866ef068 44
xaficz 4:53ac11e9e8b9 45
xaficz 5:25bd866ef068 46 //Variaveis de estimacao de posicao
xaficz 5:25bd866ef068 47 double var_l; //contagens do encoder esquerdas
xaficz 5:25bd866ef068 48 double var_r; //contagens do encoder direitas
xaficz 5:25bd866ef068 49 double var_d; //variacao linear entre iteracoes
xaficz 5:25bd866ef068 50 double var_phi; //variacao angular entre iteracoes
xaficz 5:25bd866ef068 51
xaficz 5:25bd866ef068 52 double L = 150; //distancia entre rodas
xaficz 5:25bd866ef068 53 double r = 35; //raio da roda
LuisRA 0:2b691d200d6f 54
xaficz 5:25bd866ef068 55 //Variaveis de comando as rodas
xaficz 5:25bd866ef068 56 double V_left = 20; //velocidade da roda esquerda
xaficz 5:25bd866ef068 57 double V_right = 25; //velocidade da roda direita
xaficz 4:53ac11e9e8b9 58
xaficz 5:25bd866ef068 59 //Auxiliares
xaficz 5:25bd866ef068 60 int ciclos = 0;
xaficz 5:25bd866ef068 61 int Leituras = 0;
xaficz 5:25bd866ef068 62
xaficz 5:25bd866ef068 63 //Funcao de obtencao do Mapa
xaficz 5:25bd866ef068 64 read_map(Map_Matrix);
xaficz 5:25bd866ef068 65
xaficz 4:53ac11e9e8b9 66
xaficz 5:25bd866ef068 67 while(1){
xaficz 5:25bd866ef068 68
xaficz 5:25bd866ef068 69 //Este while é para fazer uma paragem de 10 em 10 ciclos para dar tempo para o lidar executar leituras
xaficz 5:25bd866ef068 70 while(ciclos < 10)
xaficz 5:25bd866ef068 71
xaficz 5:25bd866ef068 72 //Manda andar
xaficz 5:25bd866ef068 73 setSpeeds(V_left,V_right);
xaficz 5:25bd866ef068 74
xaficz 5:25bd866ef068 75 //conta como mais um ciclo
xaficz 5:25bd866ef068 76 ciclos = ciclos+1;
xaficz 5:25bd866ef068 77
xaficz 5:25bd866ef068 78
xaficz 5:25bd866ef068 79 //:::::::::::::: ODOMETRIA :::::::::::::::
xaficz 5:25bd866ef068 80 //Obter as contagens dos encoders
xaficz 5:25bd866ef068 81 getCountsAndReset();
xaficz 5:25bd866ef068 82
xaficz 5:25bd866ef068 83 //Estimacao de pose
xaficz 5:25bd866ef068 84 var_l = (2*pi*r*countsLeft/1440);
xaficz 5:25bd866ef068 85 var_r = (2*pi*r*countsRight/1440);
xaficz 5:25bd866ef068 86 var_d = (var_l+var_r)/2;
xaficz 5:25bd866ef068 87 var_phi= (var_r-var_l)/L;
xaficz 5:25bd866ef068 88
xaficz 5:25bd866ef068 89 if(var_phi == 0){
xaficz 5:25bd866ef068 90 x = x + var_d*cos(phi);
xaficz 5:25bd866ef068 91 y = y + var_d*sin(phi);
xaficz 5:25bd866ef068 92 }
xaficz 5:25bd866ef068 93 else{
xaficz 5:25bd866ef068 94 x = x + (var_d)*((sin(phi/2)/(phi/2))*cos(phi + var_phi/2));
xaficz 5:25bd866ef068 95 y = y + (var_d)*((sin(phi/2)/(phi/2))*sin(phi + var_phi/2));
xaficz 5:25bd866ef068 96 phi = phi + var_phi;
LuisRA 0:2b691d200d6f 97 }
xaficz 5:25bd866ef068 98
xaficz 5:25bd866ef068 99 pc.printf("x_odometria = %f, y_odometria = %f\n", x, y);
xaficz 5:25bd866ef068 100
xaficz 5:25bd866ef068 101 } // fim do while de 10 ciclos
xaficz 4:53ac11e9e8b9 102
xaficz 5:25bd866ef068 103 // reset de ciclos
xaficz 5:25bd866ef068 104 //
xaficz 4:53ac11e9e8b9 105
xaficz 4:53ac11e9e8b9 106
xaficz 5:25bd866ef068 107 return(0);
xaficz 5:25bd866ef068 108 }
xaficz 5:25bd866ef068 109