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@5:bc42c03f2a23, 2021-05-07 (annotated)

Committer:
ppovoa
Date:
Fri May 07 14:58:46 2021 +0000
Revision:
5:bc42c03f2a23
Parent:
4:256f2cbe3fdd
Child:
6:59fbbeaac2af
alteracoes no Bresenham.

Who changed what in which revision?

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