Java PID control for mecanum drive w/ gyro w/o encoders

[nullmainmethod]

So our driver wants better control over our bot… which means incorporating PID loops. We have a fancy IMU (ADIS16448_IMU) but no encoders. He basically just wants it to hold straight when driving forward or translating but he also wants it to be able to rotate on a whim.

I’ve spent a good 30 minutes digging around for already existing implementations of this and I can’t find anything which pertains to this situation. We were using mecanumDrive_Cartesian without gyro input (didn’t want field alignment).

I have a fairly good understanding of PID loops so I don’t mind a complicated solution… the only thing I can’t think of how to calculate and compare expected headings to actual heading when the only thing you could use to approximate it is joystick input.

Is there a way to accomplish getting a locked in/straight control while just using a gyro and being able to rotate whenever? If so, what’s the concept behind calculating and comparing the expected and actual headings?

Thanks in advance… and sorry if there is a thread similar to this. As I said, I looked around but didn’t find anything.

[cpapplefamily]

We are using part of the code shared here https://www.chiefdelphi.com/forums/showthread.php?t=141120 take a look at the maintainheading. I’m on my phone so i can’t identify exsact code location other than the drive subsystem. We really like the maintain headding.

[zkeen]

We also do not like relative to field driving.

For driving straight forward, what we do is zero our out encoder (gyroangle-gyrozero), then as we are driving, multiply that calculated gyro angle by a scale to calculate a “rotation” value. We zero out our gyro on any rotation input by the driver. For straight mecanum strafing, we do the same thing. Before the strafe, we again zero out the gyro. Then we apply the same type of gyro adjust. If we notice we’re pointing a bit right, we add in more rotation, using the same scale.

All these are updated about 100 times per second. We are also using the same gyro and it seems to work well.

I would be happy to explain more or give a sample of our code. We write c++

Team 494
Goodrich Martians

[nullmainmethod]

@zkeen

We also do not like relative to field driving.

For driving straight forward, what we do is zero our out encoder (gyroangle-gyrozero), then as we are driving, multiply that calculated gyro angle by a scale to calculate a “rotation” value. We zero out our gyro on any rotation input by the driver. For straight mecanum strafing, we do the same thing. Before the strafe, we again zero out the gyro. Then we apply the same type of gyro adjust. If we notice we’re pointing a bit right, we add in more rotation, using the same scale.

All these are updated about 100 times per second. We are also using the same gyro and it seems to work well.

I would be happy to explain more or give a sample of our code. We write c++

Team 494
Goodrich Martians

So basically you just reset gyro after any rotation input whatsoever?

[cpapplefamily]

What i like about the code i pointed to is even when the bot is still or driving if there is no driver twist command the bot will fight to hold its heading.

[Oblarg]

Check our implementation of gyro drive straight. We’re running a tank drive, but the logic is basically exactly the same for mecanum.

[nullmainmethod]

@Oblarg

Check our implementation of gyro drive straight. We’re running a tank drive, but the logic is basically exactly the same for mecanum.

I’m trying to decipher this…

I took a look at the PIDController class… what would you use for PIDSource and PIDOutput when creating a PIDController object? Do you need to make one for each of the motors? (We have one motor per wheel)

[Oblarg]

@nullmainmethod

I’m trying to decipher this…

I took a look at the PIDController class… what would you use for PIDSource and PIDOutput when creating a PIDController object? Do you need to make one for each of the motors? (We have one motor per wheel)

We’re using the PIDCommand class, which manages the controller for you. You specify all the relevant source/output stuff in the overridden methods.

[nullmainmethod]

@Oblarg

We’re using the PIDCommand class, which manages the controller for you. You specify all the relevant source/output stuff in the overridden methods.

Checking out the PIDCommand class on WPI just confused me more… we are using iterative structure with multiple external classes. How does the PIDCommand class know what to output to/read from?

My understanding of command-based project structure isn’t very good, so maybe I’m missing something related to that. Is there a way I can implement something similar with iterative structure?

[Oblarg]

@nullmainmethod

Checking out the PIDCommand class on WPI just confused me more… we are using iterative structure with multiple external classes. How does the PIDCommand class know what to output to/read from?

My understanding of command-based project structure isn’t very good, so maybe I’m missing something related to that. Is there a way I can implement something similar with iterative structure?

You set what PIDCommand reads from by overriding the returnPIDInput method (not visible in our class there because it’s done in a general superclass that we wrote for gyro-based PIDCommands, “PIDAngleCommand”), and you set what it outputs to by overriding the usePIDOutput method. You never instantiate a PIDCommand, you always extend it.

