File content as of revision 0:b21301d612b9:

#include "odometry.h"
#include <math.h>
Odom::Odom() {
}

void Odom::init(){
    // Hardware Parameters
    radius = 0.075;
    width  = 0.045;
    // Odom Variables
    x   = 0.0;
    y   = 0.0;
    th  = 0.0;
    vx  = 0.0;
    vy  = 0.0;
    vth = 0.0;
    // Calculation Parameters
    enc_left  = 0.0;
    enc_right = 0.0;
    delta_distance_left  = 0.0;
    delta_distance_right = 0.0;
}

void Odom::get_vel(float v, float w){
    vx  = v * cos(th);
    vy  = v * sin(th);
    vth = w;
}

void Odom::get_distance(float new_enc_left, float new_enc_right){
    float delta_enc_left = new_enc_left - enc_left;
    delta_distance_left = delta_enc_left * radius;
    enc_right = new_enc_left;

    float delta_enc_right = new_enc_right - enc_right;
    delta_distance_right = delta_enc_right * radius;
    enc_right = new_enc_right;
}

void Odom::odom_calculation(){
    float delta_distance_center = ( delta_distance_right - delta_distance_left ) / 2;
    x  += delta_distance_center * cos(th);
    y  += delta_distance_center * sin(th);
    th += ( delta_distance_right - delta_distance_left ) / width;
}