koobas.hobune.stream
Help with editing array from inside functions for PID controller (C#)[Fixed]
Hello all and thank you for helping if you can.
I am making a PID control loop simulation in unity and all is going very well, however there is a slight problem when I try to make a single function that can PID tune any value given the correct gains for a simulated quadcopter. Here is the function so far:
float PIDSmooth (float nowpoint, float setpoint, float Pgain, float LastP, float Igain, float Icurrent, float Dgain){
float force;
float P;
float I=Icurrent;
float D;
P = Pgain * (setpoint - nowpoint);
I = I + (P * Igain);
D = Dgain * (P-LastP);
force = P + I + D;
Icurrent = I;
LastP.Equals(P);
return force;
}
float PIDSmooth (float nowpoint, float setpoint, float Pgain, float LastP, float Igain, float Icurrent, float Dgain, float TotalGain){
if (TotalGain == 0) {
float force;
float P;
float I = Icurrent;
float D;
P = Pgain * (setpoint - nowpoint);
I = I + (P * Igain);
D = Dgain * (P - LastP);
force = (P + I + D);
Icurrent = I;
LastP.Equals(P);
return force;
}
else {
float force;
float P;
float I = Icurrent;
float D;
P = Pgain * (setpoint - nowpoint);
I = I + (P * Igain);
D = Dgain * (P - LastP);
force = TotalGain * (P + I + D);
Icurrent = I;
LastP.Equals(P);
return force;
}
}
The problem with this script lies in the LastP.Equals(P), as that does not seem to change the LastP value at all. This also goes for the Icurrent=I part of this script. It is important to note that I cannot use global variables to fill in for LastP and Icurrent because that would remove the verstatility of this function.
Currently, LastP and Icurrent are being stored in float[] arrays so as to make programing the different functions easier and not requireing multiple variables with similar names (like LastPRoll or IcurrentYaw). I could see how this could lead to a problem, but I don't know how it exactly does.
Would there be any way to "hard equals" so that it changes the input value of LastP or Icurrent, or should I make the output of the function also include those values, like by making a placeholding Vector3 type that holds the force, the LastP, and the Icurrent?
LastP.Equals(P), as that does not seem to change the LastP value at all
.Equals() returns a boolean, which is roughly equivalent to LastP == P (equality test) not LastP = P (an assignment, which is what you seem to want).
Would there be any way to "hard equals" so that it changes the input value of LastP or Icurrent,
Use ref ?
As an aside, you have a bunch of duplicated code in there, which is fine when prototyping, or trying something quick IMO, but when you hit a problem, strip it out. When hitting a bug, I try to simplify the code to home in on possible problems.
Also, when doing method overloading, I tend to write a private MyMethodWorker method, called by all public overloaded methods, which a) removes code duplication and b) makes it much easier to see what is actually different between the overloaded methods.
Well analyzed. In addition to that the way how "D" is calculated is wrong as it depends on "P" while it should only depend on the current error. The difference is that the error is not scaled by "Pgain".
Here's a quite simple PID implementation which does everything right. Since it's a class it also holds the last error value internally.
would you change the line
D = Dgain * (P - LastP);
to be more like
D= Dgain * ((setpoint-nowpoint) - lastP/Pgain)
? that way unless Pgain Changes (Which it shouldn't) the D is only dependent on the error(because LastP is just error*Pgain in the last frame)
Well, you over complicate things here. First of all you shouldn't save "lastP" but the last error. The input of all three PID components is the current error:
float E = (setpoint - nowpoint); // This is the current error
P = Pgain * E;
I += Igain * E;
D = Dgain * (E - LastE);
LastE = E;
Icurrent = I;
Of course "LastE" as well as "Icurrent" have to be "ref" parameters.
Thank you! I didn't know ref existed! I understand your caution around the duplicates, but they are all a bit different. Like how the first function has a Tgain and the second doesn't. I am just trying to make this as user-friendly as possible, but if there is a way to simplify it, I would like to hear it!
Sure!
Here's my default thinking for refactoring this kind of code.
1) take the method sig with the most parameters. 2) create a new private fn based on that, add "Worker" on the end 3) copy and paste the guts of the code 4) play "spot the difference" 5) rework the original, overloaded methods
float PIDSmooth (float nowpoint, float setpoint, float Pgain, float LastP, float Igain, float Icurrent, float Dgain, float TotalGain){
// simply pass all the params straight through to the worker method
return PIDSmoothWorker (nowpoint, setpoint, Pgain, LastP, Igain, Icurrent, Dgain, TotalGain)
}
float PIDSmooth (float nowpoint, float setpoint, float Pgain, float LastP, float Igain, float Icurrent, float Dgain){
// ASSU$$anonymous$$PTION: no TotalGain supplied, so pass zero as a safe TotalGain default
return PIDSmoothWorker (nowpoint, setpoint, Pgain, LastP, Igain, Icurrent, Dgain, 0f)
}
float PIDSmoothWorker (float nowpoint, float setpoint, float Pgain, float LastP, float Igain, float Icurrent, float Dgain, float TotalGain){
float force;
float P;
float I = Icurrent;
float D;
P = Pgain * (setpoint - nowpoint);
I = I + (P * Igain);
D = Dgain * (P-LastP);
force = (TotalGain == 0) ? P + I + D : TotalGain * (P + I + D); // the only diff
Icurrent = I;
LastP.Equals(P);
return force;
}
However, the difference between the methods is so small, and the data types are nice and simple, so I think we can do this much more elegantly using an Optional Argument. I think you can replace all your code with the following single method. Note in the method signature I assume that 0 is a safe default for TotalGain.
Do bear in $$anonymous$$d I have no idea what a PID is (I'm guessing you are not talking about Process IDs) so I have no idea if the code actually does what it should :) so please bear the comments by @Bunny83 in $$anonymous$$d here.
float PIDSmooth (float nowpoint, float setpoint, float Pgain, float LastP, float Igain, float Icurrent, float Dgain, float TotalGain = 0){
float force;
float P;
float I = Icurrent;
float D;
P = Pgain * (setpoint - nowpoint);
I = I + (P * Igain);
D = Dgain * (P-LastP);
force = (TotalGain == 0) ? P + I + D : TotalGain * (P + I + D); // the only diff
Icurrent = I;
LastP.Equals(P); // see other comment, I assume you want LastP = P;
return force;
}
I was trying to make an optional parameter, but I thought you just had to make another function with the same name. Ah, Okay. I'll work that in as well.
P.S. If you're curious about what PID controllers are, they are fascinating and you might want to look them up if you have some free time. They are the math behind why thermometers, cruise control, and quadcopters are able to stay in a certain state even with outside forces.