Implementing something similar with iterative structure is possible but involves lots more overhead.

[nullmainmethod]

@Oblarg

You set what PIDCommand reads from by overriding the returnPIDInput method (not visible in our class there because it’s done in a general superclass that we wrote for gyro-based PIDCommands, “PIDAngleCommand”), and you set what it outputs to by overriding the usePIDOutput method. You never instantiate a PIDCommand, you always extend it.

Implementing something similar with iterative structure is possible but involves lots more overhead.

After reading a bit more here, I think I have an idea as to how we can do this. So for example…

Could we instantiate four PID controller objects (one for each motor, Spark ESC objects as our PIDOutputs, and gyro as our PIDSource) and just reset the gyro setpoint after there is any rotation input from the driver? Therefore error would always be corrected for in context to the most recent time the bot stopped rotating.

Would the PID controller objects handle all of the motor output or would we have to add in a mecanumDrive_Cartesian in somewhere and plug the outputs in…? How would the joystick communicate with the PID controller objects?

[Oblarg]

@nullmainmethod

After reading a bit more here, I think I have an idea as to how we can do this. So for example…

Could we instantiate four PID controller objects (one for each motor, Spark ESC objects as our PIDOutputs, and gyro as our PIDSource) and just reset the gyro setpoint after there is any rotation input from the driver? Therefore error would always be corrected for in context to the most recent time the bot stopped rotating.

Would the PID controller objects handle all of the motor output or would we have to add in a mecanumDrive_Cartesian in somewhere and plug the outputs in…? How would the joystick communicate with the PID controller objects?

You don’t want to run four loops in parallel like that - controlling the angle of your robot is a job for a single loop (at least at the top level - you can cascade to multiple loops lower down if you’re using speed control too, but I digress). If you want to stick to iterativerobot and avoid the command objects, the cleanest way to do it is probably to give the PIDController a dummy output (i.e. just an empty wrapper that implements speedcontroller), and then use the output yourself via. PIDController.get().

The idea is that, for a turning loop, the output from your loop turns into a speed differential between the two sides of the robot, as seen here in our code:

driveSubsystem.setDefaultThrottle(vel - output, vel + output);

Here, “output” is the output from the PID loop, and “vel” is our desired forward throttle. It would be similar with mecanum, except instead of a simple forward throttle you have whatever outputs are required for your desired translation.

[nullmainmethod]

@Oblarg

You don’t want to run four loops in parallel like that - controlling the angle of your robot is a job for a single loop (at least at the top level - you can cascade to multiple loops lower down if you’re using speed control too, but I digress). If you want to stick to iterativerobot and avoid the command objects, the cleanest way to do it is probably to give the PIDController a dummy output (i.e. just an empty wrapper that implements speedcontroller), and then use the output yourself via. PIDController.get().

The idea is that, for a turning loop, the output from your loop turns into a speed differential between the two sides of the robot, as seen here in our code:

driveSubsystem.setDefaultThrottle(vel - output, vel + output);

Here, “output” is the output from the PID loopo, and “vel” is our desired forward throttle. It would be similar with mecanum, except instead of a simple forward throttle you have whatever outputs are required for your desired translation.

I see what you’re doing with the throttle and the driveSubsystem… but I’m confused as to how one output from a turning loop can be given to 4 ESCs. Is the output value of the PID controller object just a correction amount for rotation? Looks like you’re just adding it on top of what the joystick is telling the bot to do with that

driveSubsystem.setDefaultThrottle(vel - output, vel + output);

line. Thanks so much for the responses by the way, would be lost without them

[Oblarg]

@nullmainmethod

I see what you’re doing with the throttle and the driveSubsystem… but I’m confused as to how one output from a turning loop can be given to 4 ESCs. Is the output value of the PID controller object just a correction amount for rotation? Looks like you’re just adding it on top of what the joystick is telling the bot to do with that

driveSubsystem.setDefaultThrottle(vel - output, vel + output);

line. Thanks so much for the responses by the way, would be lost without them

The drive code logic goes as follows:

We have two joystick axes: vel and rot.

If rot is nonzero, we’re commanding a turn, and thus are “free driving.” In this state, the gyro is not used at all, and the drive outputs are completely as normal: (vel - rot, vel + rot). (That is, the left side of the drive is commanded to speed vel - rot, and the right to vel + rot).

