PID doesn't work

VEX V5 Technical Support UNOFFICIAL Tech Support
#1

I’m having an issue with my PID loop. I’m using this to keep the launcher constant.

The issue is that the launcher “stutters”. It will quickly go forward or backward and I can hear gears skipping when this happens.

Here’s the code:


#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    gyro,           sensorGyro)
#pragma config(Sensor, dgtl1,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  limitSwitch,    sensorTouch)
#pragma config(Sensor, I2C_1,  leftIME,        sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  rightIME,       sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           intake,        tmotorVex393TurboSpeed_HBridge, openLoop)
#pragma config(Motor,  port2,           left1,         tmotorVex393_MC29, openLoop, reversed, driveLeft)
#pragma config(Motor,  port3,           right1,        tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port4,           topRightLaunch, tmotorVex393_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port5,           bottomRightLaunch, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port6,           topLeftLaunch, tmotorVex393_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor,  port7,           bottomLeftLaunch, tmotorVex393_MC29, openLoop, reversed, encoderPort, I2C_2)
#pragma config(Motor,  port8,           right2,        tmotorVex393_MC29, openLoop, reversed, driveRight)
#pragma config(Motor,  port9,           left2,         tmotorVex393_MC29, openLoop, driveLeft)
#pragma config(Motor,  port10,          intake2,       tmotorVex393TurboSpeed_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//encoder: 627.2 ticks per revolution
int time = 25;
int Hz = 1000 / time;
int ticks = 627.2;
float Kp = 2.0;	
float Ki = 0.04;
float Kd = 0;
int pidRunning = 1;
float PIDLastError;
float PIDDerivative;
float PIDTarget = 100;
float PIDIntegral = 0;
float PIDIntegralLimit = 50;
float PIDError;
float RPM;

void RPMCalc()
{
	SensorValue[leftIME] = 0;
	wait1Msec(time);
	RPM = SensorValue[leftIME] * Hz * 60 / ticks;
}

void Error()
{
	PIDError = PIDTarget - RPM;
}


void PID()
{
	if(pidRunning == 1)
	{
		if(Ki != 0)
		{
			if(abs(PIDError) < PIDIntegralLimit) PIDIntegral = PIDIntegral + PIDError;
			else PIDIntegral = 0;
		}
		PIDLastError = PIDError;
		PIDDerivative = PIDError - PIDLastError;
		float PIDDrive = (Kp * PIDError) + (Ki * PIDIntegral) + (Kd * PIDDerivative);
		if(PIDDrive > 127) PIDDrive = 127;
		else if(PIDDrive < 127) PIDDrive = -127;
		motor[topLeftLaunch] = (PIDDrive);
		motor[bottomLeftLaunch] = (PIDDrive);
	}
	else
	{
		PIDError      = 0;
		PIDLastError  = 0;
		PIDIntegral   = 0;
		PIDDerivative = 0;
	}
}

task main()
{
	while(1)
	{
		RPMCalc();
		Error();
                PID();
	}
}

What am I doing wrong?

#2

Have you properly tuned your variables? The situation that you’re describing sounds like some form of oscillation. In particular, you should probably take a look at your kp and kd. Your kp is pretty high for the average PID loop, and your derivative function isn’t even implemented since kd = 0.

#3

In addition to that, your entire code looks pretty iffy.
This part is pretty suspect:


if(PIDDrive > 127) PIDDrive = 127;
else if(PIDDrive < 127) PIDDrive = -127;
motor[topLeftLaunch] = (PIDDrive);
motor[bottomLeftLaunch] = (PIDDrive);

Besides the missing braces around everything, this code doesn’t seem to make much sense in terms of PID control. PID automatically adjusts motor speed through the final output and shouldn’t be limited like this. Also, this limit code doesn’t make much sense either since your else if loop would set any value under 127 to -127.(I.e. 126 gets set to -127.)

P.S. I’m just going to make a post for each issue in the code I see.

#4

Yes this is the issue. I was about to post about this.

if(PIDDrive > 127) PIDDrive = 127;
else if(PIDDrive < 127) PIDDrive = -127;
should be
if(PIDDrive > 127) PIDDrive = 127;
else if(PIDDrive < -127) PIDDrive = -127;

But really with this type of system that limit isn’t really necessary. Do you plan on sending your flywheel commands to spin backwards?

#5

Next up are the random “fail safes” you have scattered in your code:


if(pidRunning == 1)
	{
		if(Ki != 0)
		{
                }
        }

The two conditional statements that you have don’t actually seem to have much use in this context. Even if your ki = 0, you may as well let your integral calculation run since during the final output equation it’ll be zeroed out anyways(ki * integral). The pidRunning portion of your code also seems pretty strange. You set pidRunning to 1 at the very beginning of your code and don’t do anything else with it. Do you have other code that toggles the value of pidRunning so that you can turn PID on and off?

P.S. Thanks Tabor

#6

facepalm

This is why you don’t write code when you’re tired.

I know the limit isn’t necessary but I pretty much just woke up when working on it :stuck_out_tongue:

As for no brackets on the limits, it’s a terrible habit I have. When I only have one statement I don’t put brackets as the compiler doesn’t care at that pouint.

The pidRunning is a placeholder. I was testing the PID by itself and later on I was going to turn this file into an include, put everything into a task, and call it in my main code.

Thanks for correcting my stupidity. :stuck_out_tongue:

#7

I think a lot of that code came from this old thread.
https://vexforum.com/t/a-pid-controller-in-robotc/20105/1

Aside from the typos, clipping values such as final motor drive is a reasonable thing to do when you don’t know what happens if you call a function with an undefined value, for example, what happens if you set motor port1 ] = 12345678; ? There may be some internal overflow or error that makes the motor run the wrong way (there isn’t, this is hypothetical).

#8

Yeah, I got a bit of the PID from that thread.

#9

Touche. My point was meant to be that the limits should make sense for each application specific problem. There is a good chance a control mishap could result in full negative power on a flywheel and break all the gears thus I would suggest making the lower bound of the flywheel power 0. Obviously this goes out of the window if you have a ratchet system.

#10