PID infinite movement

Programming Support VEXcode Pro V5 Text Tech Support
#1

We are using a PID loop that was converted from a ROBOTC project. When used in a test base with 4 motors and 2 encoders, it worked just fine. When applied to a base with the same specs (4 motor, 2 encoder (1 reversed encoder)), it is broken. What happens is the robot moves forward for infinity mo matter how small or big the error is. No matter what we do to the constants, the result is the same as well. Mathematically, the loop checks out so I’m not sure what the problem is. Here is my code included:

void pidDrive(int target)
{

float pid_Kp = 0.1;
float pid_Ki = 0.0;
float pid_Kd = 0.0;

int ErrorL;
int ErrorR;
float LastErrorL;
float LastErrorR;
float CurrentSensorValueL;
float CurrentSensorValueR;
float DriveL;
float DriveR;
float IntegralL;
float IntegralR;
float DerivativeL;
float DerivativeR;

E1.resetRotation();
E2.resetRotation();

LastErrorL = 0;
LastErrorR = 0;
IntegralL = 0;
IntegralR = 0;

while(true)
{

	 CurrentSensorValueL = -E2.value();    //encoder reads negative
	 CurrentSensorValueR = E1.value();

	//Proportional 
	ErrorL = target - CurrentSensorValueL;
	ErrorR = target - CurrentSensorValueR;

	//Integral 
	if(pid_Ki != 0)
	{
		if(((abs(ErrorL) + abs(ErrorR)) / 2) < 50)
		{
			IntegralL = IntegralL + ErrorL;
			IntegralR = IntegralR + ErrorR;

  if(IntegralR > 10){
   IntegralR = 10;
  }
   else if (IntegralR < -10){
    IntegralR = -10;
  }

   if(IntegralL > 10){
    IntegralL = 10;
  }
   else if (IntegralL < -10){
    IntegralL = -10;
  }
  else 
		{
			IntegralL = 0;
			IntegralR = 0;
		}
	}
	}
	else
	{
	IntegralL = 0;
	IntegralR = 0;
}


	//Derivative 
	DerivativeL = ErrorL - LastErrorL;
LastErrorL  = ErrorL;
DerivativeR = ErrorR - LastErrorR;
LastErrorR  = ErrorR;

//Calculating the motor speed
	DriveL = (pid_Kp * ErrorL) + (pid_Ki * IntegralL) + (pid_Kd * LastErrorL)/12;
	DriveR = (pid_Kp * ErrorR) + (pid_Ki * IntegralR) + (pid_Kd * LastErrorR)/12;

//Limits power to 12 volts
if(DriveL > 12){
  DriveL = 12;
}
else if(DriveL < -12){
  DriveL = -12;
}

if(DriveR > 12){
  DriveR = 12;
}
else if(DriveR < -12){
  DriveR = -12;
}

BL.spin(forward,DriveL,voltageUnits::volt);
FL.spin(forward,DriveL,voltageUnits::volt);
	BR.spin(forward,DriveR,voltageUnits::volt);
	FR.spin(forward,DriveR,voltageUnits::volt);

wait(10,msec);

}

}

#2

Let’s rule out some external factors before checking the math. Keep the robot elevated from its wheels and turn the encoder wheels by hand. You should print the target, encoder values, error, and power to the screen. Verify that the power approaches 0, as you turn the encoder towards the target. (This should happen since you have only set the kP to a nonzero number) Adjust your setup accordingly if it does not have this behavior.

Now, here’s some general feedback about your current code. You can use std::clamp from the <algorithm> header to limit IntegralL and IntegralR between a certain set of boundaries. This will probably not fix the main issue, but it can help clean up the code. You can read about it here, or research it further online. Additionally it fixes the bug in your code that is locking your integral values to either -10, 0, or 10 and nothing in-between. You could also use std::clamp to limit the drive powers too.