If rot is zero (i.e. no turn is commanded) and the robot is no longer rotating (this latter condition is *extremely *important for handling purposes - you can detect it with the gyro), then we enter “drive straight” mode. In this mode, we run a PID loop on the robot’s angular error, and feed the output to our drive command: (vel - output, vel + output). Thus, any angular error is, in essence, responded to by a corrective turn command. This keeps the robot driving straight.

(Not visible in this version of the code, we’ve also added a 250ms delay before the onset of drive straight mode, which helps handling somewhat, as well - YMMV on what tweaks you find necessary to get the thing running well.)

For mecanum, it would function exactly the same - only your “vel” term is more complicated because you can translate in any direction.

In order to use the output of the PIDController object in multiple motor controllers at once, give the PIDController an empty wrapper that implements speedcontroller as its output - you will not be using the automatic output. Instead, set a variable equal to the current output of the loop with PIDController.get(), and then send that manually to your drive command.

[nullmainmethod]

@Oblarg

The drive code logic goes as follows:

We have two joystick axes: vel and rot.

If rot is nonzero, we’re commanding a turn, and thus are “free driving.” In this state, the gyro is not used at all, and the drive outputs are completely as normal: (vel - rot, vel + rot). (That is, the left side of the drive is commanded to speed vel - rot, and the right to vel + rot).

If rot is zero (i.e. no turn is commanded) and the robot is no longer rotating (this latter condition is *extremely *important for handling purposes - you can detect it with the gyro), then we enter “drive straight” mode. In this mode, we run a PID loop on the robot’s angular error, and feed the output to our drive command: (vel - output, vel + output). Thus, any angular error is, in essence, responded to by a corrective turn command. This keeps the robot driving straight.

(Not visible in this version of the code, we’ve also added a 250ms delay before the onset of drive straight mode, which helps handling somewhat, as well - YMMV on what tweaks you find necessary to get the thing running well.)

For mecanum, it would function exactly the same - only your “vel” term is more complicated because you can translate in any direction.

In order to use the output of the PIDController object in multiple motor controllers at once, give the PIDController an empty wrapper that implements speedcontroller as its output - you will not be using the automatic output. Instead, set a variable equal to the current output of the loop with PIDController.get(), and then send that manually to your drive command.

I’ve been toying around with this for an hour now and I’m not getting anywhere.

To give you an idea of our project structure: Our teleopPeriodic() method in Robot.java repeatedly calls a function called teleopDrive() in another class called Drivetrain. I’m trying to declare a PIDController object within my Drivetrain class, with the gyro’s built in getPIDSourceType() method as the PIDSource and an empty “wrapper” class called WrapperController which does nothing but implement the PIDOutput interface.

Should there be any code inside of the implemented pidWrite() method in the WrapperController class? When do I enable the PID loop/reset the setpoint?

Here’s a simplified version of what I have so far (question marks are things I can’t figure out what to add in):

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.wpilibj.Timer;

public class Drivetrain {
	private final RobotDrive drivetrain;
	private final Spark frontLeftMotor, frontRightMotor, rearLeftMotor, rearRightMotor;
	private Timer timer = new Timer();
	private PIDController pid;
	private WrapperController wrapper;

	public Drivetrain (Spark fL, Spark fR, Spark rL, Spark rR) {
		frontLeftMotor = fL;
		frontRightMotor = fR;
		rearLeftMotor = rL;
		rearRightMotor = rR;
		
		drivetrain = new RobotDrive(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
		drivetrain.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
		drivetrain.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);

		frontLeftMotor.enableDeadbandElimination(true);
		frontRightMotor.enableDeadbandElimination(true);
		rearLeftMotor.enableDeadbandElimination(true);
		rearRightMotor.enableDeadbandElimination(true);
		
		wrapper = new WrapperController();
		pid = new PIDController(0.1, 0, 0, OI.gyro.getPIDSourceType(), wrapper);
	}
	
	public void teleopDrive() {
		if(Math.abs(OI.joystick1.getTwist()) > 0.1) { //free drive
			drivetrain.mecanumDrive_Cartesian(OI.joystick1.getX(), OI.joystick1.getY(), OI.joystick1.getTwist(), 180);
			timer.reset();
		}
		else if(timer.get() > 0.2) { //delay 200ms after free rotation before enabling PID control
			pid.setSetpoint(OI.gyro.getAngleZ());
			while(Math.abs(OI.joystick1.getTwist()) > 0.1) {
				drivetrain.mecanumDrive_Cartesian(OI.joystick1.getX(), OI.joystick1.getY()+??, 180)
			}
		}
	}
}

