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@7:f1c122bc63c8, 2021-05-10 (annotated)

Committer:
ppovoa
Date:
Mon May 10 15:23:24 2021 +0000
Revision:
7:f1c122bc63c8
Parent:
6:59fbbeaac2af
Child:
8:ad8766cf2ec0
10/05/2021

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 float z_max = 200; // 2 m
ppovoa 4:256f2cbe3fdd 39 float alfa = 5; // 5 cm
ppovoa 4:256f2cbe3fdd 40 //float beta = 1; // 1 grau
ppovoa 4:256f2cbe3fdd 41 float L0 = 0.0;
ppovoa 4:256f2cbe3fdd 42 float Locc = 0.65;
ppovoa 4:256f2cbe3fdd 43 float Lfree = -0.65;
ppovoa 4:256f2cbe3fdd 44 float L;
ppovoa 4:256f2cbe3fdd 45
ppovoa 4:256f2cbe3fdd 46 float r = sqrt( pow((xi-xt),2) + pow((yi-yt),2) );
ppovoa 4:256f2cbe3fdd 47 //phi = atan2( yi-yt, xi-xt ) - theta;
ppovoa 4:256f2cbe3fdd 48
ppovoa 4:256f2cbe3fdd 49 //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
ppovoa 4:256f2cbe3fdd 50 //L = L0;
ppovoa 4:256f2cbe3fdd 51 if ((z < z_max) && (abs(r-z_max) < alfa/2.0))
ppovoa 4:256f2cbe3fdd 52 L = Locc;
ppovoa 4:256f2cbe3fdd 53 else if (r <= z)
ppovoa 4:256f2cbe3fdd 54 L = Lfree;
ppovoa 4:256f2cbe3fdd 55 else
ppovoa 4:256f2cbe3fdd 56 L = L0;
ppovoa 4:256f2cbe3fdd 57
ppovoa 4:256f2cbe3fdd 58 return L;
ppovoa 4:256f2cbe3fdd 59
ppovoa 4:256f2cbe3fdd 60 }
ppovoa 4:256f2cbe3fdd 61
ppovoa 7:f1c122bc63c8 62 void bresenham(int poseX, int poseY, int xf, int yf, float (&MapaLog)[40][40], float z){
ppovoa 7:f1c122bc63c8 63 //void cona(float poseX, float poseY, float x1, float y1, float z){
ppovoa 7:f1c122bc63c8 64 printf("bresenham!\n\r");
ppovoa 6:59fbbeaac2af 65 /*
ppovoa 5:bc42c03f2a23 66 int T, E, A, B;
ppovoa 7:f1c122bc63c8 67 int x = poseX;
ppovoa 7:f1c122bc63c8 68 int y = poseY;
ppovoa 7:f1c122bc63c8 69 int dx = abs(xf - poseX);
ppovoa 7:f1c122bc63c8 70 int dy = abs(yf - poseY);
ppovoa 4:256f2cbe3fdd 71
ppovoa 7:f1c122bc63c8 72 int s1 = (xf - poseX)/dx; // substitui o sign() do matlab
ppovoa 7:f1c122bc63c8 73 int s2 = (yf - poseY)/dy;
ppovoa 5:bc42c03f2a23 74
ppovoa 5:bc42c03f2a23 75 int interchange = 0;
ppovoa 5:bc42c03f2a23 76
ppovoa 5:bc42c03f2a23 77 if (dy > dx){
ppovoa 5:bc42c03f2a23 78 T = dx;
ppovoa 5:bc42c03f2a23 79 dx = dy;
ppovoa 5:bc42c03f2a23 80 dy = T;
ppovoa 5:bc42c03f2a23 81 interchange = 1;
ppovoa 4:256f2cbe3fdd 82 }
ppovoa 4:256f2cbe3fdd 83
ppovoa 5:bc42c03f2a23 84 E = 2*dy - dx;
ppovoa 5:bc42c03f2a23 85 A = 2*dy;
ppovoa 5:bc42c03f2a23 86 B = 2*dy - 2*dx;
ppovoa 5:bc42c03f2a23 87
ppovoa 5:bc42c03f2a23 88 for (int i = 0; i<dx; i++){
ppovoa 5:bc42c03f2a23 89 if (E < 0){
ppovoa 5:bc42c03f2a23 90 if (interchange == 1){
ppovoa 5:bc42c03f2a23 91 y = y + s2;
ppovoa 5:bc42c03f2a23 92 }
ppovoa 5:bc42c03f2a23 93 else{
ppovoa 5:bc42c03f2a23 94 x = x + s1;
ppovoa 5:bc42c03f2a23 95 }
ppovoa 5:bc42c03f2a23 96 E = E + A;
ppovoa 5:bc42c03f2a23 97 }
ppovoa 5:bc42c03f2a23 98
ppovoa 5:bc42c03f2a23 99 else{
ppovoa 5:bc42c03f2a23 100 y = y + s2;
ppovoa 5:bc42c03f2a23 101 x = x + s1;
ppovoa 5:bc42c03f2a23 102 E = E + B;
ppovoa 5:bc42c03f2a23 103 }
ppovoa 5:bc42c03f2a23 104
ppovoa 5:bc42c03f2a23 105 // Mapear mapa do Logaritmo
ppovoa 6:59fbbeaac2af 106 //MapaLog[x][y] = MapaLog[x][y]; //+ Algorith_Inverse(poseX, poseY, x, y, z);
ppovoa 5:bc42c03f2a23 107
ppovoa 5:bc42c03f2a23 108 }
ppovoa 6:59fbbeaac2af 109 */
ppovoa 5:bc42c03f2a23 110
ppovoa 5:bc42c03f2a23 111 }