# Need help with PID for going straight in autonomous

Hello everyone!

I recently started learning how to implement PID in RobotC. I already the previous threads that were posted on how to do PID. From those threads I came up with the following code on how to make the robot go straight for a given encoder value(target):

lFwd refers to the motor values given to the left side of the robot.
rFwd: motor values given to the right side of the robot.
The robot’s drive system has four motors with a quad encoder on each side.

``````//PID Variables
float kpL = 0.15;
float kpR = 0.25;
float kiL = 0.00005;
float kiR = 0.00005;
float kdL = 0.1;
float kdR = 0.1;
float errorL;
float previousErrorL;
float integralL;
float derivativeL;
float errorR;
float previousErrorR;
float integralR;
float derivativeR;``````
``````void Straight(int target){
ResetSensors();
integralL = 0;
previousErrorL = target;
integralR = 0;
previousErrorR = target;

while(!reached){
errorL = target - SensorValue[ltEncoder];
errorR = target - SensorValue[rtEncoder];
integralL = integralL + errorL;
integralR = integralR + errorR;

if(abs(errorL) < 5 || errorL > 500){
integralL = 0;
}
if(abs(errorR) < 5 || errorR > 500){
integralR = 0;
}

derivativeL = errorL-previousErrorL;
derivativeR = errorR-previousErrorR;
previousErrorL = errorL;
previousErrorR = errorR;

lFwd = kpL*errorL + kiL*integralL + kdL*derivativeL;
rFwd = kpR*errorR + kiR*integralR + kdR*derivativeR;

if((abs(SensorValue[ltEncoder]) > abs(SensorValue[rtEncoder]+ 5)) && errorL > 50)
{
lFwd = lFwd - (lFwd/abs(lFwd))*10;
}
if((abs(SensorValue[ltEncoder]) < abs(SensorValue[rtEncoder] - 5)) && errorR > 50)
{
rFwd = rFwd - (rFwd/abs(rFwd))*10;
}
SetDriveMotors();
}
}``````

The first problem with this code is that the robot goes fairly straight for most of the travel distance but when it reaches the end, it makes a sharp turn to the right.

Second problem: after overshooting, it takes too long to adjust. To fix this, do I just have to keep on tinkering with the values?

Is this the right way to go straight with PID? If there is a better way, please let me know, while keeping in mind that I’m a newbie when it comes to PID. If not, please help me fix this code.
I will greatly appreciate your help.

Thank you

May be an issue with deceleration or perhaps the task that’s calling the Straight function. It’s always hard to tell when only part of the code is posted. It may also be mechanical, chain tensions may be different etc. Under driver control with no PID how straight does the robot drive, it’s always best to get this as good as possible first so the software has an easier job.

PID is easy to code and hard to tune. With the VEX system there are also significant delays in sending commands to the motors and the relatively slow update rate of the control loops.

Which team are you with in LA ?

The straight function is being called in the autonomous task and it is the only function that is being called(for the purposes of testing). In user control, the robot’s right side does lag behind slightly. The tension in the chain is the same on both sides. But yes, we should make it go as straight as possible mechanically as well. Here’s the rest of the code that relates to PID:

``````
{
Straight(1000);
}

void SetDriveMotors()
{
motor[topRightDrive] = rFwd;
motor[botRightDrive] = rFwd;
motor[topLeftDrive] = lFwd;
motor[botLeftDrive] = lFwd;
motor[centerLeft]   = strafe;
motor[centerRight]  = strafe;
}

void ResetSensors()
{
SensorValue[ltEncoder] = 0;
SensorValue[rtEncoder] = 0;
}

``````

So am I on the right track with PID?

I am in college VEX with the California State University of Northridge VEX team, The Matabots.

You are on the right track, I did notice you have different proportional constants for left and right.

``````float kpL = 0.15;
float kpR = 0.25;
float kiL = 0.00005;
float kiR = 0.00005;
float kdL = 0.1;
float kdR = 0.1;``````

This will give more drive to the right side for a given error.

You may also wish to slow down the loop, there’s little point in updating faster than about 20mS unless you are using ports 1 & 10, in fact, how are the motors distributed on the cortex, ports 1 & 10 behave a little differently. See this thread

https://vexforum.com/t/not-all-motor-ports-are-created-equal/20755/1

probably not your problem but good background info.

Try with the two Kp constants the same.

Personally, I do the driving straight code slightly differently. Instead of having one set of code per side, I have one main one followed by a second one, which adjusts the speeds according to the differences. For example:

``````
{ //start of loop
dist_error = set_point - ( (SensorValue(leftEncoder) + SensorValue(rightEncoder)) /2);
diff_error = SensorValue(leftEncoder) - SensorValue(rightEncoder);

...

motor[left] = dist_output - diff_output;
motor[right] = dist_output + diff_output;
} //end of loop``````

In this code, dist refers to distance, so how far you want to travel. Diff refers to difference, the difference between the two sides, therefore how much needs to be turned.

If you want, I could post the full version.

~George

Thanks for the info!

hmm…that could also work.

