RobotC won't exit PID loop

VEX V5 Technical Support UNOFFICIAL Tech Support
1

So, I have modeled a PID loop after Martin’s PID series on his YouTube channel found here. Here is a function that I use to drive forward:

void PIDForwards(float target, float waitTime, float maxPower = 1)
{
	float kp = 0.05;//0.035
	float ki = 0.00555;//0.00055
	float kd = 0.03;//0.003

	int finalPower;//Sum of proportional, integral, and derivitive values//
	int error;
	int integralRaw;
	float integralActiveZone = inchToTicks(3);
	float integralLimit = 50/ki;
	int lastError;

	float proportion;
	float integral;
	int derivative;

	float kpc = 0.01;
	int error_drift;
	float proportion_drift;

	SensorValue[rightDrive] = 0;//The two drive shaft encoders
	SensorValue[leftDrive] = 0;

	while(error < target)
	{
		//Calculate the error(target - current)//
		error = inchToTicks(target)-(SensorValue[rightDrive]+SensorValue[leftDrive]);

		proportion = kp*error;

		if(abs(error) < integralActiveZone && error != 0)
		{
			integralRaw = integralRaw+error;
		}
		else
		{
			integralRaw = 0;
		}
		if(integralRaw > integralLimit)
		{
			integralRaw = integralLimit;
		}
		if(integralRaw < -integralLimit)
		{
			integralRaw = -integralLimit;
		}

		integral = ki*integralRaw;

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

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

		finalPower = proportion+integral+derivative;

		if(finalPower > maxPower*127)
		{
			finalPower = maxPower*127;
		}
		else if(finalPower < -maxPower*127)
		{
			finalPower = -maxPower*127;
		}

		error_drift = SensorValue[rightDrive]-SensorValue[leftDrive];
		proportion_drift = kpc*error_drift;

		moveLeftBaseForwards(finalPower+proportion_drift);
		moveRightBaseForwards(finalPower-proportion_drift);

		wait1Msec(40);
	}
	moveBase(0);
}

When I run this function it will run fine. However, when I run this function with another PID function the program will run the first function but then it stops without doing anything else. Did I do something wrong? Whenever I run simple movement commands without the PID stuff it all works fine. Then when I add the PID back in though the program won’t exit the while loop I have set up and the shaft encoders don’t reset and I’m not sure why :stuck_out_tongue: Could my integral be too low(the motors are sent a small amount of power after the robot stops moving so I’m guessing the integral is a little low).
Any help/suggestions as to what I could do or fix is appreciated, as well as any improvements :slight_smile:

2

Here is your problem

while(error < target)
{

Remember error gets smaller and smaller so it will always be less than target. You are running PID fine but your exit condition doesn’t work. I would suggest

While(abs(error) <20) to ensure you actually are able to exit.

3

Having the PID loop in a task can help with these issues. Then you have a global variable to turn it on and off and make the error a global variable to be read as well. Going to a small error may mean you just saw it fly by and your PID loop turns off prematurely. This way, you stay on the PID until you

while(PID_on == true)
{

In your auton or driver function play with the PID_on variable and the precision


target= 300;
PID_on = true;

//use what you want here
target_precision = 20;
maxTime = 2500;

//now wait for it....  You could make this into a function.....
clearTimer(T2);

// loop around for you to be within range or if you don't kick out with a timer and move on.
while (abs(PID_error) > target_precision || time1[T2] > maxTime)
{
   wait1Msec(20);
}
4

Ok, a quick update. Before You replied back I actually changed it to a sensor value :stuck_out_tongue:
So, I have this code:

void PIDForwards(float target, float waitTime, float maxPower = 1)
{
	float kp = 0.035;//0.035
	float ki = 0.00655;//0.00055
	float kd = 0.05;//0.003

	int finalPower;//Sum of proportional, integral, and derivitive values//
	int error;
	int integralRaw;
	float integralActiveZone = inchToTicks(3);
	float integralLimit = 50/ki;
	int lastError;

	float proportion;
	float integral;
	int derivative;

	float kpc = 0.01;
	int error_drift;
	float proportion_drift;

	SensorValue[rightDrive] = 0;
	SensorValue[leftDrive] = 0;

	while(SensorValue[leftDrive]+SensorValue[rightDrive] < target)
	{
		//Calculate the error(target - current)//
		error = inchToTicks(target)-(SensorValue[rightDrive]+SensorValue[leftDrive]);

		proportion = kp*error;

		if(abs(error) < integralActiveZone && error != 0)
		{
			integralRaw = integralRaw+error;
		}
		else
		{
			integralRaw = 0;
		}
		if(integralRaw > integralLimit)
		{
			integralRaw = integralLimit;
		}
		if(integralRaw < -integralLimit)
		{
			integralRaw = -integralLimit;
		}

		integral = ki*integralRaw;

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

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

		finalPower = proportion+integral+derivative;

		if(finalPower > maxPower*127)
		{
			finalPower = maxPower*127;
		}
		else if(finalPower < -maxPower*127)
		{
			finalPower = -maxPower*127;
		}

		error_drift = SensorValue[rightDrive]-SensorValue[leftDrive];
		proportion_drift = kpc*error_drift;

		moveLeftBaseForwards(finalPower+proportion_drift);
		moveRightBaseForwards(finalPower-proportion_drift);

		wait1Msec(40);
	}
	moveLeftBaseForwards(0);
	moveRightBaseForwards(0);
}

From what I can see everything is fine but when I try to run this it just goes forward a small distance(no matter how high I set the target) and then It won’t run anything else; then if I take out the forward function and put some others in it won’t run at all :frowning:
The moveLeftBaseForwards and the moveRightBaseForwards commands are written like this:

void moveLeftBaseForwards(int speed)
{
motor[WheelFrontLeft] = speed;
motor[WheelRearLeft] = speed;
}

And this:

void moveRightBaseForwards(int speed)
{
motor[WheelFrontRight] = speed;
motor[WheelRearRight] = speed;
}

Any help as to why this is happening would be helpful. Ever since I started using this PID stuff the program seems to be acting really spotty. Earlier today I ran it and it worked fine then without even closing the compiler I ran it again and it didn’t work at all :stuck_out_tongue:
Thank you both for responding too :slight_smile:

5

Nevermind, I figured it out. My unit conversion function was off :stuck_out_tongue:

6

You also had the addition of the left & right sensors. I think you want the average of them.


SensorValue[leftDrive]+SensorValue[rightDrive]

Should probably be in two places…


(SensorValue[leftDrive]+SensorValue[rightDrive])/2