And here is the WrapperController class:

import edu.wpi.first.wpilibj.PIDOutput;

public class WrapperController implements PIDOutput {

	@Override
	public void pidWrite(double output) {
		// TODO Auto-generated method stub

	}

}

To note: The ADIS16448_IMU libraries have a .getPIDSourceType() method. Here is the note in the Javadoc: “Get which parameter of the device you are using as a process control variable.”

[Oblarg]

You don’t need to put anything in the wrapper - it is totally empty, and exists merely to satisfy the library arbitrarily requiring that you give it something to call pidWrite on. You will be using the loop output manually.

You’ve made a good start - to finish, you need to configure the PIDController, and then use its output (via. the get() function) in the else block of your drive code. To do this:

Firstly, your constructor is not quite right - you need to pass the gyro object itself as the source, not its source type. The PIDController will then call pidGet() on it automatically. You will likely need to set the source type of the gyro so that it returns angle, rather than angular velocity - the gyro documentation likely will tell you how to do this.

Secondly, you need to configure the input range of your PIDController. This should be either 0 to 360 or -180 to 180, depending on how your gyro object implements pidGet(). You also will need to set the input range to be continuous (using setContinuous()) - this indicates that the values are on a circle (rather than a line), which is an important property of headings ;). You may also want to configure an output range, but it is not strictly necessary - however, you must configure the input range for a gyro loop to work properly.

You can forego configuring tolerance, as you will not be exiting the loop when you reach your setpoint.

After that’s done, you need to to make some small modifications to your drive code so that it actually uses the output.

Firstly, you should add a “drivingStraight” boolean so you can detect the state change, rather than just the current state - there are some things you want to do explicitly when you transition between turning and driving straight and vice-versa.

When you go from driving straight to turning, you want to disable and reset the PIDController. This will ensure the controller is not running when you don’t need it to (this isn’t so important since it’s not automatically outputting, but is still good practice) and that you don’t “carry over” any accumulated integral error from one instance of driving straight to the next.

When you go from turning to driving straight, you want to enable the PIDController and set the setpoint to the current heading. You must do this on the state change - you cannot simply keep doing this every iteration, or your setpoint will just track your current heading and the loop will be useless.

Finally, you simply need to send pid.get() (this is the output of the controller!) as the turn command in the drive straight portion (I believe this is what you’re currently using joystick twist for in your turning case - I strongly suggest you switch to a two-joystick setup and use the x axis of a second joystick for this in the interest of drivebility), and then you should be good to go. Keep in mind, you might need to negate this value if your gyro angle’s “positive” is not the same as your drive subroutine’s “positive.” You’ll be able to see this, because it will result in an unstable loop.

[nullmainmethod]

@Oblarg

You don’t need to put anything in the wrapper - it is totally empty, and exists merely to satisfy the library arbitrarily requiring that you give it something to call pidWrite on. You will be using the loop output manually.

You’ve made a good start - to finish, you need to configure the PIDController, and then use its output (via. the get() function) in else block of your drive code. To do this:

Firstly, your constructor is not quite right - you need to pass the gyro object itself as the source, not its source type. The PIDController will then call pidGet() on it automatically. You will likely need to set the source type of the gyro so that it returns angle, rather than angular velocity - the gyro documentation likely will tell you how to do this.

Secondly, you need to configure the input range of your PIDController. This should be either 0 to 360 or -180 to 180, depending on how your gyro object implements pidGet(). You also will need to set the input range to be continuous (using setContinuous()) - this indicates that the values are on a circle (rather than a line), which is an important property of headings ;). You may also want to configure an output range, but it is not strictly necessary - however, you must configure the input range for a gyro loop to work properly.

You can forego configuring tolerance, as you will not be exiting the loop when you reach your setpoint.

After that’s done, you need to to make some small modifications to your drive code so that it actually uses the output.

Firstly, you should add a “drivingStraight” boolean so you can detect the state change, rather than just the current state - there are some things you want to do explicitly when you transition between turning and driving straight and vice-versa.

When you go from driving straight to turning, you want to disable and reset the PIDController. This will ensure the controller is not running when you don’t need it to (this isn’t so important since it’s not automatically outputting, but is still good practice) and that you don’t “carry over” any accumulated integral error from one instance of driving straight to the next.

When you go from turning to driving straight, you want to enable the PIDController and set the setpoint to the current heading. You must do this on the state change - you cannot simply keep doing this every iteration, or your setpoint will just track your current heading and the loop will be useless.

