[Chris Hibner]

If you're wondering how to delay your turret angle, you need a circular buffer that covers the time of delay.

Here's an example:

Let's say you want to delay measurements by 0.5 sec. Your fast periodic loop (the loop you're running the PID in) runs at 10 msec. In this case, your circular buffer must contain 50 elements (0.5 sec / 0.01 s = 50).

Then in your code:

in the initialization code, create an array with 50 elements. Set all elements to 0 (or whatever the starting angle of your turret is, if it isn't 0).

example:

double turretAngleDelayed[50];
for (int i = 0; i < 50; i++)
{
 turretAngleDelayed[i] = 0;
}

Then in your fast loop, you do the following:

static int currentTurretCount = 0;
double currentTurretAngle_delayed;

currentTurretAngle_delayed = turretAngleDelayed[currentTurretCount];  // this retrieves the value from 50 samples ago (i.e. 0.5 sec ago)
// now overwrite the sample from 50 samples ago with the current sample
turretAngleDelayed[currentTurretCount] = currentTurretAngle; // current turretAngle is the angle measure this loop

// update the counter for the next loop
currentTurretCount++;
if (currentTurretCount >= 50)
{
   currentTurretCount = 0;
}

(note: above is how you would do it in C. Given that you'll be using classes in Java, you shouldn't need a static variable declaration in your class. The currentTurretCount variable should just be a class member.)