Gonçalo Lopes / Mbed OS 3

VFH com Lidar

Dependencies:   BufferedSerial

main.cpp@7:5fa6f21eb739, 2021-05-24 (annotated)

Committer:
xaficz
Date:
Mon May 24 15:32:13 2021 +0000
Revision:
7:5fa6f21eb739
Parent:
6:df6b8b2468d8 
4__zenaga

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 RPLidar lidar;
xaficz 5:25bd866ef068 14 BufferedSerial se_lidar(PA_9, PA_10);
xaficz 5:25bd866ef068 15 PwmOut rplidar_motor(D3);
xaficz 4:53ac11e9e8b9 16
xaficz 7:5fa6f21eb739 17 //DigitalIn Button_VFH(USER_BUTTON);
xaficz 5:25bd866ef068 18
xaficz 4:53ac11e9e8b9 19
xaficz 7:5fa6f21eb739 20 //=======================================
xaficz 7:5fa6f21eb739 21 //Para o matlab
xaficz 7:5fa6f21eb739 22
xaficz 7:5fa6f21eb739 23 //init_communication(Serial *serial_in)
xaficz 7:5fa6f21eb739 24
xaficz 7:5fa6f21eb739 25 //=======================================
xaficz 7:5fa6f21eb739 26
xaficz 5:25bd866ef068 27 //Matrizes de mapeamento
xaficz 5:25bd866ef068 28 int Map_Matrix[80][80]; //Matriz do mapa
xaficz 5:25bd866ef068 29
xaficz 5:25bd866ef068 30 //Ganhos
xaficz 5:25bd866ef068 31 // double k = 8; //Ganho angular
xaficz 5:25bd866ef068 32
xaficz 5:25bd866ef068 33 //Variaveis de posicao
xaficz 6:df6b8b2468d8 34 double x = 60; //x inicial cm
xaficz 6:df6b8b2468d8 35 double y = 60; //y inicial
xaficz 5:25bd866ef068 36 double phi = 0.01; //phi inicial
xaficz 5:25bd866ef068 37
xaficz 4:53ac11e9e8b9 38
xaficz 6:df6b8b2468d8 39 // Coordenadas
xaficz 6:df6b8b2468d8 40 int x_Celula; // Coordenada X Relativa à Celula Ativa (Sistema de Coordenadas baseado em Células de Aresta 5 cm)
xaficz 6:df6b8b2468d8 41 int y_Celula; // Coordenada Y Relativa à Celula Ativa (Sistema de Coordenadas baseado em Células de Aresta 5 cm)
xaficz 6:df6b8b2468d8 42
xaficz 6:df6b8b2468d8 43
xaficz 6:df6b8b2468d8 44 float Distance; // Distância Devolvida pela Leitura do LIDAR
xaficz 6:df6b8b2468d8 45 float Angle; // Angulo Devolvida pela Leitura do LIDAR
xaficz 6:df6b8b2468d8 46
xaficz 6:df6b8b2468d8 47
xaficz 5:25bd866ef068 48 //Variaveis de estimacao de posicao
xaficz 5:25bd866ef068 49 double var_l; //contagens do encoder esquerdas
xaficz 5:25bd866ef068 50 double var_r; //contagens do encoder direitas
xaficz 5:25bd866ef068 51 double var_d; //variacao linear entre iteracoes
xaficz 5:25bd866ef068 52 double var_phi; //variacao angular entre iteracoes
xaficz 5:25bd866ef068 53
xaficz 6:df6b8b2468d8 54 double L = 15.0; //distancia entre rodas
xaficz 6:df6b8b2468d8 55 double r = 3.5; //raio da roda
LuisRA 0:2b691d200d6f 56
xaficz 5:25bd866ef068 57 //Variaveis de comando as rodas
xaficz 7:5fa6f21eb739 58 double V_left = 45; //velocidade da roda esquerda
xaficz 7:5fa6f21eb739 59 double V_right = 52; //velocidade da roda direita
xaficz 4:53ac11e9e8b9 60
xaficz 7:5fa6f21eb739 61
xaficz 7:5fa6f21eb739 62
xaficz 7:5fa6f21eb739 63
xaficz 7:5fa6f21eb739 64 int main() {
xaficz 7:5fa6f21eb739 65
xaficz 7:5fa6f21eb739 66 struct RPLidarMeasurement data;
xaficz 7:5fa6f21eb739 67
xaficz 7:5fa6f21eb739 68
xaficz 7:5fa6f21eb739 69 pc.baud(115200);
xaficz 7:5fa6f21eb739 70 init_communication(&pc);
xaficz 7:5fa6f21eb739 71
xaficz 7:5fa6f21eb739 72 rplidar_motor.period(0.01f);
xaficz 7:5fa6f21eb739 73 //rplidar_motor.write(1.0f);
xaficz 7:5fa6f21eb739 74 lidar.begin(se_lidar);
xaficz 7:5fa6f21eb739 75 lidar.setAngle(0,360);
xaficz 7:5fa6f21eb739 76 lidar.startThreadScan();
xaficz 7:5fa6f21eb739 77 rplidar_motor.write(0.7f);
xaficz 7:5fa6f21eb739 78
xaficz 5:25bd866ef068 79 //Auxiliares
xaficz 6:df6b8b2468d8 80 int Ciclos = 0;
xaficz 5:25bd866ef068 81 int Leituras = 0;
xaficz 7:5fa6f21eb739 82 int AUX = 1;
xaficz 7:5fa6f21eb739 83 int i = 0;
xaficz 7:5fa6f21eb739 84 int fin = 0;
xaficz 7:5fa6f21eb739 85 int x_end;
xaficz 7:5fa6f21eb739 86 int y_end;
xaficz 7:5fa6f21eb739 87 int *x_cel;
xaficz 7:5fa6f21eb739 88 int *y_cel;
xaficz 7:5fa6f21eb739 89 float auxx;
xaficz 7:5fa6f21eb739 90 float auxy;
xaficz 6:df6b8b2468d8 91
xaficz 7:5fa6f21eb739 92 int dist_odometria;
xaficz 5:25bd866ef068 93
xaficz 5:25bd866ef068 94 //Funcao de obtencao do Mapa
xaficz 5:25bd866ef068 95 read_map(Map_Matrix);
xaficz 5:25bd866ef068 96
xaficz 5:25bd866ef068 97 while(1){
xaficz 5:25bd866ef068 98
xaficz 5:25bd866ef068 99 //Este while é para fazer uma paragem de 10 em 10 ciclos para dar tempo para o lidar executar leituras
xaficz 7:5fa6f21eb739 100 //while(ciclos< 10){
xaficz 5:25bd866ef068 101
xaficz 5:25bd866ef068 102 //Manda andar
xaficz 5:25bd866ef068 103 setSpeeds(V_left,V_right);
xaficz 5:25bd866ef068 104
xaficz 5:25bd866ef068 105
xaficz 5:25bd866ef068 106 //:::::::::::::: ODOMETRIA :::::::::::::::
xaficz 6:df6b8b2468d8 107
xaficz 5:25bd866ef068 108 //Obter as contagens dos encoders
xaficz 5:25bd866ef068 109 getCountsAndReset();
xaficz 5:25bd866ef068 110
xaficz 5:25bd866ef068 111 //Estimacao de pose
xaficz 5:25bd866ef068 112 var_l = (2*pi*r*countsLeft/1440);
xaficz 5:25bd866ef068 113 var_r = (2*pi*r*countsRight/1440);
xaficz 5:25bd866ef068 114 var_d = (var_l+var_r)/2;
xaficz 5:25bd866ef068 115 var_phi= (var_r-var_l)/L;
xaficz 5:25bd866ef068 116
xaficz 7:5fa6f21eb739 117
xaficz 7:5fa6f21eb739 118 x = x + (var_d)*cos(phi + var_phi/2);
xaficz 7:5fa6f21eb739 119 y = y + (var_d)*sin(phi + var_phi/2);
xaficz 5:25bd866ef068 120 phi = phi + var_phi;
xaficz 7:5fa6f21eb739 121
xaficz 5:25bd866ef068 122
xaficz 5:25bd866ef068 123 pc.printf("x_odometria = %f, y_odometria = %f\n", x, y);
xaficz 5:25bd866ef068 124
xaficz 6:df6b8b2468d8 125
xaficz 6:df6b8b2468d8 126 //Celula da Odometria
xaficz 6:df6b8b2468d8 127 x_Celula = ceil(x/5) - 1;
xaficz 6:df6b8b2468d8 128 y_Celula = ceil(y/5) - 1;
xaficz 6:df6b8b2468d8 129
xaficz 7:5fa6f21eb739 130 pc.printf("x_Celula = %d, y_Celula = %d\n", x_Celula, y_Celula);
xaficz 6:df6b8b2468d8 131
xaficz 7:5fa6f21eb739 132 //===========================================
xaficz 7:5fa6f21eb739 133 //Para o matlab
xaficz 7:5fa6f21eb739 134
xaficz 7:5fa6f21eb739 135 //send_odometry(value1,value2,ticks_left,ticks_right,x,y,theta)
xaficz 6:df6b8b2468d8 136
xaficz 6:df6b8b2468d8 137 //============================================
xaficz 6:df6b8b2468d8 138
xaficz 7:5fa6f21eb739 139 if(lidar.pollSensorData(&data)== 0) {
xaficz 7:5fa6f21eb739 140
xaficz 7:5fa6f21eb739 141 //Distancia Lida
xaficz 7:5fa6f21eb739 142 Distance = (data.distance)/10; // aqui este 10 é para cm TENHO QUE VER
xaficz 7:5fa6f21eb739 143
xaficz 7:5fa6f21eb739 144
xaficz 7:5fa6f21eb739 145 //Angulo da leitura
xaficz 7:5fa6f21eb739 146 Angle = pi*(data.angle)/180;
xaficz 7:5fa6f21eb739 147 Angle = atan2(sin(Angle),cos(Angle)); //em rad
xaficz 7:5fa6f21eb739 148
xaficz 7:5fa6f21eb739 149 pc.printf("Distancia = %f,-----------, Angle= %f\n", Distance,Angle);
xaficz 6:df6b8b2468d8 150
xaficz 6:df6b8b2468d8 151 //Distancia suposta
xaficz 7:5fa6f21eb739 152 // dist_odometria = distancia_prevista(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
xaficz 7:5fa6f21eb739 153 // dist_odometria = DistanciaPrevBresh(x,y,x_Celula,y_Celula,Angle,Map_Matrix[80][80]);
xaficz 7:5fa6f21eb739 154
xaficz 7:5fa6f21eb739 155
xaficz 7:5fa6f21eb739 156 if(Angle < pi/2 && Angle > 3*pi/2) {
xaficz 7:5fa6f21eb739 157 x_end = 80;}
xaficz 7:5fa6f21eb739 158 else{
xaficz 7:5fa6f21eb739 159 x_end = 0;}
xaficz 6:df6b8b2468d8 160
xaficz 7:5fa6f21eb739 161 if(Angle < 0 && Angle < pi) {
xaficz 7:5fa6f21eb739 162 y_end = 80;}
xaficz 7:5fa6f21eb739 163 else{
xaficz 7:5fa6f21eb739 164 y_end = 0;}
xaficz 7:5fa6f21eb739 165
xaficz 7:5fa6f21eb739 166 x_cel = x_bresenham(x_Celula, y_Celula, x_end, y_end);
xaficz 7:5fa6f21eb739 167 y_cel = y_bresenham(x_Celula, y_Celula, x_end, y_end);
xaficz 6:df6b8b2468d8 168
xaficz 6:df6b8b2468d8 169
xaficz 7:5fa6f21eb739 170 while(fin == 0){
xaficz 7:5fa6f21eb739 171 if(Map_Matrix[x_cel[i]][y_cel[i]] == 1){
xaficz 7:5fa6f21eb739 172 fin = 1;
xaficz 7:5fa6f21eb739 173 }
xaficz 7:5fa6f21eb739 174 else{
xaficz 7:5fa6f21eb739 175 i=i+1;
xaficz 7:5fa6f21eb739 176 }
xaficz 7:5fa6f21eb739 177 }
xaficz 6:df6b8b2468d8 178
xaficz 7:5fa6f21eb739 179 auxx = ((x_cel[i]-x_Celula)*(x_cel[i]-x_Celula));
xaficz 7:5fa6f21eb739 180 auxy = ((y_cel[i]-y_Celula)*(y_cel[i]-y_Celula));
xaficz 7:5fa6f21eb739 181 dist_odometria = sqrt( auxx + auxy );
xaficz 6:df6b8b2468d8 182
xaficz 7:5fa6f21eb739 183 pc.printf("DistanciaPREVISTA = %f\n", dist_odometria);
xaficz 6:df6b8b2468d8 184
xaficz 7:5fa6f21eb739 185 }
xaficz 7:5fa6f21eb739 186
xaficz 6:df6b8b2468d8 187 //==========================================
xaficz 6:df6b8b2468d8 188 //conta como mais um ciclo
xaficz 6:df6b8b2468d8 189 Ciclos = Ciclos+1;
xaficz 6:df6b8b2468d8 190
xaficz 6:df6b8b2468d8 191
xaficz 6:df6b8b2468d8 192
xaficz 6:df6b8b2468d8 193 //::::::::::::::IF de LEITURAS :::::::::::::::::::::::::::
xaficz 6:df6b8b2468d8 194 if(Ciclos == 10){
xaficz 6:df6b8b2468d8 195 while(Leituras<30) // faz 30 leituras parado
xaficz 6:df6b8b2468d8 196
xaficz 6:df6b8b2468d8 197 if(lidar.pollSensorData(&data)== 0) {
xaficz 6:df6b8b2468d8 198
xaficz 6:df6b8b2468d8 199 Distance = (data.distance)/10; // aqui este 10 é para cm TENHO QUE VER
xaficz 6:df6b8b2468d8 200 Angle = pi*(data.angle)/180;
xaficz 6:df6b8b2468d8 201 Angle = atan2(sin(Angle),cos(Angle)); //em rad
xaficz 6:df6b8b2468d8 202
xaficz 6:df6b8b2468d8 203 //Distancia suposta
xaficz 7:5fa6f21eb739 204 // dist_odometria = distancia_prevista(x, y, x_celula, y_celula, Angle,Map_Matrix[80][80]){
xaficz 6:df6b8b2468d8 205
xaficz 6:df6b8b2468d8 206
xaficz 6:df6b8b2468d8 207 Leituras = Leituras + 1; // incrementa as leituras
xaficz 6:df6b8b2468d8 208 }
xaficz 6:df6b8b2468d8 209
xaficz 7:5fa6f21eb739 210 // reset de ciclos
xaficz 6:df6b8b2468d8 211 Ciclos = 0;
xaficz 6:df6b8b2468d8 212 } // fim do numero de leituras
xaficz 6:df6b8b2468d8 213
xaficz 6:df6b8b2468d8 214
xaficz 6:df6b8b2468d8 215 } //fim de if leituras
xaficz 6:df6b8b2468d8 216 //======================================
xaficz 6:df6b8b2468d8 217
xaficz 6:df6b8b2468d8 218
xaficz 7:5fa6f21eb739 219 return(0);
xaficz 6:df6b8b2468d8 220 } // fim do while
xaficz 6:df6b8b2468d8 221
xaficz 7:5fa6f21eb739 222 //return(0);
xaficz 4:53ac11e9e8b9 223
xaficz 7:5fa6f21eb739 224 //}
xaficz 4:53ac11e9e8b9 225
xaficz 7:5fa6f21eb739 226