PID controller integral term causing extreme instability - c#

I have a PID controller running on a robot that is designed to make the robot steer onto a compass heading. The PID correction is recalculated/applied at a rate of 20Hz.
Although the PID controller works well in PD mode (IE, with the integral term zero'd out) even the slightest amount of integral will force the output unstable in such a way that the steering actuator is pushed to either the left or right extreme.
Code:
private static void DoPID(object o)
{
// Bring the LED up to signify frame start
BoardLED.Write(true);
// Get IMU heading
float currentHeading = (float)RazorIMU.Yaw;
// We just got the IMU heading, so we need to calculate the time from the last correction to the heading read
// *immediately*. The units don't so much matter, but we are converting Ticks to milliseconds
int deltaTime = (int)((LastCorrectionTime - DateTime.Now.Ticks) / 10000);
// Calculate error
// (let's just assume CurrentHeading really is the current GPS heading, OK?)
float error = (TargetHeading - currentHeading);
LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");
// We calculated the error, but we need to make sure the error is set so that we will be correcting in the
// direction of least work. For example, if we are flying a heading of 2 degrees and the error is a few degrees
// to the left of that ( IE, somewhere around 360) there will be a large error and the rover will try to turn all
// the way around to correct, when it could just turn to the right a few degrees.
// In short, we are adjusting for the fact that a compass heading wraps around in a circle instead of continuing
// infinity on a line
if (error < -180)
error = error + 360;
else if (error > 180)
error = error - 360;
// Add the error calculated in this frame to the running total
SteadyError = SteadyError + (error * deltaTime);
// We need to allow for a certain amount of tolerance.
// If the abs(error) is less than the set amount, we will
// set error to 0, effectively telling the equation that the
// rover is perfectly on course.
if (MyAbs(error) < AllowError)
error = 0;
LCD.Lines[2].Text = "Error: " + error.ToString("F2");
// Calculate proportional term
float proportional = Kp * error;
// Calculate integral term
float integral = Ki * (SteadyError * deltaTime);
// Calculate derivative term
float derivative = Kd * ((error - PrevError) / deltaTime);
// Add them all together to get the correction delta
// Set the steering servo to the correction
Steering.Degree = 90 + proportional + integral + derivative;
// We have applied the correction, so we need to *immediately* record the
// absolute time for generation of deltaTime in the next frame
LastCorrectionTime = DateTime.Now.Ticks;
// At this point, the current PID frame is finished
// ------------------------------------------------------------
// Now, we need to setup for the next PID frame and close out
// The "current" error is now the previous error
// (Remember, we are done with the current frame, so in
// relative terms, the previous frame IS the "current" frame)
PrevError = error;
// Done
BoardLED.Write(false);
}
Does anyone have any idea why this is happening or how to fix it?

It looks like you are applying your time base to the integral three times.
Error is already the accumulated error since the last sample so yo don't need to multiply deltaTime times it. So I would change the code to the following.
SteadyError += error ;
SteadyError is the integral or sum of error.
So the integral should just be SteadyError * Ki
float integral = Ki * SteadyError;
Edit:
I have gone through your code again and there are several other items that I would fix in addition to the above fix.
1) You don't want delta time in milliseconds. In a normal sampled system the delta term would be one but you are putting in a value like 50 for the 20Hz rate this has the effect of increasing Ki by this factor and decreasing Kd by a factor of 50 as well. If you are worried about jitter then you need to convert delta time to a relative sample time. I would use the formula instead.
float deltaTime = (LastCorrectionTime - DateTime.Now.Ticks) / 500000.0
the 500000.0 is the number of expected ticks per sample which for 20Hz is 50ms.
2) Keep the integral term within a range.
if ( SteadyError > MaxSteadyError ) SteadyError = MaxSteadyError;
if ( SteadyError < MinSteadyError ) SteadyError = MinSteadyError;
3) Change the following code so that when error is around -180 you do not get a step in error with a small change.
if (error < -270) error += 360;
if (error > 270) error -= 360;
4) Verify Steering.Degree is receiving the correct resolution and sign.
5) Lastly yo can probably just drop deltaTime all together and calculate the differential term the following way.
float derivative = Kd * (error - PrevError);
With all of that your code becomes.
private static void DoPID(object o)
{
// Bring the LED up to signify frame start
BoardLED.Write(true);
// Get IMU heading
float currentHeading = (float)RazorIMU.Yaw;
// Calculate error
// (let's just assume CurrentHeading really is the current GPS heading, OK?)
float error = (TargetHeading - currentHeading);
LCD.Lines[0].Text = "Heading: "+ currentHeading.ToString("F2");
// We calculated the error, but we need to make sure the error is set
// so that we will be correcting in the
// direction of least work. For example, if we are flying a heading
// of 2 degrees and the error is a few degrees
// to the left of that ( IE, somewhere around 360) there will be a
// large error and the rover will try to turn all
// the way around to correct, when it could just turn to the right
// a few degrees.
// In short, we are adjusting for the fact that a compass heading wraps
// around in a circle instead of continuing infinity on a line
if (error < -270) error += 360;
if (error > 270) error -= 360;
// Add the error calculated in this frame to the running total
SteadyError += error;
if ( SteadyError > MaxSteadyError ) SteadyError = MaxSteadyError;
if ( SteadyError < MinSteadyError ) SteadyError = MinSteadyError;
LCD.Lines[2].Text = "Error: " + error.ToString("F2");
// Calculate proportional term
float proportional = Kp * error;
// Calculate integral term
float integral = Ki * SteadyError ;
// Calculate derivative term
float derivative = Kd * (error - PrevError) ;
// Add them all together to get the correction delta
// Set the steering servo to the correction
Steering.Degree = 90 + proportional + integral + derivative;
// At this point, the current PID frame is finished
// ------------------------------------------------------------
// Now, we need to setup for the next PID frame and close out
// The "current" error is now the previous error
// (Remember, we are done with the current frame, so in
// relative terms, the previous frame IS the "current" frame)
PrevError = error;
// Done
BoardLED.Write(false);
}

