Gyro being weird and Robot Not Turning in a PID loop

VEX IQ Challenge Discussion
#1

The turn function doesn’t work at all. after looking at it for a long time i can’t find any bugs. Another issue with the pid loop is the drive function doesn’t correct overshooting. when it crosses the encoder target, it just stops and drifts instead of oscillating(we haven’t tuned it yet). What should i use instead of a while(abs(error)<3) for the loop?

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

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

//double proportion;
double totalError;
double rtotalError;
double integral;
double rintegral;
double integralActiveZone = 200;//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;
double tderivative;

int side;

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
    terror = heading - gyroValue;
    tlasterror = heading - gyrovalue;
    if (heading > gyroValue){
        side = -1;
    }
    else{
        side = 1;
    }
    while(abs(terror)<1.5){
        terror = heading - gyroValue;
        
        if(abs(error) < diff/3 && error != 0){
                turn_totalError += terror;
        }    
        tderivative = td * (terror - tlastError);
        tlastError = error;

        if(error == 0){
            tderivative = 0;
        }

        LeftPower = tp * terror + ti * turn_TotalError + tderivative;
        rightPower = tp * terror + ti * turn_TotalError + tderivative;
        setMotorSpeed(left,side*LeftPower);
        setMotorSpeed(right,-1*side*rightPower);
        wait1Msec(15);
    }
    
    
    
    
    
    /*
    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);
}
1 Like
#2

Why did you put the turning PID in pseudo code? Did you just do that to highlight the correct area we are supposed to look at, or is that in the program the whole time?

2 Likes
#3

it’s not pseudocode, i just commented out the old obscure code in case i need it again.

1 Like
#4

Comment is what I meant to say… not pseudocode. Sorry for the confusion.

1 Like
#5

a video of what is happening would be helpful. When I am debugging PID loops in V5, I try to display the the error and motor setpoints on the screen to observe what the robot is trying to do. I would try printing your error values and motor setpoints to see what the robot is doing around the target angle.

1 Like
#6

So I figured out the issue was i didn’t calibrate the gyro at the start of the program and their were mathematical errors for setting the motor power. Now everything works fine : )

2 Likes