PID Tuning + Turning

Programming Support VEXcode IQ Technical Support
1

How do I tune a pid program? This sounds like beginners luck but the loop works quite well with only the proportional component.

Also I added turnerror in the drive forward as well so that it drives straight but it doesn’t correct when i purposely nudge it off track.

Is there anything incorrect that you can see?

here is the code:

const double kp = 0.4;
const double ki = 0.0;
const double kd = 0.2;

//turn costants
const double tp = 0.5;
const double ti = 0.0005

//double proportion;
double totalError;
double rtotalError;
double integral;
double rintegral;
double integralActiveZone = 90;//adjust later
double integralPowerLimit = 50;//adjust later
double derivative;
double rderivative;
double leftPower;
double rightPower;
double terror; //turn error >:D
double gyroValue;
double gyroerror;
double turn_TotalError;

task gyroTask() //stolen from backwards crew :/
{
	long rate;
	long angle, lastAngle;
	lastAngle=0.0;
	gyroError=0.0;
	// Change sensitivity, this allows the rate reading to be higher
	setGyroSensitivity(gyro, gyroNormalSensitivity);
	//Reset the gyro sensor to remove any previous data.
	resetGyro(gyro);
	wait1Msec(1000);
	while (true) {
		rate = getGyroRate(gyro);
		angle = getGyroDegrees(gyro);
		// If big rate then ignore gyro changes
		if( abs( rate ) < 2 )
		{
			if( angle != lastAngle )
				gyroError += lastAngle - angle;
		}
		lastAngle = angle;
		gyroValue = angle + gyroError;
		wait1Msec(10);
	}
}

void Drive(double target) {
    double error = target;
    double lastError = target;
    double rError = target;
    double rLastError = target;
    wait1Msec(20);
    while(abs(error) > 3){
        error = target - getMotorEncoder(left);
        rError = target - getMotorEncoder(right);
        if(error == 0){
            break;
        }


        //left integral
        if(abs(error) < integralActiveZone && error != 0){
        totalError += error;
        }
        else totalError = 0.0;

        if(totalError > integralPowerLimit){
            totalError = integralPowerLimit;
        }
        if(totalError < -integralPowerLimit){
            totalError = -integralPowerLimit;
        }

        integral = ki * totalError;
        ///////

        //right integral
        if(abs(rError) < integralActiveZone && rError != 0){
        rtotalError += rerror;
        }
        else rtotalError = 0.0;

        if(rtotalError > integralPowerLimit){
            rtotalError = integralPowerLimit;
        }
        if(rtotalError < -integralPowerLimit){
            rtotalError = -integralPowerLimit;
        }

        rintegral = ki * rtotalError;
        ///////

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

        rderivative = kd * (rerror - rlastError);
        rlastError = rerror;

        if(error == 0){
            derivative = 0;
            rderivative = 0;
        }
        //terror = turn error lol
        terror = gyroValue*tp

        LeftPower = 0.5 * (kp * error + integral + derivative);
        RightPower = 0.5 * (kp * rError + rintegral + rderivative);
        setMotorSpeed(Left,LeftPower+terror);
        setMotorSpeed(Right,RightPower-terror);
        
        wait1Msec(15);
    }
    StopMultipleMotors(Left,Right);
}
void PID_turn(heading,speed,diff){ //currently a PI loop. Might add D later
    if (heading > 0){
        while(terror<2){
            terror = (heading - gyroValue)*tp;
            if(abs(error) < diff/3 && error != 0){
                    turn_totalError += terror;
            }
            setMotorSpeed(left,-speed-(terror+ti*turn_totalerror));
            setMotorSpeed(right,speed+terror+ti*turn_totalerror);
        }
    }
    else if (heading < 0){
        while(terror>-2){
            terror = (heading - gyroValue)*tp;
            if(abs(error) < diff/3 && error != 0){
                turn_totalError += terror;
            }
            setMotorSpeed(left,speed-(terror+ti*turn_totalerror));
            setMotorSpeed(right,-speed+terror+ti*turn_totalerror);
            wait1Msec(15);
        }
    }
    stopMotor(left);
    stopMotor(right);
}
2 Likes
2

You’d usually with all tuning constants being 0, then you repeatedly run a specific motion on the robot (driving forward for a certain amount) while gradually increasing your P term. If the control loop starts to cause the robot to oscillate around it’s endpoint, then you increase the D term until the oscillations stop, and repeat this until you can no longer stop the oscillations being caused by the P term(and reduce it to the highest value which did not cause oscillations).

And the I term can be ignored in most situations.

9 Likes
3

Firstly, @Gameoa is absolutely right on how a PID loop is typically tuned.

For your application, a P loop probably is and should be effective. The D term is typically used as a way to speed up the maneuver as a whole, specifically when the robot is faster and therefore further from its target.

As for the drive correction, I suspect that your P-value is too low. Try upping tp to, say, 5 or 10, and see if you notice a difference.

8 Likes