Functions.cpp

00001 
00002 #include <math.h>
00003 
00004 void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
00005 {
00006     w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius;
00007     w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius;
00008 }
00009 
00010 
00011 void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[3])
00012 {
00013     // Deslocamentos
00014     float d_l, d_r, desl, delta_ang, delta_x, delta_y;
00015     
00016     d_l = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f ));
00017     d_r = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f ));
00018     
00019     desl = (d_l+d_r)/2;
00020     
00021     
00022     delta_ang = (d_r-d_l)/wheelsDistance;
00023     
00024     delta_x = desl * cos(pose[2]+delta_ang/2);
00025     delta_y = desl * sin(pose[2]+delta_ang/2);
00026     
00027     
00028     pose[0] = pose[0] + delta_x;
00029     pose[1] = pose[1] + delta_y;
00030     pose[2] = pose[2] + delta_ang;
00031 }
00032 
00033 float Algorithm_Inverse(float xi, float yi, float xt, float yt, float z){
00034     
00035     float z_max = 200; // 2 m
00036     float alfa = 6.0f/5.0f;
00037     //float beta = 1; // 1 grau
00038     float L0 = 0.0;
00039     float Locc = 0.65;
00040     float Lfree = -0.65;
00041     float L;
00042     
00043     float r = sqrt( pow((xi-xt),2) + pow((yi-yt),2) );
00044     //phi = atan2( yi-yt, xi-xt ) - theta;
00045     
00046     //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
00047         //L = L0;
00048     if ((z < z_max) && (abs(r-z) < alfa/2.0f))
00049         L = Locc;
00050     else if (r <= z)
00051         L = Lfree;
00052     else
00053         L = L0;
00054     
00055     return L;
00056     
00057 }