Henrique Cardoso / Mbed OS Lidar_Rodas

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

Functions.cpp@8:ad8766cf2ec0, 2021-05-11 (annotated)

Committer:
ppovoa
Date:
Tue May 11 15:05:49 2021 +0000
Revision:
8:ad8766cf2ec0
Parent:
7:f1c122bc63c8
Child:
9:76b59c5220f1
SRA - erro no sendodometry

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ppovoa 4:256f2cbe3fdd 1 #include <math.h>
ppovoa 5:bc42c03f2a23 2 #include <stdio.h>
ppovoa 8:ad8766cf2ec0 3 #include "Functions.h"
ppovoa 4:256f2cbe3fdd 4
ppovoa 4:256f2cbe3fdd 5 void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
ppovoa 4:256f2cbe3fdd 6 {
ppovoa 4:256f2cbe3fdd 7 w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius;
ppovoa 4:256f2cbe3fdd 8 w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius;
ppovoa 4:256f2cbe3fdd 9 }
ppovoa 4:256f2cbe3fdd 10
ppovoa 4:256f2cbe3fdd 11
ppovoa 4:256f2cbe3fdd 12 void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[3])
ppovoa 4:256f2cbe3fdd 13 {
ppovoa 4:256f2cbe3fdd 14 // Deslocamentos
ppovoa 4:256f2cbe3fdd 15 float d_l, d_r, desl, delta_ang, delta_x, delta_y;
ppovoa 4:256f2cbe3fdd 16
ppovoa 4:256f2cbe3fdd 17 d_l = 2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f );
ppovoa 4:256f2cbe3fdd 18 d_r = 2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f );
ppovoa 4:256f2cbe3fdd 19
ppovoa 4:256f2cbe3fdd 20 desl = (d_l+d_r)/2.0f;
ppovoa 4:256f2cbe3fdd 21
ppovoa 4:256f2cbe3fdd 22
ppovoa 4:256f2cbe3fdd 23 delta_ang = (d_r-d_l)/wheelsDistance;
ppovoa 4:256f2cbe3fdd 24
ppovoa 4:256f2cbe3fdd 25 delta_x = desl * cos(pose[2]+delta_ang/2.0f);
ppovoa 4:256f2cbe3fdd 26 delta_y = desl * sin(pose[2]+delta_ang/2.0f);
ppovoa 4:256f2cbe3fdd 27
ppovoa 4:256f2cbe3fdd 28
ppovoa 4:256f2cbe3fdd 29 pose[0] = pose[0] + delta_x;
ppovoa 4:256f2cbe3fdd 30 pose[1] = pose[1] + delta_y;
ppovoa 4:256f2cbe3fdd 31 pose[2] = pose[2] + delta_ang;
ppovoa 4:256f2cbe3fdd 32 }
ppovoa 4:256f2cbe3fdd 33
ppovoa 4:256f2cbe3fdd 34
ppovoa 4:256f2cbe3fdd 35 float Algorith_Inverse(float xi, float yi, float xt, float yt, float z){
ppovoa 4:256f2cbe3fdd 36
ppovoa 4:256f2cbe3fdd 37 float z_max = 200; // 2 m
ppovoa 4:256f2cbe3fdd 38 float alfa = 5; // 5 cm
ppovoa 4:256f2cbe3fdd 39 //float beta = 1; // 1 grau
ppovoa 4:256f2cbe3fdd 40 float L0 = 0.0;
ppovoa 4:256f2cbe3fdd 41 float Locc = 0.65;
ppovoa 4:256f2cbe3fdd 42 float Lfree = -0.65;
ppovoa 4:256f2cbe3fdd 43 float L;
ppovoa 4:256f2cbe3fdd 44
ppovoa 4:256f2cbe3fdd 45 float r = sqrt( pow((xi-xt),2) + pow((yi-yt),2) );
ppovoa 4:256f2cbe3fdd 46 //phi = atan2( yi-yt, xi-xt ) - theta;
ppovoa 4:256f2cbe3fdd 47
ppovoa 4:256f2cbe3fdd 48 //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
ppovoa 4:256f2cbe3fdd 49 //L = L0;
ppovoa 4:256f2cbe3fdd 50 if ((z < z_max) && (abs(r-z_max) < alfa/2.0))
ppovoa 4:256f2cbe3fdd 51 L = Locc;
ppovoa 4:256f2cbe3fdd 52 else if (r <= z)
ppovoa 4:256f2cbe3fdd 53 L = Lfree;
ppovoa 4:256f2cbe3fdd 54 else
ppovoa 4:256f2cbe3fdd 55 L = L0;
ppovoa 4:256f2cbe3fdd 56
ppovoa 4:256f2cbe3fdd 57 return L;
ppovoa 4:256f2cbe3fdd 58
ppovoa 4:256f2cbe3fdd 59 }
ppovoa 4:256f2cbe3fdd 60
ppovoa 8:ad8766cf2ec0 61 void bresenham(float poseX, float poseY, float xf, float yf, float z){
ppovoa 5:bc42c03f2a23 62 int T, E, A, B;
ppovoa 8:ad8766cf2ec0 63 int x = static_cast<int>(poseX);
ppovoa 8:ad8766cf2ec0 64 int y = static_cast<int>(poseY);
ppovoa 8:ad8766cf2ec0 65 int dx = static_cast<int>(abs(xf - poseX));
ppovoa 8:ad8766cf2ec0 66 int dy = static_cast<int>(abs(yf - poseY));
ppovoa 4:256f2cbe3fdd 67
ppovoa 8:ad8766cf2ec0 68 int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
ppovoa 8:ad8766cf2ec0 69 int s2 = static_cast<int>((yf - poseY)/dy);
ppovoa 5:bc42c03f2a23 70
ppovoa 5:bc42c03f2a23 71 int interchange = 0;
ppovoa 5:bc42c03f2a23 72
ppovoa 5:bc42c03f2a23 73 if (dy > dx){
ppovoa 5:bc42c03f2a23 74 T = dx;
ppovoa 5:bc42c03f2a23 75 dx = dy;
ppovoa 5:bc42c03f2a23 76 dy = T;
ppovoa 5:bc42c03f2a23 77 interchange = 1;
ppovoa 4:256f2cbe3fdd 78 }
ppovoa 4:256f2cbe3fdd 79
ppovoa 5:bc42c03f2a23 80 E = 2*dy - dx;
ppovoa 5:bc42c03f2a23 81 A = 2*dy;
ppovoa 5:bc42c03f2a23 82 B = 2*dy - 2*dx;
ppovoa 5:bc42c03f2a23 83
ppovoa 5:bc42c03f2a23 84 for (int i = 0; i<dx; i++){
ppovoa 5:bc42c03f2a23 85 if (E < 0){
ppovoa 5:bc42c03f2a23 86 if (interchange == 1){
ppovoa 5:bc42c03f2a23 87 y = y + s2;
ppovoa 5:bc42c03f2a23 88 }
ppovoa 5:bc42c03f2a23 89 else{
ppovoa 5:bc42c03f2a23 90 x = x + s1;
ppovoa 5:bc42c03f2a23 91 }
ppovoa 5:bc42c03f2a23 92 E = E + A;
ppovoa 5:bc42c03f2a23 93 }
ppovoa 5:bc42c03f2a23 94
ppovoa 5:bc42c03f2a23 95 else{
ppovoa 5:bc42c03f2a23 96 y = y + s2;
ppovoa 5:bc42c03f2a23 97 x = x + s1;
ppovoa 5:bc42c03f2a23 98 E = E + B;
ppovoa 5:bc42c03f2a23 99 }
ppovoa 5:bc42c03f2a23 100
ppovoa 8:ad8766cf2ec0 101 if (x >= 0 && y >= 0 && x < 40 && y < 40){
ppovoa 8:ad8766cf2ec0 102 // Mapear mapa do Logaritmo
ppovoa 8:ad8766cf2ec0 103 MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
ppovoa 8:ad8766cf2ec0 104 Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
ppovoa 8:ad8766cf2ec0 105 }
ppovoa 5:bc42c03f2a23 106
ppovoa 8:ad8766cf2ec0 107
ppovoa 8:ad8766cf2ec0 108
ppovoa 5:bc42c03f2a23 109 }
ppovoa 5:bc42c03f2a23 110 }