Are you initializing SteadyError (bizarre name...why not "integrator")? If it contains some random value on start-up it might never return to near zero (1e100 + 1 == 1e100).
You might be suffering from integrator windup, which ordinarily should go away, but not if it takes longer to diminish than it does for your vehicle to complete a full rotation (and windup the integrator again). The trivial solution is to impose limits on the integrator, though there are more advanced solutions (PDF, 879 kB) if your system requires.
Does Ki have the correct sign?
I would strongly discourage the use of floats for PID parameters because of their arbitrary precision. Use integers (maybe fixed point). You will have to impose limit checking, but it will be much more sane than using floats.

The integral term is already accumulated over time, multiplying by deltaTime will make it accumulate at a rate of time-squared. In fact since SteadyError is already erroneously calculated by multiplying error by deltaTime, that is time-cubed!
In SteadyError, if you are trying to compensate for an aperiodic update, it would be better to fix the aperiodicity. However, the calculation is flawed in any case. You have calculated in units of error/time whereas you want just error units. The arithmentiaclly correct way to compensate for timing jitter if really necessary would be:
SteadyError += (error * 50.0f/deltaTime);
if deltaTime remains in milliseconds and the nominal update rate is 20Hz. However deltaTime would be better calculated as a float or not converted to milliseconds at all if it is timing jitter you are trying to detect; you are needlessly discarding precision. Either way what you need is to modify the error value by the ratio of nominal time to actual time.
A good read is PID without a PhD

