RobotC and while loops (new to PID)

VEX V5 Technical Support UNOFFICIAL Tech Support
1

I am new to using PID control and I just had some questions.

This is my code (constants are not tuned)…

int Kp = 0.5;
int Ki = 0.2;
int Kd = 0.1;
int integral = 0;
int derivative = 0;
int speed = 0;
int error = 0;
task autonomous();
{
	while (nMotorEncoder[backLeft] < 1000)
	{
		error = 1000 - nMotorEncoder[backLeft];
		integral = integral + error;
		if (error == 0)
		{
			integral = 0;
		}
		if ( abs(error) > 40)
		{
			integral = 0;
		}
		derivative = error - previous_error;
		previous_error = error;
		speed = Kp*error + Ki*integral + Kd*derivative;
	}
	motor[backLeft] = speed;
	motor[backRight] = speed;
	motor[frontLeft] = speed;
	motor[frontRight] = speed;
}

Is my placement for the motor speed correct? Will the motors not be assigned this variable because the while loop is running?

How should I define previous_error as a variable? I understand I should declare it globally, but how do I make it so it takes the value of the previous error, and holds it until a new value comes in?

Is there anything else I should know before using this PID loop?

2

No, placement for motor speed should be inside the while loop. You are correct in saying the motors will not be assigned the


speed

variable since they are outside the loop.

Inside the loop, right before you assign a new value to


error

, set


previous_error

to


error

.
It might look something like this:


	while ( want PID to be enabled )
	{
		error = targ_val-cur_val;
                //random PID stuff
                // like setting motor speed and calculating derivative, etc.
		previous_error = error;
	}

What you have currently is actually fine because you’re not modifying or using either


previous_error

or


error

after you change them until the next loop

Yes. There are several bugs in the code that was posted

bug 1: semicolon in the conditional for the while loop

bug 2: you’re missing minus signs? This might be just my Chrome browser bugging out but it seems like the first line in the loop should have been


error=target_value-sensor_reading;

and later when calculating


derivative

it should have been


derivative=error-previous_error;

bug 3: The motors are written to outside of the loop (already discussed).

bug 4: Why is


integral

being reset to zero in the second if statement inside the loop? Not a very big deal but for optimal tuning I might have set it to a cap (like, 40/Ki) instead of resetting all the way back to zero.

All in all, your final code should look something like this:


Kp = 0.5;
Ki = 0.2;
Kd = 0.1;
task autonomous();
{
int error=0;
int integral=0;
int derivative=0;
int previous_error=0;
int speed=0;
int targ_speed=100;//or whatever other speed you want

	while (nMotorEncoder[backLeft] < 1000;)
	{
                cur_speed = ????; // calculate current speed somehow
		error = targ_speed-cur_speed;
		integral = integral + error;
		if (error == 0) {
			integral = 0;
	        	}
		if ( integral>40/Ki ) {
			integral = 40/Ki; // this caps the maximum power effect integral can have at 40
		        }
		derivative = error -previous_error;
		previous_error = error;
		speed = Kp*error + Ki*integral + Kd*derivative;

        	motor[backLeft] = speed;
        	motor[backRight] = speed;
        	motor[frontLeft] = speed;
	        motor[frontRight] = speed;
	}
}

I am kind of confused as to what you are trying to do with this PID code. Do you want to drive at a certain exact velocity? Or go a certain distance? Go a certain distance at an exact velocity?

If you want to go a certain distance, you can just do


while( nMotorEncoder[backleft]<1000 ) { 
       motor[backLeft] = 127;
       motor[backRight] = 127;
       motor[frontLeft] = 127;
       motor[frontRight] = 127;
       }
motor[backLeft] = -127;
motor[backRight] = -127;
motor[frontLeft] = -127;
motor[frontRight] = -127;
delay(200); // eliminate momentum
3

I edited the code that I included in my statements seemingly after you posted your reply, but before I saw it.

I used a pdf document to help me with this, as it is my first time coding a PID loop.

It said this about integral control;

Also, for the first speed that the motors get assigned, the value of


speed

is 500. What happens to the motors in that situation?

4

Ah. Thanks for the clarification. So basically it takes the integral factor out of the equation as a whole until it is small enough to be beneficial and not contribute to integral wind-up, that is interesting.

Nothing terrible would happen, I believe the cortex would just write the max value of 127 to the motors.

Not sure if you saw my edit saying so but I can’t figure out what you are trying to accomplish with this PID code. It looks like you are just trying to drive to a certain spot as accurately as possible? In that case you want to get rid of the conditional in the while loop and just make it a


while(true)

because if you overshoot, the PID loop will instantly end because


nMotorEncoder[backLeft]

will be > 1000 and the while loop will close and the PID won’t be able to account for the overshoot.

Final code:


int Kp = 0.5;
int Ki = 0.2;
int Kd = 0.1;
int integral = 0;
int derivative = 0;
int speed = 0;
int error = 0;
int previous_error = 0;
task autonomous();
{
	while (true)
	{
		error = 1000 - nMotorEncoder[backLeft];
		integral = integral + error;
		if (error == 0)
		{
			integral = 0;
		}
		if ( abs(error) > 40)
		{
			integral = 0;
		}
		derivative = error - previous_error;
		previous_error = error;
		speed = Kp*error + Ki*integral + Kd*derivative;

	        motor[backLeft] = speed;
	        motor[backRight] = speed;
	        motor[frontLeft] = speed;
	        motor[frontRight] = speed;
	}
}

Best of luck!

5

How would I break out of the while loop in this case?

Is there any benifit to using this in the place of

while( nMotorEncoder[backleft]<1000 ) { 
       motor[backLeft] = 127;
       motor[backRight] = 127;
       motor[frontLeft] = 127;
       motor[frontRight] = 127;
       }
6

You could use a timer, for example after 15 seconds it’s likely the robot will have reached its destination and is now sitting there making negligible adjustments to its position.

You could also have a separate variable that detects changes in the position but does not affect the PID calculation, and when this variable is small it means your robot has tiny errors that wont matter and you can shut off.

Yes, as long as it is running PID will continuously readjust itself to account for any error. For example, if the robot is on an incline and the robot starts to roll down, the error will increase, and the Kp will multiply this error, increasing the power being written to the motors, causing the robot to adjust back to the correct position.

while( nMotorEncoder[backleft]<1000 ) { 
       motor[backLeft] = 127;
       motor[backRight] = 127;
       motor[frontLeft] = 127;
       motor[frontRight] = 127;
       }

will shut off the moment it reaches its destination. If it is on a flat surface this is fine (as long as you elminiate momentum by writing a backwards jolt after the while loop ends) but if it is on an incline you will start rolling down and the robot won’t do anything to stop it.

7

Add a delay in the PID loop otherwise it won’t work properly. For example,

derivative will be zero most of the time, you will be running the loop so fast that error and previous_error will almost always be the same.