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

Committer:
ppovoa
Date:
Wed May 12 19:00:45 2021 +0000
Revision:
10:6c8ea68e9bac
Parent:
9:76b59c5220f1
Child:
11:58187c7de709
Lidar

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 10:6c8ea68e9bac 38 float alfa = 10;
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;
henkiwan 9:76b59c5220f1 50 if ((z < z_max) && (abs(r-z) < 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