PID Controller behaving erratically

This year, my team is trying to get our robot to drive straight using a PID controller and a NavX rather than having our driver correct for it manually. However, whenever I do this, the robot seems to shake left and right. I’ve tried raising/lowering the P value to get the robot to oscillate around the 0 degree mark, but once it gets to the point, it starts to shake rather violently. Changing the D value seems to make next to no difference, and if anything the robot seemed to shake more violently.

From some googling, it seems that the cause is a P value that is too high, but I can’t seem to find a P value where the robot doesn’t either shake, brown out, or not have enough power to move. I’ve posted the relevant pieces of my code below. Can someone help me out? Thanks so much.

public class Robot extends IterativeRobot implements PIDOutput {
    double turnCompensation = 0;
    final double kP = 0.01;
    final double kI = 0.00;
    final  double kD = 0.00;
    final double kF = 0.00;	
    CANTalon frontLeft = new CANTalon(2);
    CANTalon backLeft = new CANTalon(3);
    CANTalon frontRight = new CANTalon(4);
    CANTalon backRight = new CANTalon(5);
    Joystick joystick = new Joystick(0);
    Compressor compressor = new Compressor();
    int toggleDirection = -1;
    RobotDrive robotDrive = new RobotDrive(frontLeft, backLeft, frontRight, backRight);
    AHRS gyro = new AHRS(SPI.Port.kMXP);
    PIDController straightController = new PIDController(kP, kI, kD, kF, gyro, this);

	private double getLeftStick(){
		return joystick.getRawAxis(1);
	private double getRightStick(){
		return joystick.getRawAxis(5);
	private double getCorrectedRightStick(){
		double correctedValue = getRightStick() * 2;
		return correctedValue < 1 ? correctedValue : 1;
	private double getCorrectedLeftStick(){
		double correctedValue = getLeftStick() * 2;
		return correctedValue < 1 ? correctedValue : 1;
	public void robotInit(){
	    straightController.setInputRange(0,  360);
	    straightController.setOutputRange(-1.0, 1.0);
            SmartDashboard.putData("DrivePID", straightController);
	public void autonomousInit(){
	//Called periodically during auto
	//Use quick operations, must be done in less than 20ms.
	public void autonomousPeriodic(){
	public void teleopInit(){

	public void teleopPeriodic(){
		SmartDashboard.putNumber("gyroValue", gyro.getAngle());
		if (joystick.getRawButton(8)){
			toggleDirection = 1;
		else {
			toggleDirection = -1;
		robotDrive.arcadeDrive(getCorrectedLeftStick() * toggleDirection, turnCompensation);
	public void disabledInit(){

	public void pidWrite(double output) {
		turnCompensation = output;

Check out this thread:

So is the answer just to pick better values? Alright. I’ll try this when I’m in the lab tomorrow.

Set the kP just low enough that you don’t get oscillation. As you’ve seen, that doesn’t always give enough power to get to the desired target, and you get a steady-state error. Now increase kI just high enough to overcome that error. Then tweak kD to account for rotational inertia so it doesn’t overshoot badly and oscillate again.

It helps if you can modify the PID constants while the code is running. Perhaps you could use SmartDashboard for that.