Gonçalo Lopes / Mbed OS 3_VFH

Dependencies:   BufferedSerial

main.cpp@4:213afe3d5c4b, 2021-05-03 (annotated)

Committer:
xaficz
Date:
Mon May 03 15:45:29 2021 +0000
Revision:
4:213afe3d5c4b
Parent:
3:0a718d139ed1 
3_VFH

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xaficz 4:213afe3d5c4b 1
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"
xaficz 4:213afe3d5c4b 7 #include "funcs.h"
LuisRA 0:2b691d200d6f 8
fabiofaria 1:dc87724abce8 9 Serial pc(SERIAL_TX, SERIAL_RX);
LuisRA 0:2b691d200d6f 10 RPLidar lidar;
LuisRA 0:2b691d200d6f 11 BufferedSerial se_lidar(PA_9, PA_10);
fabiofaria 3:0a718d139ed1 12 PwmOut rplidar_motor(D3);
xaficz 4:213afe3d5c4b 13 DigitalIn Button(USER_BUTTON);
LuisRA 0:2b691d200d6f 14
xaficz 4:213afe3d5c4b 15 static const double pi = 3.14159265358;
xaficz 4:213afe3d5c4b 16
xaficz 4:213afe3d5c4b 17
xaficz 4:213afe3d5c4b 18 //Matrizes de mapeamento
xaficz 4:213afe3d5c4b 19 int Map_Matrix[80][80] = {{0}}; //Matriz do mapa
xaficz 4:213afe3d5c4b 20 double Log_Map_Matrix[80][80] = {{0}}; //Matriz de Logs
xaficz 4:213afe3d5c4b 21 int Aux_Matrix[80][80] = {{0}};
xaficz 4:213afe3d5c4b 22 int Sector_Matrix_Sections[15][15]; //Matriz com os indices das seccoes
xaficz 4:213afe3d5c4b 23 double Sector_Matrix_Abs[15][15]; //Matriz com as amplitudes
xaficz 4:213afe3d5c4b 24 int Surroundings_Matrix[15][15]; //Matriz das redondezas do bot
xaficz 4:213afe3d5c4b 25 double angle;
xaficz 4:213afe3d5c4b 26
xaficz 4:213afe3d5c4b 27 //Ganhos
xaficz 4:213afe3d5c4b 28 double k = 12; //ganho angular
xaficz 4:213afe3d5c4b 29
xaficz 4:213afe3d5c4b 30 //Variaveis de posicao
xaficz 4:213afe3d5c4b 31 double x = 100; //x inicial
xaficz 4:213afe3d5c4b 32 double y = 2000; //y inicial
xaficz 4:213afe3d5c4b 33 double phi = 0.01; //phi inicial
xaficz 4:213afe3d5c4b 34 double end_position_x = 1900; //x final
xaficz 4:213afe3d5c4b 35 double end_position_y = 2000; //y final
xaficz 4:213afe3d5c4b 36 double next_phi; //phi seguinte
xaficz 4:213afe3d5c4b 37
xaficz 4:213afe3d5c4b 38 //Variaveis de estimacao de posicao
xaficz 4:213afe3d5c4b 39 double var_l; //contagens do encoder esquerdas
xaficz 4:213afe3d5c4b 40 double var_r; //contagens do encoder direitas
xaficz 4:213afe3d5c4b 41 double var_d; //variacao linear entre iteracoes
xaficz 4:213afe3d5c4b 42 double var_phi; //variacao angular entre iteracoes
xaficz 4:213afe3d5c4b 43 double L = 150; //distancia entre rodas
xaficz 4:213afe3d5c4b 44 double r = 35; //raio da roda
xaficz 4:213afe3d5c4b 45
xaficz 4:213afe3d5c4b 46 //Variaveis de comando as rodas
xaficz 4:213afe3d5c4b 47 double w; //velociade angular do robo
xaficz 4:213afe3d5c4b 48 double w_left; //velocidade roda esquerda
xaficz 4:213afe3d5c4b 49 double w_right; //velocidade roda direita
xaficz 4:213afe3d5c4b 50 double v = 2100; //velocidade linear do robo
xaficz 4:213afe3d5c4b 51 double dist;
xaficz 4:213afe3d5c4b 52
LuisRA 0:2b691d200d6f 53 int main()
LuisRA 0:2b691d200d6f 54 {
xaficz 4:213afe3d5c4b 55 while (Button == 1) {}
xaficz 4:213afe3d5c4b 56 wait(1);
xaficz 4:213afe3d5c4b 57
xaficz 4:213afe3d5c4b 58 //float odomX, odomY, odomTheta;
fabiofaria 1:dc87724abce8 59 struct RPLidarMeasurement data;
fabiofaria 1:dc87724abce8 60
fabiofaria 1:dc87724abce8 61 pc.baud(115200);
fabiofaria 1:dc87724abce8 62 init_communication(&pc);
LuisRA 0:2b691d200d6f 63
fabiofaria 1:dc87724abce8 64 // Lidar initialization
xaficz 4:213afe3d5c4b 65 rplidar_motor.write(1.0f);
LuisRA 0:2b691d200d6f 66 rplidar_motor.period(0.001f);
LuisRA 0:2b691d200d6f 67 lidar.begin(se_lidar);
LuisRA 0:2b691d200d6f 68 lidar.setAngle(0,360);
LuisRA 0:2b691d200d6f 69
LuisRA 0:2b691d200d6f 70 lidar.startThreadScan();
xaficz 4:213afe3d5c4b 71
xaficz 4:213afe3d5c4b 72 //--------------------------------------------------------------------------
LuisRA 0:2b691d200d6f 73
xaficz 4:213afe3d5c4b 74 //Espera que o botao seja premido
xaficz 4:213afe3d5c4b 75 while (Button == 1) {}
xaficz 4:213afe3d5c4b 76 wait(1);
xaficz 4:213afe3d5c4b 77
xaficz 4:213afe3d5c4b 78 //Funcao para obter a matriz com os indices das seccoes assim como as amplitudes
xaficz 4:213afe3d5c4b 79 get_Sector_Matrix(Sector_Matrix_Sections, Sector_Matrix_Abs);
xaficz 4:213afe3d5c4b 80
xaficz 4:213afe3d5c4b 81 //Distancia ao ponto objetivo
xaficz 4:213afe3d5c4b 82 dist = sqrt((double)pow(end_position_x-x,2) + pow(end_position_y-y,2));
xaficz 4:213afe3d5c4b 83
xaficz 4:213afe3d5c4b 84 while(dist > 50) {
LuisRA 0:2b691d200d6f 85 // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
LuisRA 0:2b691d200d6f 86 if(lidar.pollSensorData(&data) == 0)
LuisRA 0:2b691d200d6f 87 {
xaficz 4:213afe3d5c4b 88 //pc.printf("%f;%f;%d;%c;\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
xaficz 4:213afe3d5c4b 89
xaficz 4:213afe3d5c4b 90
xaficz 4:213afe3d5c4b 91 getCountsAndReset();
xaficz 4:213afe3d5c4b 92
xaficz 4:213afe3d5c4b 93 //Estimacao de pose
xaficz 4:213afe3d5c4b 94 var_l = (2*pi*r*countsLeft/1440);
xaficz 4:213afe3d5c4b 95 var_r = (2*pi*r*countsRight/1440);
xaficz 4:213afe3d5c4b 96 var_d = (var_l+var_r)/2;
xaficz 4:213afe3d5c4b 97 var_phi= (var_r-var_l)/L;
xaficz 4:213afe3d5c4b 98
xaficz 4:213afe3d5c4b 99 if(var_phi == 0){
xaficz 4:213afe3d5c4b 100 x = x + var_d*cos(phi);
xaficz 4:213afe3d5c4b 101 y = y + var_d*sin(phi);
xaficz 4:213afe3d5c4b 102 }
xaficz 4:213afe3d5c4b 103 else{
xaficz 4:213afe3d5c4b 104 x = x + (var_d)*((sin(phi/2)/(phi/2))*cos(phi + var_phi/2));
xaficz 4:213afe3d5c4b 105 y = y + (var_d)*((sin(phi/2)/(phi/2))*sin(phi + var_phi/2));
xaficz 4:213afe3d5c4b 106 phi = phi + var_phi;
LuisRA 0:2b691d200d6f 107 }
xaficz 4:213afe3d5c4b 108 //Print coordenadas habituais
xaficz 4:213afe3d5c4b 109 //pc.printf("x_atual = %f, y_atual = %f\n",x,y);
xaficz 4:213afe3d5c4b 110 //Distancia ao ponto objetivo
xaficz 4:213afe3d5c4b 111 dist = sqrt((double)pow(end_position_x-x,2) + pow(end_position_y-y,2));
xaficz 4:213afe3d5c4b 112 //pc.printf("\ndist = %f\n", dist);
xaficz 4:213afe3d5c4b 113
xaficz 4:213afe3d5c4b 114 //Conversao dos angulos em radianos
xaficz 4:213afe3d5c4b 115 angle = (2*pi/360)*(data.angle); //
xaficz 4:213afe3d5c4b 116 angle = atan2(sin(angle + pi/2 + phi),cos(angle + pi/2 + phi));
xaficz 4:213afe3d5c4b 117 update_map(Log_Map_Matrix, Aux_Matrix, Map_Matrix, data.distance/50, angle, x, y); //Funcao para atualizar o mapa
xaficz 4:213afe3d5c4b 118
xaficz 4:213afe3d5c4b 119 //Funcao para obter o mapa das redondezas do robo
xaficz 4:213afe3d5c4b 120 get_Surroundings_Matrix(Surroundings_Matrix, x, y, Map_Matrix);
xaficz 4:213afe3d5c4b 121
xaficz 4:213afe3d5c4b 122 for(int n = 0; n < 15; n++){
xaficz 4:213afe3d5c4b 123 for(int m = 0; m < 15; m++){
xaficz 4:213afe3d5c4b 124 pc.printf("%d,",Surroundings_Matrix[n][m]);
xaficz 4:213afe3d5c4b 125 }
xaficz 4:213afe3d5c4b 126 pc.printf("\n");
xaficz 4:213afe3d5c4b 127 }
xaficz 4:213afe3d5c4b 128
xaficz 4:213afe3d5c4b 129 //Funcao para obter a direcao que o robo deve tomar em funcao da sua posicao atual e das suas redondezas
xaficz 4:213afe3d5c4b 130 next_phi = get_orientation_angle(x, y, end_position_x, end_position_y, Surroundings_Matrix, Sector_Matrix_Sections, Sector_Matrix_Abs, 31);
xaficz 4:213afe3d5c4b 131 //pc.printf("\n\n next phi = %f\n\n", next_phi);
xaficz 4:213afe3d5c4b 132
xaficz 4:213afe3d5c4b 133 //Controlo proporcional da direcao
xaficz 4:213afe3d5c4b 134 w = k*atan2(sin(next_phi-phi),cos(next_phi-phi));
xaficz 4:213afe3d5c4b 135
xaficz 4:213afe3d5c4b 136 //Enviar os comandos de velocidades para as rodas
xaficz 4:213afe3d5c4b 137 w_left=(v-(L/2)*w)/r;
xaficz 4:213afe3d5c4b 138 w_right=(v+(L/2)*w)/r;
xaficz 4:213afe3d5c4b 139 setSpeeds(w_left,w_right);
xaficz 4:213afe3d5c4b 140 }
LuisRA 0:2b691d200d6f 141 }
xaficz 4:213afe3d5c4b 142 setSpeeds(0,0);
xaficz 4:213afe3d5c4b 143
xaficz 4:213afe3d5c4b 144 return(0);
xaficz 4:213afe3d5c4b 145 }