Odometry not changing any position values

Programming Support
1

Could anyone help me with odometry
this is my code so far

My Podomwheel is my paralel odometry wheel
Hodomwheel is my horizontal odom whell
ponty is my inertial
I have a global variable of
double position[2];
there is no change in position, IE my position does not change at all, it just stays at 0;
Thank you. -Here be the code

int odom() {
    
   
    double verticalwheel[4] = {Podomwheel.position(vex::rotationUnits::deg),0,0}; 
    double horizontalwheel[4] = {Hodomwheel.position(vex::rotationUnits::deg),0,0};
    double positionalmath[5];
    double anglediff[6];
    odomtrack=true;
    while (odomtrack)
    {
verticalwheel[0] = Podomwheel.position(vex::rotationUnits::deg);
horizontalwheel[0] = Hodomwheel.position(vex::rotationUnits::deg);
anglediff[0] = ponty.rotation(vex::rotationUnits::deg)*(M_PI/360);
anglediff[3] = ponty.heading(vex::rotationUnits::deg)*(M_PI/360);
verticalwheel[2] = (verticalwheel[0]-verticalwheel[1]);
horizontalwheel[2] = (horizontalwheel[0]-horizontalwheel[1]);
anglediff[2] = (anglediff[0]-anglediff[1]);
anglediff[1]= anglediff[0];
verticalwheel[1] =  verticalwheel[0];
horizontalwheel[1] = horizontalwheel[0];



anglediff[4] = anglediff[3]+anglediff[2]/2;

if (anglediff[2] !=0) {
positionalmath[0] = 2*sin(anglediff[3])*(horizontalwheel[2]/anglediff[2]);
positionalmath[1] = 2*sin(anglediff[3])*(verticalwheel[2]/anglediff[2]);
} else {

    

positionalmath[0]=horizontalwheel[2];
positionalmath[1]=verticalwheel[2];

}
positionalmath[2] = sqrt(pow(positionalmath[0],2) +pow(positionalmath[1],2)); 
positionalmath[3] = atan(positionalmath[1] / positionalmath[0]);

positionalmath[0] = positionalmath[2] * cos(positionalmath[3]-anglediff[4]); 
positionalmath[1] = positionalmath[2] * sin(positionalmath[3]-anglediff[4]);



position[0] += positionalmath[0];
position[1] += positionalmath[1];
wait(25,msec);
    }
    
}

edit: code tags added by mods, please remember to use them

2

You are setting anglediff[1] after using it, which will cause anglediff[2] to be a random garbage value. From testing, a garbage double is likely to be either 0 or a really (REALLY; ~10^-312) small double, so anglediff[2] will be virtually equal to anglediff[0]

Since anglediff[2] is virtually equal to anglediff[0] your math is probably getting messed up.