File content as of revision 0:402f8c75d394:

#include "Odometry.h"

Robot::Robot(float wheelbase, float width, float countPerMeter)
{
    _whbs = wheelbase;
    _w = width;
    _countPerMeter = countPerMeter;
    
    _odom.resize(3);
    _ratio.resize(2);
}

float Robot::getRadius(float servoAngle)
{    
    if( abs(servoAngle - (M_PI/2) ) < 0.05 )
        _R = 999.9;       
    else {
        float alfa = servoAngle;
        float beta = alfa - (M_PI/2);
        _R = (_whbs / sin(beta)) * sin(alfa);         
    }
    return _R;
}

void Robot::countRadius(float servoAngle)
{    
    if( abs(servoAngle - (M_PI/2) ) < 0.05 )
        _R = 999.9;       
    else {
        float alfa = servoAngle;
        float beta = alfa - (M_PI/2);
        _R = (_whbs / sin(beta)) * sin(alfa);         
    }
}

float Robot::getServoAngle(float R)
{    
    R = -R;
    float beta = atan(_whbs / R);
    float servoAngle = (M_PI / 2) - beta;
    return servoAngle;
}

std::vector<float> Robot::getSpeed(std::vector<int> encoders) 
{    
    std::vector<float> data(3);
    float v, v1, v2, fi;
    
    v1 =  ((float)encoders[0] * 125.0)/(_countPerMeter); // left wheel
    v2 =  ((float)encoders[1] * 125.0)/(_countPerMeter); // right wheel
    
    v = (v1+v2)/2;
    
    if( abs(v) < 0.001 ) {
        _odom[0] = 0.0;
        _odom[1] = 0.0;
        _odom[2] = 0.0;
    }
    else {
        fi = v/_R;
        _odom[0] = _R*sin(fi);
        _odom[1] = _R*(1-cos(fi));
        _odom[2] = fi;
    }

    data[0] = _odom[0];
    data[1] = _odom[1];
    data[2] = _odom[2];
    
    return data;
}


std::vector<float> Robot::getMotorRatio()
{
  _ratio[0] = (1 - (_w/(2*_R)));
  _ratio[1] = (1 + (_w/(2*_R)));

  return _ratio;
}

std::vector<float> Robot::getMotorRatio(float R)
{
  _ratio[0] = (1 - (_w/(2*R)));
  _ratio[1] = (1 + (_w/(2*R)));

  return _ratio;
}