Next, you need a way to exit the while loop. Even if you corrected any math mistakes and the robot actually settled at the target, the code will still infinitely process this loop. You could replace the condition of the while loop with a boolean variable. When the robot is within an acceptable range of the target, then you can toggle the variable. Just for good measure you should also set the power of all the motors to 0 after the loop is complete.

That might look something like this:

/* other variable definitions */
bool atTarget = false;
while(!atTarget) {
/* calculate feedback and send power to motors */
   if(/* close enough to target*/) {
      atTarget = true
   }
}
/* stop all drive motors */

There are a lot of moving parts to keep track of as you create your movement function, so printing the values and watching them live is the best advice I can give you.

7 Likes
#3

Thanks, I’ll give it a try.

A problem im running into however is that when i go to use std::clamp, it states “No member named ‘clamp’ in namespace ‘std’” even when i’ve incuded the necessary libraries. Any suggestions?

EDIT: I found out that Vexcode Pro V5 runs on an older version of c++ so std::clamp isnt availible through it. Are there any other ways to keep the value down without using clamp?

#4

Well you could write your own clamp function pretty easily. Something like…

float clamp(float val, float low, float high){
    return val > high ? high : (val < low ? low : val);
}

or equivalently using if/else instead of ternary ops:

float clamp(float val, float low, float high){
    if (val > high){
        return high;
    }
    else {
        if (val < low){
            return low;
        }
        else {
            return val;
        }
    }
}
4 Likes
#5

New issue coming up as i get to test this finally, nothing is popping up for sensor/other variable values. I’m not sure if this is mechanical or not but ill include my printing code just to make sure im not making a mistake putting values on the brain. Also nothing you suggested helped solve the problem:

Brain.Screen.clearLine(2,color::black);
Brain.Screen.clearLine(3,color::black);
Brain.Screen.clearLine(4,color::black);
Brain.Screen.clearLine(5,color::black);
Brain.Screen.clearLine(6,color::black);
Brain.Screen.setCursor(1,0);
Brain.Screen.print(“ErrorL:”,ErrorL);
Brain.Screen.setCursor(2,0);
Brain.Screen.print(“ErrorR:”,ErrorR);
Brain.Screen.setCursor(3,0);
Brain.Screen.print(“E1 Value:”,E1.value());
Brain.Screen.setCursor(4,0);
Brain.Screen.print(“E2 value:”,E2.value());
Brain.Screen.setCursor(5,0);
Brain.Screen.print(“Left Power:”,DriveL);
Brain.Screen.setCursor(6,0);
Brain.Screen.print(“Right power:”,DriveR);

#6

Sure, lets first take a look at the documentation for the print method. The two parameters that it takes are a formatting character (or string of characters) and a list of values that you want to print. I emphasized formatting because you are required to specify how the value gets “inserted” into the text –– in other words: how should the text be formatted? We can do this with format specifiers

You can read more about format specifiers in another thread here. But this is the section to pay attention to:

If you are not familiar with format specifiers in C/C++ you can take a look at this resource too. It explains what each one is, and how to use them. This is important if you want to print values, as opposed to static text. These are really important because if you print a value with the wrong format, you might see a really strange value on your screen. Generally, %d for int and bool , %f for float and double and %s for std::string . Although if you are curious, you can experiment with other combinations.

Once you get a good idea of how format specifiers work, you’ll be able to fix your code very easily. I encourage you to give it a try before I give away the solution. Getting the values to update on the screen should be your priority since it will help you diagnose the issue with your PID calculations.

2 Likes
#7

Fixed it, now im reading that the encoders are not reading for some reason and the error is also at 0 for some reason. In spite of this, the motors move at 10 volts.

#8

Are you updating the labels on the screen in a while loop? If you don’t have it in a loop, that might be why you only see zeros.

1 Like
#9

now it updates and i managed to get it to stop moving for infinity somehow, but when it goes forward it and turns to the left once it stops for some reason. Or, it will spas out in very direction. either one of those two.