Merge branch 'kevins-auto'

Author: DriverStationComputer

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

@@ -25,6 +25,7 @@
 import edu.wpi.first.wpilibj.CameraServer;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.IterativeRobot;
+import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.Preferences;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
@@ -69,6 +70,14 @@ public double getConst(String key, double def) {
 			}
 			return pref.getDouble("Const/" + key, def);
 		}
+
+		public void putData(String string, PIDController controller) {
+			SmartDashboard.putData(string, controller);
+		}
+
+		public void putNumber(String string, double d) {
+			SmartDashboard.putNumber(string, d);
+		}
 		/*
 		 * if (!SmartDashboard.containsKey("Const/" + key)) { if
 		 * (!SmartDashboard.putNumber("Const/" + key, def)) {

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

@@ -1,5 +1,11 @@
 package org.usfirst.frc.team199.Robot2018;
 
+import edu.wpi.first.wpilibj.PIDController;
+
 public interface SmartDashboardInterface {
 	public double getConst(String key, double def);
+
+	public void putData(String string, PIDController controller);
+
+	public void putNumber(String string, double d);
 }

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

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
  * Drives the robot a certain target distance using PID. Implements PIDOutput in
@@ -21,6 +20,7 @@ public class PIDMove extends Command implements PIDOutput {
 	private DrivetrainInterface dt;
 	private PIDController moveController;
 	private PIDSource avg;
+	private SmartDashboardInterface sd;
 
 	/**
 	 * Constructs this command with a new PIDController. Sets all of the
@@ -43,13 +43,14 @@ public PIDMove(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		target = targ;
 		this.dt = dt;
 		this.avg = avg;
+		this.sd = sd;
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
 		double kf = 1 / (dt.getCurrentMaxSpeed() * sd.getConst("Default PID Update Time", 0.05));
 		moveController = new PIDController(sd.getConst("MovekP", 0.1), sd.getConst("MovekI", 0),
 				sd.getConst("MovekD", 0), kf, avg, this);
-		SmartDashboard.putData("Move PID", moveController);
+		sd.putData("Move PID", moveController);
 	}
 
 	/**
@@ -78,6 +79,7 @@ public PIDMove(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 		this.target = dist;
 		this.dt = dt;
 		this.avg = avg;
+		this.sd = sd;
 		if (Robot.dt != null) {
 			requires(Robot.dt);
 		}
@@ -114,8 +116,8 @@ public void initialize() {
 	@Override
 	protected void execute() {
 		System.out.println("Enc Avg Dist: " + avg.pidGet());
-		SmartDashboard.putNumber("Move PID Result", moveController.get());
-		SmartDashboard.putNumber("Move PID Error", moveController.getError());
+		sd.putNumber("Move PID Result", moveController.get());
+		sd.putNumber("Move PID Error", moveController.getError());
 	}
 
 	/**
@@ -173,6 +175,6 @@ protected void interrupted() {
 	@Override
 	public void pidWrite(double output) {
 		dt.arcadeDrive(output, 0);
-		SmartDashboard.putNumber("Move PID Output", output);
+		sd.putNumber("Move PID Output", output);
 	}
 }

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

@@ -24,6 +24,7 @@ public class PIDTurn extends Command implements PIDOutput {
 	private PIDSource ahrs;
 	private Timer tim;
 	private double lastTime;
+	private SmartDashboardInterface sd;
 
 	/**
 	 * Constructs this command with a new PIDController. Sets all of the
@@ -49,6 +50,7 @@ public PIDTurn(double targ, DrivetrainInterface dt, SmartDashboardInterface sd,
 		// Use requires() here to declare subsystem dependencies
 		this.dt = dt;
 		this.ahrs = ahrs;
+		this.sd = sd;
 
 		if (absolute) {
 			target = targ - AutoUtils.position.getRot();
@@ -128,7 +130,7 @@ public PIDTurn(double[] point, DrivetrainInterface dt, SmartDashboardInterface s
 		turnController = new PIDController(sd.getConst("TurnkP", 1), sd.getConst("TurnkI", 0), sd.getConst("TurnkD", 0),
 				kf, ahrs, this);
 		// tim = new Timer();
-		SmartDashboard.putData("Turn PID", turnController);
+		sd.putData("Turn PID", turnController);
 	}
 
 	/**
@@ -203,8 +205,8 @@ protected boolean isFinished() {
 	protected void end() {
 		turnController.disable();
 		System.out.println("end");
-		SmartDashboard.putNumber("Turn PID Result", turnController.get());
-		SmartDashboard.putNumber("Turn PID Error", turnController.getError());
+		sd.putNumber("Turn PID Result", turnController.get());
+		sd.putNumber("Turn PID Error", turnController.getError());
 		// turnController.free();
 
 		AutoUtils.position.changeRot(dt.getAHRSAngle());