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.