PID controller that would allow me to drive forward in auto

Technical Java
1

I want to create a command that uses pid with a ultrasonic sensor to drive forward a distance i set. I looked into some example code and i made one of my own. I am using the command based robot with robot builder. Keep in mind i have no prior knowledge of pid controllers so if it wrong dont be surprised.

The Code is below in a command

package org.usfirst.frc5713.BuckyBot.commands;

import org.usfirst.frc5713.BuckyBot.Robot;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;
import edu.wpi.first.wpilibj.PIDSource;
import edu.wpi.first.wpilibj.PIDSourceType;
import edu.wpi.first.wpilibj.command.Command;

/** This Class drive the robot forward using a rangefinder a set distance you determine
*
*/
public class SetDistanceToPeg extends Command {

private PIDController pid;


public SetDistanceToPeg(double distance) {
	requires(Robot.driveTrain);
	pid = new PIDController(-2, 0, 0, new PIDSource() {
		PIDSourceType m_sourceType = PIDSourceType.kDisplacement;

		@Override
		public double pidGet() {
			return Robot.driveTrain.getDistanceToObstacle();
		}

		@Override
		public void setPIDSourceType(PIDSourceType pidSource) {
			m_sourceType = pidSource;
		}

		@Override
		public PIDSourceType getPIDSourceType() {
			return m_sourceType;
		}
	}, new PIDOutput() {
		@Override
		public void pidWrite(double d) {
			Robot.driveTrain.drive(d, d);
		}
	});
	
	pid.setAbsoluteTolerance(0.01);
	pid.setSetpoint(distance); 
}

// Called just before this Command runs the first time
@Override
protected void initialize() {
	// Get everything in a safe starting state.
	Robot.driveTrain.reset();
	pid.reset();
	pid.enable();
}

// Make this return true when this Command no longer needs to run execute()
@Override
protected boolean isFinished() {
	return pid.onTarget();
}

// Called once after isFinished returns true
@Override
protected void end() {
	// Stop PID and the wheels
	pid.disable();
	Robot.driveTrain.drive(0, 0);
}

}

2

Have you tried running this program? If so, what were the results?
After a quick scan of the code, the negative P value jumps out at me as odd. Also, have you confirmed that getDistanceToObstacle() is returning a good value?

3

We switched to using encoders