Merge branch 'working' of https://github.com/DriverStationComputer/RobotCode2018

Author: DriverStationComputer

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

@@ -165,12 +165,23 @@ public void teleopPeriodic() {
 		Scheduler.getInstance().run();
 	}
 
+	boolean firstTime = true;
 	/**
 	 * This function is called periodically during test mode
 	 */
 	@Override
 	public void testPeriodic() {
-//		Robot.dt.setLeft(0.2);
-		Robot.dt.setRight(0.2);
+		if(firstTime) {
+			Robot.dt.enableVelocityPIDs();
+			firstTime = false;
+		}
+		Robot.dt.setVPIDs(Robot.getConst("VPID Test Set", 0.5));
+		SmartDashboard.putNumber("Drivetrain/Left VPID Targ", Robot.dt.getLeftVPIDSetpoint());
+		SmartDashboard.putNumber("Drivetrain/Right VPID Targ", Robot.dt.getRightVPIDSetpoint());
+		SmartDashboard.putNumber("Left VPID Error", Robot.dt.getLeftVPIDerror());
+		SmartDashboard.putNumber("Right VPID Error", Robot.dt.getRightVPIDerror());
+		SmartDashboard.putNumber("Left Enc Rate", Robot.dt.getLeftEncRate());
+		SmartDashboard.putNumber("Right Enc Rate", Robot.dt.getRightEncRate());
+		
 	}
 }

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

@@ -52,6 +52,38 @@ public void setLeft(double spd) {
 	public void setRight(double spd) {
 		dtRight.set(spd);
 	}
+	
+	/**
+	 * Use for testing only (i.e. when not going through robotDrive)
+	 * */
+	public void setVPIDs(double realSpd) {
+		leftVelocityController.set(realSpd);
+		rightVelocityController.set(-realSpd);
+	}
+	
+	public double getLeftVPIDerror() {
+		return leftVelocityController.getError();
+	}
+
+	public double getRightVPIDerror() {
+		return rightVelocityController.getError();
+	}
+	
+	public double getLeftVPIDSetpoint() {
+		return leftVelocityController.get();
+	}
+
+	public double getRightVPIDSetpoint() {
+		return rightVelocityController.get();
+	}
+	
+	public double getLeftEncRate() {
+		return leftEncRate.getRate();
+	}
+	
+	public double getRightEncRate() {
+		return rightEncRate.getRate();
+	}
 
 	/**
 	 * Drives based on joystick input and SmartDashboard values