I'm not sure why your code isn't working, but I'm almost positive you can't test it to see why, either. You might inject a timer service so you can mock it out and see what's happening:
public interace ITimer
{
long GetCurrentTicks()
}
public class Timer : ITimer
{
public long GetCurrentTicks()
{
return DateTime.Now.Ticks;
}
}
public class TestTimer : ITimer
{
private bool firstCall = true;
private long last;
private int counter = 1000000000;
public long GetCurrentTicks()
{
if (firstCall)
last = counter * 10000;
else
last += 3500; //ticks; not sure what a good value is here
//set up for next call;
firstCall = !firstCall;
counter++;
return last;
}
}
Then, replace both calls to DateTime.Now.Ticks with GetCurrentTicks(), and you can step through the code and see what the values look like.

Related

Synchronising rhythm game notes by changing the maxValue in Mathf.Lerp

enter image description here
I can't describe the problem thoroughly because I don't know rhythm game terms.
In the picture
The white cube Note represents the object which the player needs to hit on time.
The green rectangle Line is the representation of the timing. When the Note perfectly lines up with the Line is when the player presses a button to hit the note.
Now the problem is, I cannot seem to find a way to make the Note perfectly line up with the Line using Mathf.Lerp while it still reaches the end.
void GenerateBeat()
{
if (timeItems.Count == 0)
return;
if (timeItems.Peek() <= currentBeatPosition + BeatLookForwardValue)
{
Debug.Log("Item instantiated, Spawned At : " + currentBeatPosition + " Will reach 0 at : " + timeItems.Peek());
GameObject obj = Instantiate(tempSpawnObject);
obj.transform.SetParent(parentObject.transform);
existingBeats.Add(obj);
existingBeatsTime.Add(currentBeatPosition);
timeItems.Dequeue();
}
}
void MoveBeat()
{
// if (timeItems.Count == 0)
// return;
for (int i = 0; i < existingBeats.Count; i++)
{
NoteBeatline beatline = existingBeats[i].GetComponent<NoteBeatline>();
Vector2 StartingPos = new Vector2(0, 800f);
Vector2 EndPos = new Vector2(0, 0);
float offset = (endTime[i] - existingBeatsTime[i]) / 3;
float time = Mathf.InverseLerp(existingBeatsTime[i], endTime[i] + offset, currentBeatPosition);
beatline.Image.rectTransform.anchoredPosition = Vector2.Lerp(StartingPos, EndPos, time);
}
}
[Edit]
Sorry for the lack of clarification.
I'm trying to synchronise Notes by adding extra value to the endTime because existingBeatsTime (keeps track of the time at which the object is spawned) can vary depending on how great BeatLookFOrwardValue is.currentBeatPosition represents how long in beat time has passed since the start of the song, so I cannot manipulate the value.
the endTime value is always a constant, set by me
and items in timeItems are identical to the items in endTime.
the whole background colour of dark cyan is currently 800 units
and the y position of the green horizontal line is 600
so that the Notes can perfectly line up with the green line
when
float time = Mathf.InverseLerp(existingBeatsTime[i], endTime[i] + offset, currentBeatPosition);
is 0.75f.
I did more research and experiment after I posted this,
and found out
float offset = (endTime[i] - existingBeatsTime[i]) / 3;
somehow synchronises the Notes, but I don't understand how.
Please feel free to comment on my explanation.
This is my first time posting a question on StackOverflow and explaining my coding problem to someone else.
I will try to provide more information if it is still lack.
"Currently, Notes reach the end when currentBeatPosition == endTime.
But I want to make them reach the Line when currentBeatPosition == endTime"
So, they are lerping across the correct distance? They are just doing it a bit too fast yeah? So to fix it you can increase the total time over which they are lerping. So they reach the line at endTime and reach the end a little bit later.
get the ratio, distance from start to end / distance from start to line
Multiply the total time they are lerping over by this ratio.
I've finally fixed the synchronisaton problem.
I found out that there's Mathf.LerpUnclamped() which can extrapolate the return value. So I had to change
beatline.Image.rectTransform.anchoredPosition = Vector2.Lerp(StartingPos, EndPos, time);
to
beatline.Image.rectTransform.anchoredPosition = Vector2.LerpUnclamped(StartingPos, EndPos, time);
this. Since it can extrapolate I was able to set the endPos to the position of the green line.
Additionally, I also had to make my own Mathf.InverseLerp() which will be the unclamped version of Mathf.InverseLerp().
public static float InverseLerpUnclamped(float startValue, float maxValue, float inbetweenValue)
{
return (inbetweenValue - startValue) / (maxValue - startValue);
}
Finally, I could simply get it working by these two lines of code.
float time = Tools.InverseLerpUnclamped(existingBeatsTime[i], endTime[i], currentBeatPosition);
beatline.Image.rectTransform.anchoredPosition = Vector2.LerpUnclamped(StartingPos, EndPos, time);

