PID Loops using the Camera

[mfang]

Hi all,

We have some code that is untested , and so I thought I would ask if any of you could provide some answers/advice.

How feasible would it be to use the camera (involving image processing as similar to that in the sample camera code) in the PID Input value? From what I've heard, it takes some time for it to be calculated, which would cause a delay in the process.

[Chris Hibner]

Originally Posted by mfang

Hi all,

We have some code that is untested , and so I thought I would ask if any of you could provide some answers/advice.

How feasible would it be to use the camera (involving image processing as similar to that in the sample camera code) in the PID Input value? From what I've heard, it takes some time for it to be calculated, which would cause a delay in the process.

It can be done - we do it.

Two things to think about:

1) you might want to change the X position coordinate that you get out of the default camera code to an aim angle. You can do this by multiplying the X coordinate by half of the camera field of view. For example, if the X coordinate from the camera code is 0.5 and your camera field of view is 43 degrees, than your aim is: 0.5 * (43/2) = 10.75 degrees (i.e., you must turn 10.75 degrees to be aimed at the target). Note that your aim is 10.75 from your camera position. If your camera is fixed to a moving turret, you'll need to add 10.75 degrees to your current known turret angle.

2) Your PID will most likely be very unstable due to the time it takes to retrieve the image and process it. What you need to do is delay your known turret angle that I referred to in step 1 above by the same time as your camera lag.

Here's an example:

PID_setpoint = CurrentTurretAngle_delayed + CameraAim;
PID_processVariable = CurrentTurretAngle;

In the above example, CurrentTurretAngle_delayed is the turret angle that has been delayed by the same amount of time as your camera lag. So, if you determine that your camera lag is 0.5 seconds, then CurrentTurretAngle_delayed should also lag by 0.5 seconds.

CurrentTurretAngle (process variable) should be the real-time turret angle.

[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.)

[mfang]

Thanks you so much for your help! In the source code there is a constructor for PIDSubsystem in Java in which you can input the period between calculations- would that work?

[Chris Hibner]

Originally Posted by mfang

Thanks you so much for your help! In the source code there is a constructor for PIDSubsystem in Java in which you can input the period between calculations- would that work?

I really don't know - I've never seen the FIRST Java framework. Hopefully someone that uses Java can help in this area.