PIDMove works (actually for good this time)

changed some things in shiftGears
fixes w/ SDInterface really quick
moveController no longer sets its range based on speed (bc that made
sense)

Author: lhmcgann

Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java

@@ -78,6 +78,11 @@ public void putData(String string, PIDController controller) {
 		public void putNumber(String string, double d) {
 			SmartDashboard.putNumber(string, d);
 		}
+
+		public void putBoolean(String string, boolean b) {
+			SmartDashboard.putBoolean(string, b);
+		}
+
 		/*
 		 * if (!SmartDashboard.containsKey("Const/" + key)) { if
 		 * (!SmartDashboard.putNumber("Const/" + key, def)) {
@@ -114,7 +119,7 @@ public void robotInit() {
 		climberAssist = new ClimberAssist();
 		intakeEject = new IntakeEject();
 		lift = new Lift();
-		dt = new Drivetrain();
+		dt = new Drivetrain(sd);
 		oi = new OI();
 
 		// auto position chooser

Robot2018/src/org/usfirst/frc/team199/Robot2018/SmartDashboardInterface.java

@@ -8,4 +8,6 @@ public interface SmartDashboardInterface {
 	public void putData(String string, PIDController controller);
 
 	public void putNumber(String string, double d);
+
+	public void putBoolean(String string, boolean b);
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/PIDMove.java

@@ -151,12 +151,10 @@ public void initialize() {
 
 		dt.resetDistEncs();
 		moveController.disable();
-		// input is in inches
-		moveController.setInputRange(-dt.getCurrentMaxSpeed(), dt.getCurrentMaxSpeed());
 		// output in "motor units" (arcade and tank only accept values [-1, 1]
 		moveController.setOutputRange(-1.0, 1.0);
 		moveController.setContinuous(false);
-		moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.1));
+		moveController.setAbsoluteTolerance(Robot.getConst("MoveTolerance", 0.5));
 		System.out.println("move target = " + target);
 		moveController.setSetpoint(target);
 

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftHighGear.java

@@ -3,7 +3,6 @@
 import org.usfirst.frc.team199.Robot2018.Robot;
 
 import edu.wpi.first.wpilibj.command.InstantCommand;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * Shifts the DT to high gear.
@@ -20,7 +19,5 @@ public ShiftHighGear() {
 	@Override
 	protected void initialize() {
 		Robot.dt.shiftGears(true);
-		SmartDashboard.putBoolean("High Gear", true);
-		Robot.dt.resetAllVelocityPIDConsts();
 	}
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/ShiftLowGear.java

@@ -3,7 +3,6 @@
 import org.usfirst.frc.team199.Robot2018.Robot;
 
 import edu.wpi.first.wpilibj.command.InstantCommand;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * Shifts the DT to low gear.
@@ -20,7 +19,5 @@ public ShiftLowGear() {
 	@Override
 	protected void initialize() {
 		Robot.dt.shiftGears(false);
-		SmartDashboard.putBoolean("High Gear", false);
-		Robot.dt.resetAllVelocityPIDConsts();
 	}
 }

Robot2018/src/org/usfirst/frc/team199/Robot2018/commands/TeleopDrive.java

@@ -32,8 +32,13 @@ protected void initialize() {
 	@Override
 	protected void execute() {
 		if (Robot.getBool("Teleop velocity PID", false)) {
-			Robot.dt.resetVPIDInputRanges();
+			Robot.dt.resetVPIDAndRobotDriveRanges();
 			Robot.dt.resetAllVelocityPIDConsts();
+			if (Robot.dt.VPIDsOnTarg()) {
+				Robot.dt.disableVelocityPIDs();
+			} else {
+				Robot.dt.enableVelocityPIDs();
+			}
 		}
 		Robot.dt.teleopDrive();
 	}

Robot2018/src/org/usfirst/frc/team199/Robot2018/subsystems/Drivetrain.java

@@ -9,6 +9,7 @@
 
 import org.usfirst.frc.team199.Robot2018.Robot;
 import org.usfirst.frc.team199.Robot2018.RobotMap;
+import org.usfirst.frc.team199.Robot2018.SmartDashboardInterface;
 import org.usfirst.frc.team199.Robot2018.autonomous.PIDSourceAverage;
 import org.usfirst.frc.team199.Robot2018.autonomous.VelocityPIDController;
 import org.usfirst.frc.team199.Robot2018.commands.TeleopDrive;
@@ -48,7 +49,15 @@ public class Drivetrain extends Subsystem implements DrivetrainInterface {
 	private final AHRS fancyGyro = RobotMap.fancyGyro;
 	private final DoubleSolenoid dtGear = RobotMap.dtGear;
 
-	public Drivetrain() {
+	private SmartDashboardInterface sd;
+
+	private boolean highGear;
+
+	public Drivetrain(SmartDashboardInterface sd) {
+		this.sd = sd;
+
+		highGear = false;
+
 		// all 0s for controller construction because they all get set to right values
 		// by resetAllVelocityPIDConsts
 		leftVelocityController = new VelocityPIDController(0, 0, 0, 0, leftEncRate, dtLeft);
@@ -73,9 +82,7 @@ public Drivetrain() {
 			robotDrive.setMaxOutput(Robot.getConst("Max High Speed", 204));
 		} else {
 			robotDrive = new DifferentialDrive(dtLeft, dtRight);
-
 		}
-
 	}
 
 	@Override
@@ -131,6 +138,10 @@ public double getRightVPIDOutput() {
 		return dtRight.get();
 	}
 
+	public boolean VPIDsOnTarg() {
+		return leftVelocityController.onTarget() && rightVelocityController.onTarget();
+	}
+
 	/**
 	 * Returns the getRate() of the left encoder
 	 * 
@@ -374,12 +385,17 @@ public double getRightEncDist() {
 	 *            low gear (false, kForward)
 	 */
 	@Override
-	public void shiftGears(boolean highGear) {
-		if (highGear ^ Robot.getBool("Drivetrain Gear Shift Low", false)) {
+	public void shiftGears(boolean shiftToHighGear) {
+		if (shiftToHighGear ^ Robot.getBool("Drivetrain Gear Shift Low", false)) {
 			dtGear.set(DoubleSolenoid.Value.kReverse);
+			highGear = true;
 		} else {
 			dtGear.set(DoubleSolenoid.Value.kForward);
+			highGear = false;
 		}
+		sd.putBoolean("High Gear", highGear);
+		resetAllVelocityPIDConsts();
+		resetVPIDAndRobotDriveRanges();
 	}
 
 	/**
@@ -458,10 +474,13 @@ public void resetVelocityPIDkPConsts() {
 		SmartDashboard.putNumber("VPID Right kP", newkP);
 	}
 
-	public double resetVPIDInputRanges() {
+	public double resetVPIDAndRobotDriveRanges() {
 		double currentMaxSpd = getCurrentMaxSpeed();
 		leftVelocityController.setInputRange(-currentMaxSpd, currentMaxSpd);
 		rightVelocityController.setInputRange(-currentMaxSpd, currentMaxSpd);
+		if (Robot.getBool("Teleop velocity PID", false)) {
+			robotDrive.setMaxOutput(currentMaxSpd);
+		}
 		return currentMaxSpd;
 	}
 
@@ -472,7 +491,7 @@ public double resetVPIDInputRanges() {
 	 */
 	@Override
 	public double getCurrentMaxSpeed() {
-		if (SmartDashboard.getBoolean("High Gear", true)) {
+		if (highGear) {
 			return Robot.getConst("Max High Speed", 204);
 		} else {
 			return Robot.getConst("Max Low Speed", 84);