Chief Delphi - Casting Floats, printf's, and variable maxing

Ive been having a problem, and I wanted to wait until I was absolutely sure I didnt know how to fix it before posting it here. Well, guess what... I've hit that benchmark. We are implementing a simple navigation system on our robot this year, which is used to keep track of position and give drive commands to drive to waypoints.

This system grabs current angle from an on board gyro, and distance traveled in the sample period from shaft encoders. I have been testing this on an EDU controller, and everything worked flawlessly. But when I moved my code over to the FRC controller, I started getting some fishy outcomes.

I have a function that is called every program loop to keep track of position, then a printf in user routines.c that shows current x, y, and heading variables for debugging purposes.

Heres the navigation function:

Code:

int nav_heading;

int new_pos;

int old_pos;

int dist=0;

int nav_x;

int nav_y;



void Navigation(void)

{

        

        nav_heading = Get_Gyro_Angle();

        

        new_pos = (int)( Get_Left_Encoder_Count() + Get_Right_Encoder_Count() )/2;  

        

        dist = (new_pos - old_pos);



        nav_x+= (int) cos((float) nav_heading /1000) * dist;



        nav_y+= (int) sin((float)nav_heading/1000) * dist;



        

        old_pos = new_pos;

}



int Nav_Get_x(void)

{

        return nav_x;

}



int Nav_Get_y(void)

{

        return nav_y;

}

Heres the print:

Code:

printf("X = %d | Y = %d\r", Nav_Get_x(), Nav_Get_y());

Now, the problem I am getting is as soon as the gyro turns at all, both X and Y values go to the maximum value allowed in an integer (363363 or something ). The cos and sin functions take and output floats. I've tried casting both the arguments and the returns as floats, but that didnt help. Another interesting point is when I put the printf in the actual navigation function, I dont seem to have this problem.

Do you have any idea what is wrong?