Finally, you simply need to send pid.get() (this is the output of the controller!) as the turn command in the drive straight portion (I believe this is what you’re currently using joystick twist for in your turning case - I strongly suggest you switch to a two-joystick setup and use the x axis of a second joystick for this in the interest of drivebility), and then you should be good to go. Keep in mind, you might need to negate this value if your gyro angle’s “positive” is not the same as your drive subroutine’s “positive.” You’ll be able to see this, because it will result in an unstable loop.

Thanks for such a comprehensive answer… I’ll give all of this a shot probably tonight or tomorrow night. The more I use this site the more I wonder why I didn’t join it earlier.

[nullmainmethod]

@Oblarg

You don’t need to put anything in the wrapper - it is totally empty, and exists merely to satisfy the library arbitrarily requiring that you give it something to call pidWrite on. You will be using the loop output manually.

You’ve made a good start - to finish, you need to configure the PIDController, and then use its output (via. the get() function) in the else block of your drive code. To do this:

Firstly, your constructor is not quite right - you need to pass the gyro object itself as the source, not its source type. The PIDController will then call pidGet() on it automatically. You will likely need to set the source type of the gyro so that it returns angle, rather than angular velocity - the gyro documentation likely will tell you how to do this.

Secondly, you need to configure the input range of your PIDController. This should be either 0 to 360 or -180 to 180, depending on how your gyro object implements pidGet(). You also will need to set the input range to be continuous (using setContinuous()) - this indicates that the values are on a circle (rather than a line), which is an important property of headings ;). You may also want to configure an output range, but it is not strictly necessary - however, you must configure the input range for a gyro loop to work properly.

You can forego configuring tolerance, as you will not be exiting the loop when you reach your setpoint.

After that’s done, you need to to make some small modifications to your drive code so that it actually uses the output.

Firstly, you should add a “drivingStraight” boolean so you can detect the state change, rather than just the current state - there are some things you want to do explicitly when you transition between turning and driving straight and vice-versa.

When you go from driving straight to turning, you want to disable and reset the PIDController. This will ensure the controller is not running when you don’t need it to (this isn’t so important since it’s not automatically outputting, but is still good practice) and that you don’t “carry over” any accumulated integral error from one instance of driving straight to the next.

When you go from turning to driving straight, you want to enable the PIDController and set the setpoint to the current heading. You must do this on the state change - you cannot simply keep doing this every iteration, or your setpoint will just track your current heading and the loop will be useless.

Finally, you simply need to send pid.get() (this is the output of the controller!) as the turn command in the drive straight portion (I believe this is what you’re currently using joystick twist for in your turning case - I strongly suggest you switch to a two-joystick setup and use the x axis of a second joystick for this in the interest of drivebility), and then you should be good to go. Keep in mind, you might need to negate this value if your gyro angle’s “positive” is not the same as your drive subroutine’s “positive.” You’ll be able to see this, because it will result in an unstable loop.

I went ahead and tried to implement this. Let me know what you think and if you see any obvious problems. To note: I couldn’t find any information about setting an input range with my gyro or setting continuity. It did have a comment saying this though:

 The Yaw "South" crossing detector is necessary to allow a smooth
    // transition across the +/- 180 deg discontinuity (inherent in the ATAN
    // function).  Since -180 deg is congruent with +180 deg, Yaw needs to jump
    // between these values when crossing South (North is 0 deg).  The design
    // depends upon comparison of successive Yaw readings to detect a
    // cross-over event.  The cross-over detector monitors the current reading
    // and evaluates how far it is from the previous reading.  If it is greater
    // than the previous reading by the Discriminant (= 180 deg), then Yaw just
    // crossed South.
    // 

    // Since a Yaw of -180 degrees is congruent with +180 degrees (they
    // represent the same direction), it is possible that the Yaw output will
    // oscillate between these two values when the sensor happens to be
    // pointing due South, as sensor noise causes slight variation.  You will
    // need to account for this possibility if you are using the Yaw value for
    // decision-making in code.

So I think it is automatically set to +/- 180… wish I could test it out but we left the gyro on the bot and it’s currently bagged…

Here’s my declaration for the PIDController:

private PIDController pid = new PIDController(0.01, 0, 0, OI.gyro, (PIDOutput) new Object());
	private boolean PIDControlEnabled;

The casted Object is the “wrapper”

