File content as of revision 5:e189d55ef292:

#include "Odmetry.hpp"

Odmetry::Odmetry(){
    dt = 0.05;
    Radius = 395;
    Wradius = 25;
    XPos = 0;
    YPos = 0;
}

void Odmetry::pass_pulse(double PULSE1,double PULSE2,double PULSE3,double PULSE4){
    pulse_a[0] = PULSE1;
    pulse_a[1] = PULSE2;
    pulse_a[2] = PULSE3;
    pulse_a[3] = PULSE4;
    
    if(PULSE1 == PULSE2 == PULSE3 == PULSE4){
        return;
    }
    else{
        CalcSpd();
        Angular();
        transform();
        CalcPosition();
    }
}

void Odmetry::pass_angle(double THETA){
    Theta = THETA - PI / 4;
}
    

void Odmetry::CalcSpd(){
    for(int i = 0;i < 4;i ++){
        diff[i] = pulse_a[i] - pulse_b[i];
        spd[i] = Co * diff[i] / dt;
        pulse_b[i] = pulse_a[i];
    }
}

void Odmetry::Angular(){
    for(int i = 0;i < 4;i ++){
        Omega[i] = spd[i] / Radius;
    }
}

void Odmetry::transform(){
    Xpower = Wradius * (Omega[1] - Omega[3]);
    Ypower = Wradius * (Omega[0] - Omega[2]);
}

void Odmetry::CalcPosition(){
    Xspd = cos(Theta) * Xpower - sin(Theta) * Ypower;
    Yspd = sin(Theta) * Xpower + cos(Theta) * Ypower;
    
    XPos += Xspd * dt;
    YPos += Yspd * dt;
}

void Odmetry::print_Pos(){ printf("XPos:%lf YPos:%lf theta:%lf\n",XPos,YPos,Theta); }