File content as of revision 1:1765665581cc:

#include "Odometry.h"

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

float Robot::getRadius(float v1, float v2)
{   
    if( abs(v2-v1) < 0.0001 )
        _R = 99.9;
    else
        _R = (_w/2) - ( (_w * v2) / (v2 - v1));
    return _R;
}

void Robot::countRadius(float v1, float v2)
{   
    if( abs(v2-v1) < 0.0001 )
        _R = 99.9;
    else 
        _R = (_w/2) - ( (_w * v2) / (v2 - v1));
}

std::vector<float> Robot::getSpeed(std::vector<int> encoders) 
{    
    float v1, v2, fi;
    
    v1 =  ((float)encoders[0])/(_countPerMeter); // left wheel
    v2 =  ((float)encoders[1])/(_countPerMeter); // right wheel
    
    if( abs(v2-v1) < 0.0001 ) {
        _R = 99.9;
    }
    else {    
        _R = (_w/2) - ( (_w * v2) / (v2 - v1));
    }
    
    fi = atan((v1-v2)/_w);
    
    _odom[0] = _R * sin(fi);
    _odom[1] = -tan(fi) * _odom[0];
    _odom[2] = -fi;
    
    return _odom;
}


std::vector<float> Robot::getMotorRatio(float x, float z)
{
  float fi = ( abs(z) < 0.0001) ? 0.0001 : z;
  
  float R = x / sin(fi);
  
  _ratio[0] = tan(fi) * ( (_w/2) + R );
  _ratio[1] = -tan(fi) * ( (_w/2) - R );
  
  return _ratio;
}