[mikets]

I am sorry. I am probably not helping with your particular problem. I am merely trying to point out that in the iterative robot class, don't count on the period being accurate and use it as dt for differentiation or integration. You said your Update_Rate is 100 Hz. That makes the period 10 ms. If you put "time = time + 1/Update_Rate" in your loop, your time is really summing a constant 10 ms on every loop period, not real time. If you do this instead:

class MyRobot: public IterativeRobot
{
private:
    UINT32 m_timestamp;
    ....

public:
    void AutonomousInit()
    {
        m_timestamp = GetFPGATime();
        ....
    }
    void AutonomousPeriodic()
    {
        UINT32 timeCurr = GetFPGATime(); // in usec
        float period = (float)(timeCurr - m_timestamp)/1000000.0;
        m_timestamp = timeCurr;
        printf("loop period is %f\n", period);
        ...
    }
};

"period" will be the accurate loop interval that can be used as dt in your differentiation or integration. In theory period should print out 0.01s (10 ms), but in our case, it is way off. It could be because our loop execution took longer than the 10ms to execute that may cause a period to be skipped in some cases. Therefore, as a good practice, never use the period you set in any time critical calculations.