Henrique Cardoso
Experiencias do Henrique na quinta a noite
Functions.cpp@12:348038b466a3, 2021-05-14 (annotated)
- Committer:
- henkiwan
- Date:
- Fri May 14 05:52:55 2021 +0000
- Revision:
- 12:348038b466a3
- Parent:
- 11:58187c7de709
Experiencias que fiz de noite
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| henkiwan | 12:348038b466a3 | 1 | |
| ppovoa | 4:256f2cbe3fdd | 2 | #include <math.h> |
| ppovoa | 4:256f2cbe3fdd | 3 | |
| ppovoa | 4:256f2cbe3fdd | 4 | void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2]) |
| ppovoa | 4:256f2cbe3fdd | 5 | { |
| ppovoa | 4:256f2cbe3fdd | 6 | w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius; |
| ppovoa | 4:256f2cbe3fdd | 7 | w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius; |
| ppovoa | 4:256f2cbe3fdd | 8 | } |
| ppovoa | 4:256f2cbe3fdd | 9 | |
| ppovoa | 4:256f2cbe3fdd | 10 | |
| ppovoa | 4:256f2cbe3fdd | 11 | void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[3]) |
| ppovoa | 4:256f2cbe3fdd | 12 | { |
| ppovoa | 4:256f2cbe3fdd | 13 | // Deslocamentos |
| ppovoa | 4:256f2cbe3fdd | 14 | float d_l, d_r, desl, delta_ang, delta_x, delta_y; |
| ppovoa | 4:256f2cbe3fdd | 15 | |
| henkiwan | 12:348038b466a3 | 16 | d_l = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f )); |
| henkiwan | 12:348038b466a3 | 17 | d_r = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f )); |
| ppovoa | 4:256f2cbe3fdd | 18 | |
| henkiwan | 12:348038b466a3 | 19 | desl = (d_l+d_r)/2; |
| ppovoa | 4:256f2cbe3fdd | 20 | |
| ppovoa | 4:256f2cbe3fdd | 21 | |
| ppovoa | 4:256f2cbe3fdd | 22 | delta_ang = (d_r-d_l)/wheelsDistance; |
| ppovoa | 4:256f2cbe3fdd | 23 | |
| henkiwan | 12:348038b466a3 | 24 | delta_x = desl * cos(pose[2]+delta_ang/2); |
| henkiwan | 12:348038b466a3 | 25 | delta_y = desl * sin(pose[2]+delta_ang/2); |
| ppovoa | 4:256f2cbe3fdd | 26 | |
| ppovoa | 4:256f2cbe3fdd | 27 | |
| ppovoa | 4:256f2cbe3fdd | 28 | pose[0] = pose[0] + delta_x; |
| ppovoa | 4:256f2cbe3fdd | 29 | pose[1] = pose[1] + delta_y; |
| ppovoa | 4:256f2cbe3fdd | 30 | pose[2] = pose[2] + delta_ang; |
| ppovoa | 4:256f2cbe3fdd | 31 | } |
| ppovoa | 4:256f2cbe3fdd | 32 | |
| henkiwan | 12:348038b466a3 | 33 | float Algorithm_Inverse(float xi, float yi, float xt, float yt, float z){ |
| ppovoa | 4:256f2cbe3fdd | 34 | |
| ppovoa | 4:256f2cbe3fdd | 35 | float z_max = 200; // 2 m |
| ppovoa | 11:58187c7de709 | 36 | float alfa = 6.0f/5.0f; |
| ppovoa | 4:256f2cbe3fdd | 37 | //float beta = 1; // 1 grau |
| ppovoa | 4:256f2cbe3fdd | 38 | float L0 = 0.0; |
| ppovoa | 4:256f2cbe3fdd | 39 | float Locc = 0.65; |
| ppovoa | 4:256f2cbe3fdd | 40 | float Lfree = -0.65; |
| ppovoa | 4:256f2cbe3fdd | 41 | float L; |
| ppovoa | 4:256f2cbe3fdd | 42 | |
| ppovoa | 4:256f2cbe3fdd | 43 | float r = sqrt( pow((xi-xt),2) + pow((yi-yt),2) ); |
| ppovoa | 4:256f2cbe3fdd | 44 | //phi = atan2( yi-yt, xi-xt ) - theta; |
| ppovoa | 4:256f2cbe3fdd | 45 | |
| ppovoa | 4:256f2cbe3fdd | 46 | //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2) |
| ppovoa | 4:256f2cbe3fdd | 47 | //L = L0; |
| ppovoa | 11:58187c7de709 | 48 | if ((z < z_max) && (abs(r-z) < alfa/2.0f)) |
| ppovoa | 4:256f2cbe3fdd | 49 | L = Locc; |
| ppovoa | 4:256f2cbe3fdd | 50 | else if (r <= z) |
| ppovoa | 4:256f2cbe3fdd | 51 | L = Lfree; |
| ppovoa | 4:256f2cbe3fdd | 52 | else |
| ppovoa | 4:256f2cbe3fdd | 53 | L = L0; |
| ppovoa | 4:256f2cbe3fdd | 54 | |
| ppovoa | 4:256f2cbe3fdd | 55 | return L; |
| ppovoa | 4:256f2cbe3fdd | 56 | |
| henkiwan | 12:348038b466a3 | 57 | } |