PID (a proportional–integral–derivative controller) loop with a different command and feed back units/ types

I have a laser and I am adjusting it's position using a motor, and I am reading the laser light intensity at the top and bottom using a sensor.
then I divide the top from the bottom and the goal is to get around 0.97%. This is called delta, if delta is too high, you move the motor lower and if delta is too lower we move it up.
now I coded a PID ( A proportional–integral–derivative controller), the program I have my command is in motor steps(position) but my feed back is in light intensity percent.
so how do I set up my PID ? can I even use a PID this way ?
here is my code:
GetValue()
{
PID_controller PID = new PID_controller(2, 1, 0);
****
stepsize = (int)PID.UpdatePID(Convert.ToDouble(stepsize), delta);
stepsize = changeStepSize( stepsize);
***
}
public PID_controller(int Proportional, int Integral, int differential)
{
this.Proportional = Proportional;
this.Integral = Integral;
this.differential = differential;
Istate = 0D;
error_counter = 0;
lastposition = 0;
}
double getPTerm(double command , double position)
{
double error = command - position;
Istate += error;
error_counter++;
return error;
}
double getITerm()
{
double Integralvalue = Istate / error_counter;
return Integralvalue;
}
double getDTerm(double position)
{
lastposition = position;
double differential = lastposition - position;
return differential;
}
public double UpdatePID( double command, double position)
{
double error = getPTerm(command, position);
double perror = getITerm();
double derror = getDTerm(position);
command = (Proportional * error) + (Integral * perror) + (differential * derror);
return command;
}
the program I have my command is in motor steps(position)
This is your control variable: the value you can control.
my feed back is in light intensity percent.
This is your process variable: the value you observe.
I have a laser and I am adjusting it's position using a motor, and I am reading the laser light intensity at the top and bottom using a sensor. then I divide the top from the bottom and the goal is to get around 0.97%. This is called delta, if delta is too high, you move the motor lower and if delta is too lower we move it up.
If you divide the top by the bottom, you will never get negative error. Even if you implement the difference between positive/negative error manually, your control response will be skewed. Ignoring divide by zero issues, if you have positive error (laser too high), your error value will be anywhere between 0 and positive infinity. But if you have negative error (laser too low), your error will be between 0 and 1. You'll be feeding your controller much larger error values when the beam is too high than when it's too low.
Unless this is your desired behavior, you should use the difference between the top and bottom sensor output as your process variable error, in which case your set point would be ~0.03.
If you're looking for a C# PID controller to use, check out CSPID (disclaimer: I'm the developer).

Using a large float value as a timer in Unity3d

