PID issues

Could someone help me get my PID working for my lift? It currently causes it to “dance” by raising then lowering sporadically. We are using two potentiometers to measure the height of the two sides of the lift. The minimum height is 1030 and the maximum is 2450 on the pots. I feel it is the Gain variables being too high, since I am dealing with error of about 1400 at max.



int armspeed = 110;
int derivativeR = 0;
int derivativeL = 0;
int integralR = 0;
int integralL = 0;
int integralLimit = 1200;

void armPID()
{
	const float pGain = .0002;
	const float iGain = .0001;
	const float dGain = .0001;
	int errorL = 0;
	int errorR = 0;
	int prevErrorL = 0;
	int prevErrorR = 0;
	float leftK = 0;
	float rightK = 0;

		/* Defining the Error in the system for both sides of the lift.*/
		errorL = setHeight - SensorValue[armpotL];
		errorR = setHeight - SensorValue[armpotR];

    		if(errorL <50 && errorL > -50)
		{
			errorL = 0;
		}
		if(errorR <50&& errorR > -50)
		{
			errorR = 0;
		}

		/*Derivative Control*/
		derivativeR = errorR - prevErrorR;
		derivativeL = errorL - prevErrorL;
		prevErrorR = errorR;
		prevErrorL = errorL;

		/*Integral Control*/
		integralR += errorR;
		integralR += errorL;

		if(integralR > integralLimit)
		{
			integralR = integralLimit;
		}
		else if(integralR < -integralLimit)
		{
			integralR = -integralLimit;
		}

		if(integralL > integralLimit)
		{
			integralL = integralLimit;
		}
		else if(integralL < -integralLimit)
		{
			integralL = -integralLimit;
		}

		leftK = pGain*(errorL) + iGain *(integralL) + dGain * (derivativeL);
		rightK = pGain*(errorR) + iGain *(integralR) + dGain * (derivativeR);
		if( leftK > 1.0)
		{
			leftK = 1.0;
		}
		else if( leftK < -1.0)
		{
			leftK = -1.0;
		}

		if( rightK > 1.0)
		{
			rightK = 1.0;
		}
		else if( rightK < -1.0)
		{
			rightK = -1.0;
		}
		motor[armleft1] = armspeed* leftK;
		motor[armright1] = armspeed* leftK;
		motor[armleft2] = armspeed* rightK;
		motor[armright2] = armspeed* rightK;


}

task usecontrol()
{
  while(true)
  {
    armPID();
                       if(setHeight > 2430)
			{
				setHeight = 2430;
			}
			else if(setHeight < 1050)
			{
				setHeight = 1050;
			}

                       if(vexRT[Ch3Xmtr2] >5)
			{
				setHeight += (vexRT[Ch3Xmtr2]/4);
			}
			else if(vexRT[Ch3Xmtr2] < -5)
			{
				setHeight -= (vexRT[Ch3Xmtr2]/4);
			}

  }
}

Any help is appreciated.

I’m no expert on anything involving PID or code, but it seems to me that the following bit of code should go after the values read from the joystick controller, otherwise I think you could theoretically run a PID function with the values momentarily outside of your desired limits. But please take what I say with a grain of salt.


....
                       if(setHeight > 2430)
			{
				setHeight = 2430;
			}
			else if(setHeight < 1050)
			{
				setHeight = 1050;
			}

         ....

 .

Also, I think you want to fix this:


task use**r**control()

A few comments.

/*Derivative Control*/
derivativeR = errorR - prevErrorR;
derivativeL = errorL - prevErrorL;
prevErrorR = errorR;
prevErrorL = errorL;

The above won’t work because.

int prevErrorL = 0;
int prevErrorR = 0;

these are local variables and will always be 0.

Tighten up the tolerance on this, +/- 50 may be a bit much.

    		if(errorL <50 && errorL > -50)
		{
			errorL = 0;
		}
		if(errorR <50&& errorR > -50)
		{
			errorR = 0;
		}

You need a delay somewhere, deltaT will be almost 0 so integral will go large and derivative will be almost 0. Add wait1Msec(25) somewhere in the main while loop.

Then it’s all about the tuning. PID tuning can take some time.

This is a different method of PID control in that you are making a percentage adjustment to the motor versus setting motor values.

Log the error against the component values to see what contribution each is doing and graph the line of “dancing” so youc an tune it down.

The integral is a max of 12% contribution which it will get there pretty quick without a wait in the loop. So overshoot and the i will wait a bit to calm down. This seems like a good max, but adjust the max integral value of 1200 as you set the i values to set the contribution to something nice. I think you could go up to 3000 with that i setting of 0.0001 to allow up to 33 motor power coming from the i contribution. (But let your tuning decide the real i value)

The P can only be 2450 away so that will contribute up to 50% to the motor max distance away. Your p value should go up. it should contribute max motor value nearly until you get to the destination.

The D is the change from last time and with a really quick turn around without a wait, you will probably not get more than 20 so that contributes less than 1% to your readings. So it is basically not there with this value.

Make those other changes and see the contribution from each area to the motor value as you approach the target (and then blow right by it). Use your tuning mechanism of choice but most start with P only.

The OP is still setting motor values, it’s just that the drive is calculated in the range +/- 1 first and then multiplied by the maximum control value (110) to move into the range +/-110. It’s the way I always do it as well, it makes the code more portable. All it really means is the the PID constants are smaller than they would have otherwise been.

Results are the same, it’s just a different way to skin the cat. Either way, you have a really cold and bare cat on the other end. :smiley:

Not sure if anyone else noticed this, but it also looks like you have a copy/paste typo in the integral section.

It looks like you’re accumulating the left and right errors to only the right side integral.

/*Integral Control*/
		integralR += errorR;
		integralR += errorL;

I did not notice the copy paste error. I got the PID function running today after adding a slight delay. I spent about an hour and a half tuning it using a manual method found on the internet. It has a slight delay on the start up of the motors, but reaches and sustains height fairly well. I also tightened the dead band so it can correct severe leaning caused by movement or our intake. Thanks Guys!

so i had a minute to take a look at the code again, and i came across what could be another cut/paste typo in the motor speed assignment section of the code.

i’m not sure if your motor naming is different from what i’m expecting or if you’re crossing left and right proportional factors on the arm drive.
if i’m reading this correctly, this could cause significant issues with the motors fighting against each other.

		motor[armleft1] = armspeed* leftK;
		motorarmright1] = armspeed* leftK;
		motorarmleft2] = armspeed* rightK;
		motor[armright2] = armspeed* rightK;

Thanks