That would be very nice as I can learn from someone who knows PID :]

OK I can’t post it right now (I’m on a different computer) but I’ll do it ASAP tomorrow

~George

OK, here’s the code that I used last year (I would send you the code from this year, but I haven’t yet had time to test it).

``````void drivestraight(int distance, int maxspeed, int time)
{
resetencoders();
ClearTimer(T1);

prevdisterror = 0;
prevdifferror = 0;

while (time1[T1] < time)
{
disterror = distance - ((SensorValue(leftencoder) + SensorValue(rightencoder))/2); //Calculate distance error
differror = SensorValue(leftencoder) - SensorValue(rightencoder); //Calculate difference error

// Find the integral ONLY if within controllable range AND if the distance error is not equal to zero
if( abs(disterror) < 60 && disterror != 0)
{
distintegral = distintegral + disterror;
}
else
{
distintegral = 0; //Otherwise, reset the integral
}

// Find the integral ONLY if within controllable range AND if the difference error is not equal to zero
if( abs(differror) < 60 && differror != 0)
{
diffintegral = diffintegral + differror;
}
else
{
diffintegral = 0; //Otherwise, reset the integral
}

distderivative = disterror - prevdisterror; //Calculate distance derivative
diffderivative = differror - prevdifferror; //Calculate difference derivative

prevdisterror = disterror; //Update previous distance error
prevdifferror = differror; //Update previous difference error

distspeed = (disterror * distP) + (distintegral * distI) + (distderivative * distD); //Calculate distance speed
diffspeed = (differror * diffP) + (diffintegral * diffI) + (diffderivative* diffD); //Calculate difference (turn) speed

if(distspeed > maxspeed) //Check that the speed is not exceeding the maximum set speed
{
distspeed = maxspeed;
}

if(distspeed < -maxspeed) //Check that the speed is not exceeding the maximum set speed
{
distspeed = -maxspeed;
}

motor[left] = distspeed - diffspeed; //Set motor values
motor[right] = distspeed + diffspeed; //Set motor values

wait1Msec(20); //Give the robot a (quick) break
}
}``````

So in this code, I run the loop for the PID based on time. If I know it will only take 2 seconds or so to complete, I will send it a time value of say, 2.5 seconds. It will continuously refresh the loop and correct any overshoot until that 2.5 seconds has finished, and since I know it would only take 2 seconds, I am pretty sure it should have finished by that time. Then, when testing the code, I would shrink that 2.5 as much as I could, to save time (any spare time it has is wasted time in a competition).

Please let me know if there is anything that doesn’t quite make sense!!

~George

Thanks so much for your help guys!

How accurate is it just to use encoders? I’ve already written gyro turning PID code, and am wondering how useful it would be to use the gyro as well as encoders for going straight.

Either works. For most High School autonomous, you are not performing a long sequence of actions, so accumulated error is quite small. This makes encoders more viable, as you only need to go for 15 seconds, rather than say 1 minute in programming skills. Most teams I see use PID and encoders to great effect. I’ve seen at least one team use the gyro to good effect, so if you can make it work, go for it.

On a side note, some teams complain about the added drivetrain friction from encoders, and that their robots squeak when driving around, so a gyro might work to get around these issues. If your gyro code is working fine, I’d leave it, but encoders can also be used.

~Matt

P.S.

Always remember where your towel is.

So I took a shot at writing a program like this in EasyC V4. It uses a proportional loop for calculating the speed of the motors depending on how far the distance is and it uses a PID loop to make the robot go completely straight. THe only thing is, I can’t test it on a robot and I really want to know if it will work. I have attached the pictures of the entire code. If anyone can tell me how it will do and if I need to make any changes that would be great because I understand PID but I still don’t get how to apply the Output of the equation. Thank you very much!

Edit: I forgot to put that the parameters for this function include: ( Desired_Distance, Max_Drive_Speed, and Max_Time)

Hey, just wondering since you didn’t show the constants for the diffP or distP or any of the I and D values, were they similar or vastly different? Would you mind showing those just so I could see what the range looks like? Thanks!

Yeah I didn’t add those as I didn’t want teams to start using my values which are not tuned for their robots, but very much specifically for the robot I was using. They would likely have some bad results.

The range of values really depends on the sensor (IME or OSE) and the speed at which it is turning, amongst other things.

Note that not all of the encoders on the robots I have programmed have been spinning at the same speed, and the robots are all very different so each robot is really unique when it comes to tuning and so my values will not necessarily work for your purposes.

The values I have had before:
distP: 0.2 to 0.8 (normally)
distI: 0.005 to 0.05 (quite often)
distD: 0.1 to 2.0 (my values for D can get rather wacky, sometimes out of that range)
diffP: 0.2 to 1.8 (normally)
diffI: sometimes up to 0.15! (I think something was wrong)
diffD: 0.1 to 2.5 (same as distD in that my values are very strange for D)

So basically, do some reading on tuning the constants, I generally begin my P value at 1.0, I value at 0.005, and D value at 1.0. It’s always different for each robot.

~George