Here’s what I have changed my teleopDrive() method to:
if(PIDControlEnabled) {
drivetrain.mecanumDrive_Cartesian(OI.joystick1.getX(), OI.joystick1.getY(), OI.joystick1.getTwist()+pid.get(), 180);
}
else {
drivetrain.mecanumDrive_Cartesian(OI.joystick1.getX(), OI.joystick1.getY(), OI.joystick1.getTwist(), 180);
}

	if(Math.abs(OI.joystick1.getTwist()) > 0.1) { //free drive
		timer.reset();
		timer.start();
		pid.disable();
		PIDControlEnabled = false;
	}
	else if(timer.get() > 0.2 && PIDControlEnabled != true) { //delay 200ms after free rotation before enabling PID control
		pid.reset();
		pid.setSetpoint(OI.gyro.getAngleZ());
		pid.enable();
		PIDControlEnabled = true;
	}

And here’s what I do in robotInit() to intialize the gyro:

OI.gyro.calibrate();
		OI.gyro.reset();
		OI.gyro.setPIDSourceType(PIDSourceType.kDisplacement);
		OI.gyro.setTiltCompYaw(false);

Would I have to change the setPoint by 90 degrees whenever the driver wants to strafe? Not too sure about just plugging in the correction amount into the mecanumDrive_Cartesian method…

[nullmainmethod]

@nullmainmethod

Here’s what I have changed my teleopDrive() method to:
if(PIDControlEnabled) {
drivetrain.mecanumDrive_Cartesian(OI.joystick1.getX(), OI.joystick1.getY(), OI.joystick1.getTwist()+pid.get(), 180);
}
else {
drivetrain.mecanumDrive_Cartesian(OI.joystick1.getX(), OI.joystick1.getY(), OI.joystick1.getTwist(), 180);
}

	if(Math.abs(OI.joystick1.getTwist()) > 0.1) { //free drive
		timer.reset();
		timer.start();
		pid.disable();
		PIDControlEnabled = false;
	}
	else if(timer.get() > 0.2 && PIDControlEnabled != true) { //delay 200ms after free rotation before enabling PID control
		pid.reset();
		pid.setSetpoint(OI.gyro.getAngleZ());
		pid.enable();
		PIDControlEnabled = true;
	}

Sorry, forgot to add code tags (for legibility):

public void teleopDrive() { 
		if(PIDControlEnabled) {
			drivetrain.mecanumDrive_Cartesian(OI.joystick1.getX(), OI.joystick1.getY(), OI.joystick1.getTwist()+pid.get(), 180);
		}
		else {
			drivetrain.mecanumDrive_Cartesian(OI.joystick1.getX(), OI.joystick1.getY(), OI.joystick1.getTwist(), 180);
		}
		
		if(Math.abs(OI.joystick1.getTwist()) > 0.1) { //free drive
			timer.reset();
			timer.start();
			pid.disable();
			PIDControlEnabled = false;
		}
		else if(timer.get() > 0.2 && PIDControlEnabled != true) { //delay 200ms after free rotation before enabling PID control
			pid.reset();
			pid.setSetpoint(OI.gyro.getAngleZ());
			pid.enable();
			PIDControlEnabled = true;
		}
			
	}

[Oblarg]

This looks more or less correct - except, you do not need to have joystick.getTwist() term in your drive command in the driving straight case because you are already requiring that it be zero to enter that state in the first place, so you can change that to just be pid.get(). Let me know how it works when you get a chance to test it!

Some notes for tuning: Angle control loops can be a bit touchy to get right if you’re not “cascading” the drive outputs to PID velocity control loops. You may need to “fudge” the output in certain ways for it to work optimally. A couple of suggestions (none of these are innately supported by the PIDController object, sadly, but they are easy to implement yourself), if you find yourself unable to tune the loop satisfactorily:

1. Add a “deadband” around your setpoint within which the output is set to zero. Note that the various “tolerance” methods on the PIDController object do not do this! They only affect whether or not onTarget() returns true or false.

2. Add (or subtract, depending on the sign of your output) some “minimum output” to the output of your loop to help deal with nonlinearity introduced by stiction. I have found this to be the most useful single change one can make to a simple control loop like this.

Also, from a “programming best practices” standpoint, I strongly suggest you don’t hard-code parameters (such as the various deadbands and timer durations) like you’re doing there - it will make tuning the thing a headache later on. Eventually you should look into setting up a system to read such parameters from a config file, but for now, you should set them all in one place in code. As a rule, writing code and configuring code should be kept as separate as possible - it’s a bit more effort in the moment, but it pays off hugely in the long-run.