I am using a float to calculate and display the time the player has left (in seconds) to finish the game.
In my game time sometimes goes faster, so the time left value needs to go big.
Here's my problem: I use the following code in Update:
Update () {
timeleft -= Time.deltaTime;
Debug.Log (timeleft);
counter.text = "Time Left = " +timeleft.ToString("F0");
}
At game start, the time left value is set to a really high number (32 million, about a year in seconds).
The rate at which the time drops varies in the game, so I am using a float with time.deltaTime.
Unity stores this big initial value as 3.2E+07.
As soon as that happens, my counter.text doesn't work properly anymore, as that just waits until the next scientific notation value comes up. So it looks like the timer text is stuck, while in the back it is in fact still counting down. Hope I'm making sense here. For the avoidance of doubt: the counter display works fine for values below 1 million.
How do I fix this problem? How do I convert timeleft.ToString so that it displays the correct value?
As per the comments below, someone suggested to use a decimal instead. That won't work with deltaTime as that needs a float or I misunderstood where and how to use the decimal.
I tried to create a new int and use Mathf.RoundToInt on the float, but that doesnt work: the timer stays stuck at the larger value.
The short moral here is that floating-point arithmetic doesn't conform to ordinary human intuitions about numbers.
You write:
Unity stores this big initial value as 3.2E+07. As soon as that
happens, my counter.text doesn't work properly anymore, as that just
waits until the next scientific notation value comes up. So it looks
like the timer text is stuck, while in the back it is in fact still
counting down.
This is incorrect: it's not just the text that's not changing, the number itself isn't changing for small values of Time.deltaTime. The problem is that the precision of float reduces as the value gets larger. Moreover, since the underlying storage format is binary floating-point, the 'gap' between the next-largest and next-smallest number is difficult to intuit.
You can test this yourself with the following code:
float f = 3.2e7f;
string s1 = f.ToString();
Console.WriteLine(s1);
string s2 = f.ToString("F0");
Console.WriteLine(s2);
string s3 = (f - 7).ToString("F0");
var f2 = f;
for (int i = 0; i < 3000000; i++)
{
f2 = f2 - 1f;
}
var diff = f - f2;
Debug.Log(diff);
if (f2 == f)
{
Debug.Log("Floating point numbers are strange.");
}
The value of f2 never changes, despite being supposedly decremented 3 million times. This is because 3.2e7f - 1f is exactly equal to 3.2e7f.
The solution, as pointed out in the comments, is to use decimal, which uses a base-10 format and conforms much better to human intuitions. This will count down correctly.
decimal timeLeft = 3.2e7M;
Update () {
timeleft -= (decimal)Time.deltaTime;
Debug.Log (timeleft);
counter.text = "Time Left = " +timeleft.ToString("F0");
}

How to "round" a 2D Vector to nearest 15 degrees

