File content as of revision 12:348038b466a3:

#include <math.h>

void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
{
    w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius;
    w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius;
}


void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[3])
{
    // Deslocamentos
    float d_l, d_r, desl, delta_ang, delta_x, delta_y;
    
    d_l = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f ));
    d_r = static_cast<float>(2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f ));
    
    desl = (d_l+d_r)/2;
    
    
    delta_ang = (d_r-d_l)/wheelsDistance;
    
    delta_x = desl * cos(pose[2]+delta_ang/2);
    delta_y = desl * sin(pose[2]+delta_ang/2);
    
    
    pose[0] = pose[0] + delta_x;
    pose[1] = pose[1] + delta_y;
    pose[2] = pose[2] + delta_ang;
}

float Algorithm_Inverse(float xi, float yi, float xt, float yt, float z){
    
    float z_max = 200; // 2 m
    float alfa = 6.0f/5.0f;
    //float beta = 1; // 1 grau
    float L0 = 0.0;
    float Locc = 0.65;
    float Lfree = -0.65;
    float L;
    
    float r = sqrt( pow((xi-xt),2) + pow((yi-yt),2) );
    //phi = atan2( yi-yt, xi-xt ) - theta;
    
    //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
        //L = L0;
    if ((z < z_max) && (abs(r-z) < alfa/2.0f))
        L = Locc;
    else if (r <= z)
        L = Lfree;
    else
        L = L0;
    
    return L;
    
}