Code:
public class DriveTrain extends PIDSubsystem {
private Victor leftMotors[] = new Victor[2];
private Victor rightMotors[] = new Victor[2];
private Talon hDriveMotor;
private double lTarget;
private double rTarget;
private double lspeed;
private double rspeed;
private final double ACCEL;
private Encoder encoder;
/**
* This function is run when the class is initialized and should be
* used for any initialization code.
*/
public DriveTrain() {
super("Drive Train",3,2,0);
leftMotors[0] = new Victor(RobotMap.leftMotors[0]);
leftMotors[1] = new Victor(RobotMap.leftMotors[1]);
rightMotors[0] = new Victor(RobotMap.rightMotors[0]);
rightMotors[1] = new Victor(RobotMap.rightMotors[1]);
hDriveMotor = new Talon(RobotMap.hDriveMotor);
ACCEL = .1;
rspeed = 0;
lspeed = 0;
encoder = new Encoder(RobotMap.encoder[0], RobotMap.encoder[1],false, Encoder.EncodingType.k4X);
encoder.setDistancePerPulse(4*Math.PI);
}
/** Set the default command for a subsystem here.
* setDefaultCommand(new MySpecialCommand());
*/
public void initDefaultCommand() {
setDefaultCommand(new HDriveWithJoystick());
}
public void tankDrive(double leftSpeed, double rightSpeed) {
for (Victor lv : leftMotors)
lv.set(leftSpeed);
for (Victor rv : rightMotors)
rv.set(rightSpeed);
}
public void setHDriveMotor(double speed){
hDriveMotor.set(speed);
}
@Override
protected double returnPIDInput() {
// TODO Auto-generated method stub
return encoder.pidGet();
}
@Override
protected void usePIDOutput(double output) {
// TODO Auto-generated method stub
tankDrive(output,output);
}
}
This is our code, we do not know if we are setting up the encoder object correctly or know if we are instantiating the PID correctly outside of this class