SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Experiencias do Henrique na quinta/sexta a noite

Dependencies:   BufferedSerial

main.cpp@12:348038b466a3, 2021-05-14 (annotated)

Committer:
henkiwan
Date:
Fri May 14 05:52:55 2021 +0000
Revision:
12:348038b466a3
Parent:
11:58187c7de709
Child:
13:20e124fba426
Experiencias que fiz de noite

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"
ppovoa 4:256f2cbe3fdd 6 #include "Functions.h"
henkiwan 12:348038b466a3 7 #include <math.h>
henkiwan 12:348038b466a3 8 #include <stdio.h>
ppovoa 4:256f2cbe3fdd 9 #include <stdlib.h>
LuisRA 0:2b691d200d6f 10
henkiwan 12:348038b466a3 11 #define PI 3.141592654
ppovoa 5:bc42c03f2a23 12
fabiofaria 1:dc87724abce8 13 Serial pc(SERIAL_TX, SERIAL_RX);
LuisRA 0:2b691d200d6f 14 RPLidar lidar;
LuisRA 0:2b691d200d6f 15 BufferedSerial se_lidar(PA_9, PA_10);
fabiofaria 3:0a718d139ed1 16 PwmOut rplidar_motor(D3);
LuisRA 0:2b691d200d6f 17
henkiwan 12:348038b466a3 18 void bresenham(float poseX, float poseY, float xf, float yf, float z);
henkiwan 12:348038b466a3 19
ppovoa 8:ad8766cf2ec0 20 float MapaLog[40][40] = {0};
ppovoa 10:6c8ea68e9bac 21 float Mapa40[40][40];
henkiwan 9:76b59c5220f1 22
henkiwan 12:348038b466a3 23 int main() {
fabiofaria 1:dc87724abce8 24 pc.baud(115200);
fabiofaria 1:dc87724abce8 25 init_communication(&pc);
ppovoa 5:bc42c03f2a23 26
ppovoa 5:bc42c03f2a23 27 DigitalIn UserButton(USER_BUTTON); // Initialize Button
ppovoa 5:bc42c03f2a23 28 DigitalOut myled(LED1); // Initialize LED
ppovoa 5:bc42c03f2a23 29
henkiwan 12:348038b466a3 30 // PARAMETROS CONSTANTES
henkiwan 12:348038b466a3 31 float T = 0.5;
henkiwan 12:348038b466a3 32 float wheelsRadius = 3.5;
henkiwan 12:348038b466a3 33 float wheelsDistance = 13.5;
henkiwan 12:348038b466a3 34
henkiwan 12:348038b466a3 35 // Variaveis
henkiwan 12:348038b466a3 36 float k_v = 15; // 15
henkiwan 12:348038b466a3 37 float k_i = 0.3; //0.1
henkiwan 12:348038b466a3 38 float k_s = 11; //20
henkiwan 12:348038b466a3 39 float v_erro = 15;
henkiwan 12:348038b466a3 40
henkiwan 12:348038b466a3 41 int DPontos = 2; //distância de erro
henkiwan 12:348038b466a3 42
henkiwan 12:348038b466a3 43 float RectSize = 4.0;
henkiwan 12:348038b466a3 44 float FCRepul = -1000; // Forca de Repulsao
henkiwan 12:348038b466a3 45 float FCAtracao = 5; // Forca de Atracao
henkiwan 12:348038b466a3 46
henkiwan 12:348038b466a3 47 float pose[3] = {30, 15, 0}; // Ponto Inicial
henkiwan 12:348038b466a3 48 float pose_f[3] = {60,100,0}; // Ponto Objetivo
henkiwan 12:348038b466a3 49
henkiwan 12:348038b466a3 50 float errot = 0, erro = 0, erro_old = -1;
henkiwan 12:348038b466a3 51 float Forcas[4] = {0, 0, 0, 0};
henkiwan 12:348038b466a3 52
henkiwan 12:348038b466a3 53 float vRobot, wRobot, Norma, angForca, phi, w[2], ForcaResult[2], PIntermedio[2];
henkiwan 12:348038b466a3 54
henkiwan 12:348038b466a3 55 float LidarP[2]; // pontos na plataforma
henkiwan 12:348038b466a3 56 float LidarW[2]; // pontos no mundo
henkiwan 12:348038b466a3 57
henkiwan 12:348038b466a3 58 // matriz rotacao world plataforma
henkiwan 12:348038b466a3 59 float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
henkiwan 12:348038b466a3 60 {sin(pose[2]), cos(pose[2]), pose[1]},
henkiwan 12:348038b466a3 61 {0, 0, 1}};
henkiwan 12:348038b466a3 62
ppovoa 5:bc42c03f2a23 63 struct RPLidarMeasurement data;
ppovoa 4:256f2cbe3fdd 64
ppovoa 10:6c8ea68e9bac 65 //Inicializar Mapa das probabilidades a 0.5
ppovoa 10:6c8ea68e9bac 66 for(int i=0; i<40; i++)
ppovoa 10:6c8ea68e9bac 67 for(int j=0; j<40; j++)
ppovoa 10:6c8ea68e9bac 68 Mapa40[i][j]=0.5;
ppovoa 10:6c8ea68e9bac 69
henkiwan 12:348038b466a3 70 int leituras = 0; long dist;
henkiwan 12:348038b466a3 71
fabiofaria 1:dc87724abce8 72 // Lidar initialization
LuisRA 0:2b691d200d6f 73 rplidar_motor.period(0.001f);
ppovoa 5:bc42c03f2a23 74 //rplidar_motor.write(0.5f);
LuisRA 0:2b691d200d6f 75 lidar.begin(se_lidar);
henkiwan 9:76b59c5220f1 76 lidar.setAngle(0, 360);
henkiwan 12:348038b466a3 77
ppovoa 4:256f2cbe3fdd 78
henkiwan 9:76b59c5220f1 79 setSpeeds(0, 0);
ppovoa 5:bc42c03f2a23 80
ppovoa 5:bc42c03f2a23 81 pc.printf("waiting...\n\r");
henkiwan 12:348038b466a3 82
ppovoa 5:bc42c03f2a23 83 int start = 0;
ppovoa 5:bc42c03f2a23 84 while(start != 1) {
ppovoa 5:bc42c03f2a23 85 myled=1;
ppovoa 5:bc42c03f2a23 86 if (UserButton == 0) { // Button is pressed
ppovoa 5:bc42c03f2a23 87 myled = 0;
ppovoa 5:bc42c03f2a23 88 start = 1;
ppovoa 5:bc42c03f2a23 89 rplidar_motor.write(0.5f);
ppovoa 5:bc42c03f2a23 90 }
ppovoa 5:bc42c03f2a23 91 }
ppovoa 5:bc42c03f2a23 92
ppovoa 7:f1c122bc63c8 93 lidar.startThreadScan();
ppovoa 4:256f2cbe3fdd 94
ppovoa 10:6c8ea68e9bac 95 while(leituras < 1000){
ppovoa 5:bc42c03f2a23 96 if(lidar.pollSensorData(&data) == 0)
ppovoa 10:6c8ea68e9bac 97 {
henkiwan 9:76b59c5220f1 98 //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
ppovoa 4:256f2cbe3fdd 99
ppovoa 6:59fbbeaac2af 100 float radians = (data.angle * static_cast<float>(PI))/180.0f;
ppovoa 4:256f2cbe3fdd 101
henkiwan 12:348038b466a3 102 dist = data.distance/10.0f;
ppovoa 11:58187c7de709 103
ppovoa 11:58187c7de709 104 LidarP[0] = -dist*sin(radians)- 2.8f;
ppovoa 11:58187c7de709 105 LidarP[1] = -dist*cos(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
henkiwan 12:348038b466a3 112 bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5);
ppovoa 4:256f2cbe3fdd 113
ppovoa 5:bc42c03f2a23 114 leituras++;
ppovoa 5:bc42c03f2a23 115 }
henkiwan 12:348038b466a3 116 }
henkiwan 12:348038b466a3 117
henkiwan 12:348038b466a3 118 for(int f = 0; f < 40; f++){
henkiwan 12:348038b466a3 119 for(int g = 0; g < 40; g++){
henkiwan 12:348038b466a3 120 if(Mapa40[f][g] < 0.65f)
henkiwan 12:348038b466a3 121 pc.printf("0 ");
henkiwan 12:348038b466a3 122 else
henkiwan 12:348038b466a3 123 pc.printf("1 ");
henkiwan 12:348038b466a3 124 }
henkiwan 12:348038b466a3 125 pc.printf("nova linha \n\r");
henkiwan 12:348038b466a3 126 }
henkiwan 12:348038b466a3 127
henkiwan 12:348038b466a3 128 start = 0;
henkiwan 12:348038b466a3 129 while(start != 1) {
henkiwan 12:348038b466a3 130 myled=1;
henkiwan 12:348038b466a3 131 if (UserButton == 0) { // Button is pressed
henkiwan 12:348038b466a3 132 myled = 0;
henkiwan 12:348038b466a3 133 start = 1;
henkiwan 12:348038b466a3 134 rplidar_motor.write(0.5f);
henkiwan 12:348038b466a3 135 }
ppovoa 5:bc42c03f2a23 136 }
ppovoa 4:256f2cbe3fdd 137
henkiwan 12:348038b466a3 138 pc.printf("Running\n\r");
henkiwan 12:348038b466a3 139
henkiwan 12:348038b466a3 140 while(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 5.0f) {
henkiwan 12:348038b466a3 141
henkiwan 12:348038b466a3 142 getCountsAndReset(); // Call getCountsAndReset() to read the values of encoders
henkiwan 12:348038b466a3 143 // and reset them. The values become available on variables
henkiwan 12:348038b466a3 144 // "countsLeft" and "countsRight".
henkiwan 12:348038b466a3 145
henkiwan 12:348038b466a3 146 //if(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 13){
henkiwan 12:348038b466a3 147
henkiwan 12:348038b466a3 148 pc.printf("x; %f, y: %f\n\r", pose[0], pose[1]);
henkiwan 12:348038b466a3 149
henkiwan 12:348038b466a3 150 JanelaAtivaVFF(pose, Mapa40, pose_f, FCRepul, FCAtracao, RectSize, Forcas);
henkiwan 12:348038b466a3 151
henkiwan 12:348038b466a3 152 ForcaResult[0] = Forcas[0]+Forcas[2]; // componente de x
henkiwan 12:348038b466a3 153 ForcaResult[1] = Forcas[1]+Forcas[3]; // componente de y
henkiwan 12:348038b466a3 154
henkiwan 12:348038b466a3 155 Norma = sqrt(pow(ForcaResult[0],2) + pow(ForcaResult[1],2));
henkiwan 12:348038b466a3 156 ForcaResult[0] = ForcaResult[0]/Norma; // Normalização da forca resultante entre a forca de repulsão e a forca de atracão
henkiwan 12:348038b466a3 157 ForcaResult[1] = ForcaResult[1]/Norma;
ppovoa 6:59fbbeaac2af 158
henkiwan 12:348038b466a3 159 // Angulo da forca resultante
henkiwan 12:348038b466a3 160 angForca = atan2(ForcaResult[1], ForcaResult[0]);
henkiwan 12:348038b466a3 161 //}
henkiwan 12:348038b466a3 162 //else
henkiwan 12:348038b466a3 163 //angForca = atan2(pose_f[1] - pose[1], pose_f[0] - pose[0]);
henkiwan 12:348038b466a3 164
henkiwan 12:348038b466a3 165
henkiwan 12:348038b466a3 166 PIntermedio[0] = pose[0] + v_erro * cos(angForca);
henkiwan 12:348038b466a3 167 PIntermedio[1] = pose[1] + v_erro * sin(angForca);
henkiwan 12:348038b466a3 168
henkiwan 12:348038b466a3 169 erro = sqrt(pow(PIntermedio[1]-pose[1], 2)+ pow(PIntermedio[0]-pose[0],2)) - DPontos;
henkiwan 12:348038b466a3 170
henkiwan 12:348038b466a3 171 if(erro_old == -1){
henkiwan 12:348038b466a3 172 erro_old = erro;
henkiwan 12:348038b466a3 173 }
henkiwan 12:348038b466a3 174
henkiwan 12:348038b466a3 175 errot = errot + (erro + erro_old) * T/2.0f;
henkiwan 12:348038b466a3 176
henkiwan 12:348038b466a3 177 //Velocidade do Robo
henkiwan 12:348038b466a3 178 vRobot = k_v * erro + k_i * errot;
henkiwan 12:348038b466a3 179
henkiwan 12:348038b466a3 180 erro_old = erro;
henkiwan 12:348038b466a3 181
henkiwan 12:348038b466a3 182 phi = atan2((PIntermedio[1]-pose[1]), (PIntermedio[0]-pose[0])); // Angulo entre o ponto intermedio e o robo
henkiwan 12:348038b466a3 183
henkiwan 12:348038b466a3 184 wRobot = k_s * atan2(sin(phi-pose[2]), cos(phi-pose[2])); //velocidade angular do robo;
henkiwan 12:348038b466a3 185
henkiwan 12:348038b466a3 186 // Velocidades das rodas
henkiwan 12:348038b466a3 187 velRobot2velWheels(vRobot,wRobot,wheelsRadius,wheelsDistance,w);
henkiwan 12:348038b466a3 188
henkiwan 12:348038b466a3 189 if (w[0] > 200)
henkiwan 12:348038b466a3 190 w[0]=200;
henkiwan 12:348038b466a3 191
henkiwan 12:348038b466a3 192 if (w[1] > 200)
henkiwan 12:348038b466a3 193 w[1]=200;
henkiwan 12:348038b466a3 194
henkiwan 12:348038b466a3 195 setSpeeds(w[0], w[1]);
henkiwan 12:348038b466a3 196
henkiwan 12:348038b466a3 197 nextPose(countsLeft, countsRight, wheelsRadius, wheelsDistance, pose);
henkiwan 12:348038b466a3 198
henkiwan 12:348038b466a3 199 leituras = 0;
henkiwan 12:348038b466a3 200
henkiwan 12:348038b466a3 201 while(leituras < 5){
henkiwan 12:348038b466a3 202 if(lidar.pollSensorData(&data) == 0)
henkiwan 12:348038b466a3 203 {
henkiwan 12:348038b466a3 204 //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
henkiwan 12:348038b466a3 205
henkiwan 12:348038b466a3 206 float radians = (data.angle * static_cast<float>(PI))/180.0f;
henkiwan 12:348038b466a3 207
henkiwan 12:348038b466a3 208 LidarP[0] = -data.distance*sin(radians)- 2.8f;
henkiwan 12:348038b466a3 209 LidarP[1] = -data.distance*cos(radians)- 1.5f;
henkiwan 9:76b59c5220f1 210
henkiwan 12:348038b466a3 211 //W_P = R_WP * p_P
henkiwan 12:348038b466a3 212 LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm
henkiwan 12:348038b466a3 213 LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
henkiwan 12:348038b466a3 214
henkiwan 12:348038b466a3 215 // pontos onde o feixe passou
henkiwan 12:348038b466a3 216 bresenham(pose[0]/5.0f, pose[1]/5.0f, LidarW[0]/5.0f, LidarW[1]/5.0f, data.distance/5.0f);
henkiwan 12:348038b466a3 217
henkiwan 12:348038b466a3 218 leituras++;
henkiwan 12:348038b466a3 219 }
henkiwan 12:348038b466a3 220 }
henkiwan 12:348038b466a3 221
henkiwan 12:348038b466a3 222 wait(T); // Delay of 0.5 seconds.
henkiwan 12:348038b466a3 223 }
henkiwan 12:348038b466a3 224 myled=1;
henkiwan 12:348038b466a3 225 setSpeeds(0, 0);
ppovoa 8:ad8766cf2ec0 226 rplidar_motor.write(0.0f);
henkiwan 9:76b59c5220f1 227 }
henkiwan 9:76b59c5220f1 228
henkiwan 9:76b59c5220f1 229 void bresenham(float poseX, float poseY, float xf, float yf, float z){
henkiwan 9:76b59c5220f1 230 int T, E, A, B;
henkiwan 12:348038b466a3 231
henkiwan 9:76b59c5220f1 232 int x = static_cast<int>(poseX);
henkiwan 9:76b59c5220f1 233 int y = static_cast<int>(poseY);
henkiwan 12:348038b466a3 234 int dx = abs(static_cast<int>(xf - poseX));
henkiwan 12:348038b466a3 235 int dy = abs(static_cast<int>(yf - poseY));
henkiwan 9:76b59c5220f1 236
henkiwan 12:348038b466a3 237 int s1;
henkiwan 12:348038b466a3 238 int s2;
henkiwan 12:348038b466a3 239 // substitui o sign() do matlab
henkiwan 12:348038b466a3 240 if(static_cast<int>(xf - poseX) > 0)
henkiwan 12:348038b466a3 241 s1 = 1;
henkiwan 12:348038b466a3 242 else if (static_cast<int>(xf - poseX) == 0)
henkiwan 12:348038b466a3 243 s1 = 0;
henkiwan 12:348038b466a3 244 else
henkiwan 12:348038b466a3 245 s1 = -1;
henkiwan 12:348038b466a3 246
henkiwan 12:348038b466a3 247 if(static_cast<int>(yf - poseY) > 0)
henkiwan 12:348038b466a3 248 s2 = 1;
henkiwan 12:348038b466a3 249 else if (static_cast<int>(yf - poseY) == 0)
henkiwan 12:348038b466a3 250 s2 = 0;
henkiwan 12:348038b466a3 251 else
henkiwan 12:348038b466a3 252 s2 = -1;
henkiwan 12:348038b466a3 253
henkiwan 9:76b59c5220f1 254
henkiwan 9:76b59c5220f1 255 int interchange = 0;
henkiwan 9:76b59c5220f1 256
henkiwan 9:76b59c5220f1 257 if (dy > dx){
henkiwan 9:76b59c5220f1 258 T = dx;
henkiwan 9:76b59c5220f1 259 dx = dy;
henkiwan 9:76b59c5220f1 260 dy = T;
henkiwan 9:76b59c5220f1 261 interchange = 1;
henkiwan 9:76b59c5220f1 262 }
henkiwan 9:76b59c5220f1 263
henkiwan 9:76b59c5220f1 264 E = 2*dy - dx;
henkiwan 9:76b59c5220f1 265 A = 2*dy;
henkiwan 9:76b59c5220f1 266 B = 2*dy - 2*dx;
henkiwan 9:76b59c5220f1 267
henkiwan 9:76b59c5220f1 268 for (int i = 0; i<dx; i++){
henkiwan 9:76b59c5220f1 269 if (E < 0){
henkiwan 9:76b59c5220f1 270 if (interchange == 1){
henkiwan 9:76b59c5220f1 271 y = y + s2;
henkiwan 9:76b59c5220f1 272 }
henkiwan 9:76b59c5220f1 273 else{
henkiwan 9:76b59c5220f1 274 x = x + s1;
henkiwan 9:76b59c5220f1 275 }
henkiwan 9:76b59c5220f1 276 E = E + A;
henkiwan 9:76b59c5220f1 277 }
henkiwan 9:76b59c5220f1 278
henkiwan 9:76b59c5220f1 279 else{
henkiwan 9:76b59c5220f1 280 y = y + s2;
henkiwan 9:76b59c5220f1 281 x = x + s1;
henkiwan 9:76b59c5220f1 282 E = E + B;
henkiwan 9:76b59c5220f1 283 }
henkiwan 9:76b59c5220f1 284
henkiwan 9:76b59c5220f1 285 if (x >= 0 && y >= 0 && x < 40 && y < 40){
henkiwan 9:76b59c5220f1 286 // Mapear mapa do Logaritmo
henkiwan 12:348038b466a3 287 MapaLog[x][y] = MapaLog[x][y] + Algorithm_Inverse(poseX, poseY, x, y, z);
ppovoa 10:6c8ea68e9bac 288 //pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
henkiwan 9:76b59c5220f1 289 Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
henkiwan 9:76b59c5220f1 290 }
henkiwan 9:76b59c5220f1 291
henkiwan 9:76b59c5220f1 292
henkiwan 9:76b59c5220f1 293
ppovoa 4:256f2cbe3fdd 294 }
fabiofaria 1:dc87724abce8 295 }