[gnormhurst]

I'm not familiar with the float trig functions, so I'm no help there.

Should pos_x and pos_y be static variables? Are you expecting their values to be remembered from call-to-call?

Can you post some more code? The two lines you posted don't tell us much.

Here's a definitive way to determine how much time your code is taking, but you will need an oscilloscope and will need to know how to use it.

Just before the call to Process_Data_From_Master_uP(), set one of the digital output pins to '1'. At the end of every main loop, reset the pin to '0' (see code below).

Run your code and look at the pin with a scope. You should see a positive-going pulse every 26 ms. The duration (width) of the pulse is the duration of your code's execution. You may see the width change as you operate the robot. However, the frequency of the pulse should be a constant 38 Hz. Be sure to trigger the scope on the rising edge of the pulse.

Don't forget to configure your chosen output pin as an output.

Here's code I used last year, Note the two lines with // ***::

  while (1)   /* This loop will repeat indefinitely. */
  {

    if (statusflag.NEW_SPI_DATA)      /* 26.2ms loop area */
    {                                 /* I'm slow!  I only execute every 26.2ms because */
                                      /* that's how fast the Master uP gives me data. */
      rc_dig_out01 = 1;  // *** look at this on a scope to see when loop starts
      Process_Data_From_Master_uP();  /* You edit this in user_routines.c */

      if (autonomous_mode)            /* DO NOT CHANGE! */
      {
        User_Autonomous_Code();        /* You edit this in user_routines_fast.c */
      }
    }
    Process_Data_From_Local_IO();     /* You edit this in user_routines_fast.c */
                                      /* I'm fast!  I execute during every loop.*/
    rc_dig_out01 = 0;  // *** look on scope to see how long it took!
  } /* while (1) */