[pensono]

Hello, I am trying to set up a PID loop where I can tell my drive motors to move the robot at a certain velocity. I am using encoders to measure my current velocity, and I want to set up a PID loop to bridge the two.

I don't know how to set it up, but I will show what I have so far. I've cut it down to show only the left half of the drive train. We call the drive function to give it a throttle amount where 1 is max speed. Could somebody give example code, or correct my mistakes here so I can fix it?

Thanks!

DriveTrain.java:

public class DriveTrain extends Subsystem {
	private Encoder leftEncoder;

	private PIDController leftPID;

	public DriveTrain() {
		VictorSP frontLeft = new VictorSP(DRIVE_PORT_FRONT_LEFT);
		VictorSP rearLeft = new VictorSP(DRIVE_PORT_REAR_LEFT);
		double p = 1.0;
		double i = 0;
		double d = 0;

		leftEncoder = new Encoder(DRIVE_PORT_FRONT_LEFT, DRIVE_PORT_REAR_LEFT);
		leftEncoder.setDistancePerPulse((8 * Math.PI) / 360);
		leftEncoder.setPIDSourceType(PIDSourceType.kRate);

		leftPID = new PIDController(p, i, d, leftEncoder, new DualMotorPID(frontLeft, rearLeft));
		leftPID.setOutputRange(-.5, .5);
		leftPID.setContinuous(true);

		leftPID.enable();
	}

	public void initDefaultCommand() {
		setDefaultCommand(new TeleOpDrive());
	}

	public void drive(double moveValue, double rotateValue) {
		//Copied from RobotDrive::arcadeDrive
		double leftMotorSpeed, rightMotorSpeed;
		
		if (moveValue > 0.0) {
			if (rotateValue > 0.0) {
				leftMotorSpeed = moveValue - rotateValue;
				rightMotorSpeed = Math.max(moveValue, rotateValue);
			} else {
				leftMotorSpeed = Math.max(moveValue, -rotateValue);
				rightMotorSpeed = moveValue + rotateValue;
			}
		} else {
			if (rotateValue > 0.0) {
				leftMotorSpeed = -Math.max(-moveValue, rotateValue);
				rightMotor
Speed = moveValue + rotateValue;
			} else {
				leftMotorSpeed = moveValue - rotateValue;
				rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
			}
		}
		leftPID.setSetpoint(leftMotorSpeed);
	}
}

and DualMotorPID.java This lets us control two mechanically bound motors with one PID

public class DualMotorPID implements PIDOutput{
	private SpeedController motorA;
	private SpeedController motorB;
	
	public DualMotorPID(SpeedController a, SpeedController b){
		motorA = a;
		motorB = b;
	}
		
	@Override
	public void pidWrite(double output) {
		motorA.set(output);
		motorB.set(output);
	}
}