Strafing PID

Programming Support
1

Hi everyone, I’ve been trying to implement a strafing PID for the past few days and I’ve really hit a stump. Whenever I run this program, the robot 1) moves extremely slowly, 2) overshoots its target, 3) doesn’t work when I input different values as the parameter. Like when I call it in autonomous, I’ve just been running it with an input of 12 (inches), and it never works when I call anything else (like 6 inches). When changing the input for the first time, I got a segfault error (PROS system daemon) and my robot hasn’t been consistent ever since. At least before, the exit point of the PID loop was consistent and so were the paths. Any help would be appreciated. Thanks

Extra info:
I’ve had to multiply the input voltage by 2 because without a scaling factor like that the voltage input would be way too small and the bot would barely move. Also, the exit part of the loop where it checks for the moveError value (just below the integral) seems to have no effect on the stopping point of the bot, as I’ve tried different values ranging from 2 to 500 with all generating the same exact path.

double move_kP = 2.0;
double move_kI = 0.0;
double move_kD = 0.0;

double move_Error = 0;
double move_lastError = 0;
double move_intg = 0;
double move_deriv = 0;
double move_totalError =0;

bool atTarget = false;
//use inches for coords
void strafeBot(double distn){

	middleencoder.reset();
	double enc_val = middleencoder.get();
	double dist_to_tick = distn*41.669;

  while (!atTarget){
    /////////// STRAFE PID

    //Proportional
    move_Error = dist_to_tick-enc_val;

    //Derivative
    move_deriv = move_Error-move_lastError;

    //Integral
    if(abs(move_Error) < 300){
    move_totalError += move_Error;
  } else {
    move_totalError = 0;
  }

	if(abs(move_Error) <= 2){
			atTarget = true;
}

    double updated_voltage = move_kP*move_Error + move_kI*move_totalError + move_kD*move_deriv;

		// float input_voltage = (updated_dist/dist)*50;

		driveF.moveVoltage(-updated_voltage*2);
		driveB.moveVoltage(updated_voltage*2);

		move_lastError = move_Error;
		pros::delay(10);

}
driveF.moveVoltage(0);
driveB.moveVoltage(0);
}
1 Like
2

It looks to me like the problem is that you only assign the value of the middleencoder to enc_val variable only once outside PID loop.

Change it to something like this:

double velocity = 0;
while(1)
{
   double enc_val = middleencoder.get();
   veloocity = ... // calculate derivative here
   if( fabs(enc_val - target) < 3 &&  // obviously, replace posRange=3 and 
       fabs(velocity) < 5 ) break;    // velRange=5 with values that work for you
   // calculate integral, check for integral windup
   // calculate motor power, check for max limit
   // set motor power
   // sleep(20);
}
// set motor power to 0

Your stopping condition should be to get close to target within posRange and don’t have too much velocity so it doesn’t overshoot. Also, you may need to have time limit, so if it gets stuck, it could still move to the next step after a timeout.

2 Likes
3

Just wondering, is it common to scale up voltages because the one calculated by the PID itself may be too small?

4

Normally kP, kI, and kD are tuned enough that it shouldn’t need that, but if it works it works

2 Likes
5

I just tried it with the code below, and the same thing occurs , with the velocity/position bounds having 0 effect on the amount of distance the bot travels. The bot keeps traveling that distance consistently, though.

double move_kP = 2.0;
    double move_kI = 0.0;
    double move_kD = 0.0;

    double move_Error = 0;
    double move_lastError = 0;
    double move_intg = 0;
    double move_deriv = 0;
    double move_totalError =0;
    double velocity = 0;

    bool atTarget = false;
    //use inches for coords
    void strafeBot(double distn){

      while (!atTarget){
    		middleencoder.reset();
    		/////////// STRAFE PID
    		double enc_val = middleencoder.get();
    		double dist_to_tick = (distn/(2.75*pi))*360;
        //Proportional
        move_Error = dist_to_tick-enc_val;

        //Derivative
        velocity = move_Error-move_lastError;

    		if (fabs(enc_val - dist_to_tick) < 20 && fabs(velocity)< 10){
    			break;
    		}
        //Integral
        if(fabs(move_Error) < 300){
        move_totalError += move_Error;
      } else {
        move_totalError = 0;
    	}

        double updated_voltage = move_kP*move_Error + move_kI*move_totalError + move_kD*move_deriv;
    		// float input_voltage = (updated_dist/dist)*50;

    		driveF.moveVoltage(-updated_voltage*2);
    		driveB.moveVoltage(updated_voltage*2);

    		move_lastError = move_Error;
    		pros::delay(20);

    }
    driveF.moveVoltage(0);
    driveB.moveVoltage(0);
    }
6

The tuning constants themselves should be able to account for any scaling of outputs needed. For instance, our team uses millivolts as our PID output it is not uncommon for our tuning constants to be >100. Don’t be afraid to mess around with the control loop.

2 Likes
7

Thanks! Will begin to play around with them more

8

Tuning constants can really be anything from extremely low (.001) to 100+
This isn’t directly related to what you need help fixing, but in the future you should look into slew rate. That basically acts like the opposite of a pid loop and makes you faster the farther away from your start is. The point of it is that your motor is just going to jolt forwards with it’s current configuration, so you’ll want to accelerate to make it more consistent

2 Likes
9

This is your mistake:

You don’t want to reset encoder value back to zero every time you are about to read its value (of zero).

What you want is to reset it once and then read new value at every loop cycle:

middleencoder.reset();
while (!atTarget)
{
    /////////// STRAFE PID
    double enc_val = middleencoder.get();
    ...
2 Likes
10

Tried that, but the same thing still happens. Although now the bot now moves at a normal speed lol. I think from here it will just come down to tuning the velocity/position ranges. Additionally, I also get this error in PROS, which I think could be the key to what is going wrong. It doesn’t affect the building process though so I’m just ignoring it for now.

image
image1287×174 23.2 KB

11

pic of rquantity.hpp (cant put 2 images in one post)

image
image772×930 48.1 KB

nothing seems to be out of the ordinary

12

thats just the language server (cquery) throwing a fit about unused stuff from okapi. Most likely not the issue.

perhaps you should make it so your breakout condition is dependent on a small derivative as well as a small error?

2 Likes
13

Yes, thats what weilin suggested earlier (velocity is basically where the derivative is calculated). Right now its gradually speeding up but doesnt slow down, and still heavily overshoots its target.