Formatted commands and added infra to test

Author: dfang314

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

@@ -7,7 +7,13 @@
 
 package org.usfirst.frc.team199.Robot2018;
 
-import org.usfirst.frc.team199.Robot2018.commands.*;
+import org.usfirst.frc.team199.Robot2018.commands.PIDMove;
+import org.usfirst.frc.team199.Robot2018.commands.PIDTurn;
+import org.usfirst.frc.team199.Robot2018.commands.SetDistancePerPulse;
+import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
+import org.usfirst.frc.team199.Robot2018.commands.ShiftHighGear;
+import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
+import org.usfirst.frc.team199.Robot2018.commands.UpdatePIDConstants;
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.JoystickButton;
@@ -73,16 +79,16 @@ public OI() {
 		shiftDriveType = new JoystickButton(leftJoy, getButton("Shift Drive Type", 2));
 		shiftDriveType.whenPressed(new ShiftDriveType());
 		PIDMove = new JoystickButton(leftJoy, getButton("PID Move", 7));
-		PIDMove.whenPressed(new PIDMove(40));
+		PIDMove.whenPressed(new PIDMove(40), Robot.dt);
 		PIDTurn = new JoystickButton(leftJoy, getButton("PID Turn", 8));
-		PIDTurn.whenPressed(new PIDTurn(30));
-		
+		PIDTurn.whenPressed(new PIDTurn(30), Robot.dt);
+
 		rightJoy = new Joystick(1);
 		updatePidConstants = new JoystickButton(rightJoy, getButton("Get PID Constants", 8));
 		updatePidConstants.whenPressed(new UpdatePIDConstants());
 		updateEncoderDPP = new JoystickButton(rightJoy, getButton("Get Encoder Dist Per Pulse", 9));
 		updateEncoderDPP.whenPressed(new SetDistancePerPulse());
-		
+
 		manipulator = new Joystick(2);
 	}
 }

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

@@ -1,6 +1,7 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
 import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
 
 import edu.wpi.first.wpilibj.command.Command;
 
@@ -10,35 +11,37 @@
 public class PIDMove extends Command {
 
 	double target;
+	DrivetrainInterface dt;
 
-	public PIDMove(double targ) {
+	public PIDMove(double targ, DrivetrainInterface dt) {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
 		target = targ;
+		this.dt = dt;
 		requires(Robot.dt);
 	}
 
 	// Called just before this Command runs the first time
 	protected void initialize() {
-		Robot.dt.resetEnc();
-		Robot.dt.enableMovePid();
-		Robot.dt.setMoveSetpoint(target);
+		dt.resetEnc();
+		dt.enableMovePid();
+		dt.setMoveSetpoint(target);
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	protected void execute() {
-		Robot.dt.arcadeDrive(Robot.dt.getPidOut(), 0);
+		dt.arcadeDrive(dt.getPidOut(), 0);
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
 	protected boolean isFinished() {
-		return Robot.dt.onDriveTarg();
+		return dt.onDriveTarg();
 	}
 
 	// Called once after isFinished returns true
 	protected void end() {
-		Robot.dt.disableMovePid();
-		Robot.dt.stopDrive();
+		dt.disableMovePid();
+		dt.stopDrive();
 	}
 
 	// Called when another command which requires one or more of the same

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

@@ -1,6 +1,7 @@
 package org.usfirst.frc.team199.Robot2018.commands;
 
 import org.usfirst.frc.team199.Robot2018.Robot;
+import org.usfirst.frc.team199.Robot2018.subsystems.DrivetrainInterface;
 
 import edu.wpi.first.wpilibj.command.Command;
 
@@ -10,35 +11,37 @@
 public class PIDTurn extends Command {
 
 	double target;
+	DrivetrainInterface dt;
 
-	public PIDTurn(double targ) {
+	public PIDTurn(double targ, DrivetrainInterface dt) {
 		// Use requires() here to declare subsystem dependencies
 		// eg. requires(chassis);
 		target = targ;
+		this.dt = dt;
 		requires(Robot.dt);
 	}
 
 	// Called just before this Command runs the first time
 	protected void initialize() {
-		Robot.dt.resetAHRS();
-		Robot.dt.setTurnSetpoint(target);
-		Robot.dt.enableTurnPid();
+		dt.resetAHRS();
+		dt.setTurnSetpoint(target);
+		dt.enableTurnPid();
 	}
 
 	// Called repeatedly when this Command is scheduled to run
 	protected void execute() {
-		Robot.dt.arcadeDrive(0, Robot.dt.getPidOut());
+		dt.arcadeDrive(0, dt.getPidOut());
 	}
 
 	// Make this return true when this Command no longer needs to run execute()
 	protected boolean isFinished() {
-		return Robot.dt.onTurnTarg();
+		return dt.onTurnTarg();
 	}
 
 	// Called once after isFinished returns true
 	protected void end() {
-		Robot.dt.disableTurnPid();
-		Robot.dt.stopDrive();
+		dt.disableTurnPid();
+		dt.stopDrive();
 	}
 
 	// Called when another command which requires one or more of the same

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

@@ -11,36 +11,37 @@
 public class SetDistancePerPulse extends Command {
 
 	Timer tim;
-    public SetDistancePerPulse() {
-        // Use requires() here to declare subsystem dependencies
-        // eg. requires(chassis);
-    }
-
-    // Called just before this Command runs the first time
-    protected void initialize() {
-    	tim = new Timer();
-    	tim.reset();
-    	tim.start();
-    }
-
-    // Called repeatedly when this Command is scheduled to run
-    protected void execute() {
-    	Robot.dt.setDistancePerPulseLeft(Robot.getConst("Distance Per Pulse Left", 0.184));
-    	Robot.dt.setDistancePerPulseRight(Robot.getConst("Distance Per Pulse Right", 0.184));
-    }
-
-    // Make this return true when this Command no longer needs to run execute()
-    protected boolean isFinished() {
-        return tim.get() > Robot.getConst("Update Constants Time", 0.1);
-    }
-
-    // Called once after isFinished returns true
-    protected void end() {
-    }
-
-    // Called when another command which requires one or more of the same
-    // subsystems is scheduled to run
-    protected void interrupted() {
-    	end();
-    }
+
+	public SetDistancePerPulse() {
+		// Use requires() here to declare subsystem dependencies
+		// eg. requires(chassis);
+	}
+
+	// Called just before this Command runs the first time
+	protected void initialize() {
+		tim = new Timer();
+		tim.reset();
+		tim.start();
+	}
+
+	// Called repeatedly when this Command is scheduled to run
+	protected void execute() {
+		Robot.dt.setDistancePerPulseLeft(Robot.getConst("Distance Per Pulse Left", 0.184));
+		Robot.dt.setDistancePerPulseRight(Robot.getConst("Distance Per Pulse Right", 0.184));
+	}
+
+	// Make this return true when this Command no longer needs to run execute()
+	protected boolean isFinished() {
+		return tim.get() > Robot.getConst("Update Constants Time", 0.1);
+	}
+
+	// Called once after isFinished returns true
+	protected void end() {
+	}
+
+	// Called when another command which requires one or more of the same
+	// subsystems is scheduled to run
+	protected void interrupted() {
+		end();
+	}
 }

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

@@ -24,7 +24,7 @@
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 
-public class Drivetrain extends Subsystem implements PIDOutput, PIDSource {
+public class Drivetrain extends Subsystem implements PIDOutput, PIDSource, DrivetrainInterface {
 	// Put methods for controlling this subsystem
 	// here. Call these from Commands.