I'm working on a simple game and I'm trying to simplify part of the 2D collision reaction in the game. When certain objects hit walls, I'm calculating a collision normal (collisionPoint - objectCenter) and reflecting based on that normal. I'm interested in rounding that normal vector to its nearest 15° but I'm not sure of a good way to go about that.
My current thought is doing something like this
float angle = atan2(normal.Y, normal.X) * Rad2Deg;
float newAngle = ((int)(angle + 7.5f) / 15) * 15.0f * Deg2Rad;
vector2 newNormal = vector2(cos(newAngle), sin(newAngle));
Is this a reasonable way to do it? Is there a better way?
Try this:
float roundAngle = 15 * Deg2Rad;
float angle = (float)Math.Atan2(normal.Y, normal.X);
Vector2 newNormal;
if (angle % roundAngle != 0)
{
float newAngle = (float)Math.Round(angle / roundAngle) * roundAngle;
newNormal = new Vector2((float)Math.Cos(newAngle), (float)Math.Sin(newAngle));
}
else
{
newNormal = Vector2.Normalize(normal);
}
You don't need to add 7.5, take this example:
// 4 degrees should round to 0
(4 + 7.5) / 15 == 11.5 / 15 == 0.77
// When this gets rounded up to 1 and multiplied by 15 again, it becomes 15 degrees.
// Don't add 7.5, and you get this:
4 / 15 == 0.27
// When rounded, it becomes 0 and, as such the correct answer
// Now how about a negative number; -12
-12 / 15 == -0.8
// Again, when rounded we get the correct number
actually this is more correct if you want the nearest 15 degree angle :
do this:
newangle% = INT(((angle%+7.5)/15)*15)
INT ALWAYS rounds DOWN by default this should properly give you the nearest angle in any case that is positive or negative have fun!!
and add the part where you use degree to rad and rad to degree if needed INSIDE the parens (like right next to angle% if that angle is not given in degrees then use some sort of rad2deg multiplier inside there
this is more like how you would do this in basic, with some modification It will work in c code or such, well good luck!!

How to manage "speed" in a simple racing game?

I'm trying to develop a simple racing 2d game (view top-down) in C#, sdl.net.
Now, I'm trying to manage speed, acceleration and brakes of my car.
My problem is the algorithm.
I have the loop (Events_Tick) executed 50 times per seconds, where the position of my car is processed like the following:
private void Events_Tick(object sender, TickEventArgs e)
{
car.ProcessPosition();
}
and ProcessPosition is something like:
if (throttle)
{
speed += 1;
x += 1;
}
and finally, I draw my sprite on the new X.
The problem is that it is too fast!
So I'm asking you how to maintain 50 FPS (frames per second) and move my sprite (the car) only N pixels per second (based on its speed).
Thank you in advance! Regards!
First of all, because you have 2d not 1d game, you'll need your speed to be
class Vector
{
double x;
double y;
}
with this class, you should maintain position and speed.
Since you are 2d top-down, you'll have to implement some .RotateLeft() and .RotateRight() on the speed vector.
Rotation will be implemented like:
x' = cos(a) * x - sin(a) * y
y' = sin(a) * x + cos(a) * y
And you'll have to implement your .Move() method as follows:
void Move(Vector v)
{
x+=v.x;
y+=v.y;
}
EDIT: please ask away if clarification is needed, or some more in-depth discussion.
Also, you can use timer here, but try to calculate time spent from last timer event, and then multiply speed with that value when adding to the current position, you will get more accurate position that way.
a in sin() and cos() will be an angle in radians, and you will probably want degrees here.
Here is something to get you going regarding degrees and radians.
Use a double value for speed and x, then you can use Math.Round(x) for drawing.
if (throttle == true) {
speed+=0.05;
}
x+=speed; //move car even without acceleration
Follow Henk's advice to make your program more flexible and to be able to show the user something like 50mph instead of 0.05px per 0.02s:
double speedInMph = 50;
double screenSizeInMiles = .1;
double screenSizeInPx = 500;
if(throttle)
{
speedInMph += 1;
}
double speedInMpms = 50/(60*60*1000);//Miles per millisecond
double xInMiles = speedInMpms * 50;
double pxPerMile = screenSizeInPx/screenSizeInMiles;
x+= xInMiles * pxPerMile;
Acceleration will be constant, depending on user input (on a simple level the car is either accelerating, deccelerating or maintaining speed).
Speed will change each frame depending on that acceleration. Each frame, update the car's speed based on both the acceleration and the proportion of a second the frame covers (i.e. 1/50th of a second in your case).
So, if your car is accelerating at 10 units distance per second, in your example the speed will increase by 1/5th of a unit distance per frame.
Just thought it was worth elaborating a little on the other answers, instead of just posting more code :-)
You should add velocity according to current acceleration and handle just acceleration with your controls, limiting speed with a cap and increasing it according to time elapsed between two game loop iterations.
float accel = 10.0f; // units per squared second
float maxSpeed = 50.0f // units per second
float curSpeed = 0.0f;
//game init
while (1) {
long timeBefore = time();
// game loop
long timeElapsed = time() - timeBefore;
if (throttle) {
curSpeed += accel*(timeElapsed / 1000.0);
if (curSpeed > maxSpeed)
curSpeed = maxSpeed;
}
}
In this way you take into account the fact that a longer frame will increase your car speed more than a quicker ore, keeping it consistent with time.
This model implies a constant acceleration while you could hypotetically want a dynamic one, in that case you just move the update to the acceleration (taking into account the frame duration as for speed) and cap it to not go over a fixed threshold.
Henk is right. You will want to create a
Thread
and call its
Sleep(Int32)
method each time in your loop to slow the animation down, where the int parameter is the pause time in milliseconds.
In your case, you will want to move the car N/50 pixels each time looping, and sleep the thread for 20 milliseconds each time.

Categories