PID Programming Problems

VEX V5 Technical Support UNOFFICIAL Tech Support
#1

Hello, I am writing a PID loop on RobotC, but have encountered a problem. My PID loop will start running and will go forward, but it wont stop.

I tried adding

if (error ==0)
{
stopAllMotors;
}

but not even that would make the motors stop running.

#2

It’s unlikely that your error will ever be exactly 0. Try programming in a margin of error:

if(abs(error) < someNumber){
  stopAllMotors();
}

where


someNumber

is a reasonably small number representing an acceptable margin of error.

#3

@Barin I tried

if(abs(error) < Inches(.2))
{
stopAllMotors();
}

(I defined “Inches” earlier in the program to be the the readings of the tick value converted to be inches, so “Inches(.2)” would be an error with a magnitude of .2 inches, hence the name)

After adding that, the motors would still run continuously.

#4

Could you post all your PID code? It’s hard to tell exactly what the problem might be.

#5

@Barin

I initiated the PID with button 5D for testing purposes

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1, rightIEM, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, leftIEM, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_3, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_4, , sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, RightMotor1, tmotorVex393HighSpeed_HBridge, openLoop, encoderPort, I2C_1)
#pragma config(Motor, port2, RightMotor2, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)
#pragma config(Motor, port3, LeftMotor1, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_3)
#pragma config(Motor, port4, LeftMotor2, tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_4)

void forward (int speed)
{
motor[RightMotor1] = speed;
motor[RightMotor2] = speed;
motor[LeftMotor1] = speed;
motor[LeftMotor2] = speed;
}

//627.2 ticks/revolution
//1 turn of the wheels - 4 inch wheels = 627.24 ticks
//1 turn of wheel 4pi inches
//4 wheels with IMEs
//4pi inches = 4627.22ticks
//ticks/inch = 627.24/(4*pi) = 199.64396

int Inches (float inch)
{
int ticks;
ticks = inch*199.64396;
return ticks;
}

int MillisecondsToSeconds (float RawSeconds)
{
int milliseconds;
milliseconds = RawSeconds*1000;
return milliseconds;

}

void PIDMovement (float target, float waitTime)
{
float Kp = 0.2;
float Ki = 0.05;
float Kd = 0.5;

int error;

float proportion;
int integralRaw;
float integral;
int lastError;
int derivative;

float integralActiveZone = Inches(3);
float integralPowerLimit = 50/Ki;

int finalPower;

bool timerBool = true;

nMotorEncoder[RightMotor1] = 0;
nMotorEncoder[RightMotor2] = 0;
nMotorEncoder[LeftMotor1] = 0;
nMotorEncoder[LeftMotor2] = 0;

clearTimer(T1);

while (time1[T1] < MillisecondsToSeconds(waitTime))
{
	error = Inches(target)-(nMotorEncoder[RightMotor1]+nMotorEncoder[RightMotor2] + nMotorEncoder[LeftMotor1]+ nMotorEncoder[LeftMotor1]);

	proportion = Kp*error;

	if (abs(error) < integralActiveZone && error != 0) //	float integralActiveZone = Inches(3); defined above 
	{
		integralRaw = integralRaw+error;
	}
	else
	{
		integralRaw = 0;
	}

	if (integralRaw > integralPowerLimit) //float integralPowerLimit = 50/Ki; defined above
	{
		integralRaw = integralPowerLimit;
	}
	if (integralRaw < -integralPowerLimit)
	{
		integralRaw = -integralPowerLimit;
	}

	integral = Ki*integralRaw;

	derivative = Kd*(error-lastError);
	lastError = error;

	if (error == 0)
	{
		derivative = 0;
	}

	finalPower = proportion+integral+derivative; //proportion+derivative+integral

	forward(finalPower);

	wait1Msec(40);

	if (error < 30)
	{
		timerBool = false;		
	}

	if (timerBool)
	{
		clearTimer(T1);
	}
}

forward(0);

}

task main ()
{

while(true)
{

	motor[RightMotor1] = (vexRT[Ch2] - vexRT[Ch1])/2; //(y-x)/2
	motor[RightMotor2] = (vexRT[Ch2] - vexRT[Ch1])/2; //(y-x)/2
	motor[LeftMotor1] = (vexRT[Ch2] - vexRT[Ch1])/2; //(y-x)/2
	motor[LeftMotor2] = (vexRT[Ch2] + vexRT[Ch1])/2; //(y+x)/2

	if(vexRT[Btn5D] == 1)
	{
		PIDMovement(2,1);
	}

}

}

#6

Just something simple you might want to try is reversing the value of the sensor you are using. I know that many times I have just had the value reversed, which make the motors be ever gaining error not loosing it.

#7

@DanielIW26 That was the problem. Thank you!

#8