Merge in master

.gitignore

@@ -37,6 +37,7 @@ robot2017.iws
 #Other
 .metadata/
 .DS_Store
+bash.exe.stackdump
 
 #Copied log files
 logs/

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/Robot.java

@@ -11,24 +11,12 @@
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveTalonCluster;
-import org.usfirst.frc.team449.robot.drive.unidirectional.DriveTalonClusterShiftable;
-import org.usfirst.frc.team449.robot.generalInterfaces.shiftable.commands.SwitchToGear;
+import org.usfirst.frc.team449.robot.other.Clock;
 import org.usfirst.frc.team449.robot.other.Logger;
-import org.usfirst.frc.team449.robot.oi.unidirectional.OIUnidirectional;
-import org.usfirst.frc.team449.robot.subsystem.complex.climber.ClimberCurrentLimited;
-import org.usfirst.frc.team449.robot.subsystem.complex.intake.IntakeFixedAndActuated;
-import org.usfirst.frc.team449.robot.subsystem.complex.shooter.LoggingShooter;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.motionProfile.commands.RunLoadedProfile;
-import org.usfirst.frc.team449.robot.subsystem.interfaces.solenoid.SolenoidSimple;
-import org.usfirst.frc.team449.robot.subsystem.interfaces.solenoid.commands.SolenoidForward;
-import org.usfirst.frc.team449.robot.subsystem.interfaces.solenoid.commands.SolenoidReverse;
-import org.usfirst.frc.team449.robot.subsystem.singleImplementation.camera.CameraNetwork;
-import org.usfirst.frc.team449.robot.subsystem.singleImplementation.pneumatics.Pneumatics;
-import org.usfirst.frc.team449.robot.subsystem.singleImplementation.pneumatics.commands.StartCompressor;
 import org.yaml.snakeyaml.Yaml;
 
 import java.io.FileReader;
@@ -48,16 +36,6 @@ public class Robot extends IterativeRobot {
 	@NotNull
 	public static final String RESOURCES_PATH = "/home/lvuser/449_resources/";
 
-	/**
-	 * The current time in milliseconds as it was stored the last time a method in robot was run.
-	 */
-	private static long currentTimeMillis;
-
-	/**
-	 * The time robotInit started running.
-	 */
-	private static long startTime;
-
 	/**
 	 * The drive
 	 */
@@ -92,23 +70,13 @@ public class Robot extends IterativeRobot {
 	@Nullable
 	private Command autonomousCommand;
 
-	/**
-	 * Get the current time, in milliseconds, since startup.
-	 *
-	 * @return current time in milliseconds.
-	 */
-	@Contract(pure = true)
-	public static long currentTimeMillis() {
-		return currentTimeMillis - startTime;
-	}
-
 	/**
 	 * The method that runs when the robot is turned on. Initializes all subsystems from the map.
 	 */
 	public void robotInit() {
 		//Set up start time
-		currentTimeMillis = System.currentTimeMillis();
-		startTime = currentTimeMillis;
+		Clock.setStartTime();
+		Clock.updateTime();
 
 		//Yes this should be a print statement, it's useful to know that robotInit started.
 		System.out.println("Started robotInit.");
@@ -222,7 +190,7 @@ public void teleopInit() {
 	@Override
 	public void teleopPeriodic() {
 		//Refresh the current time.
-		currentTimeMillis = System.currentTimeMillis();
+		Clock.updateTime();
 		//Run all commands. This is a WPILib thing you don't really have to worry about.
 		Scheduler.getInstance().run();
 	}
@@ -253,7 +221,7 @@ public void autonomousInit() {
 	@Override
 	public void autonomousPeriodic() {
 		//Update the current time
-		currentTimeMillis = System.currentTimeMillis();
+		Clock.updateTime();
 		//Run all commands. This is a WPILib thing you don't really have to worry about.
 		Scheduler.getInstance().run();
 	}
@@ -295,7 +263,7 @@ private void sendModeOverI2C(I2C i2C, String mode) {
 	 */
 	private void doStartupTasks() {
 		//Refresh the current time.
-		currentTimeMillis = System.currentTimeMillis();
+		Clock.updateTime();
 
 		//Start running the logger
 		loggerNotifier.startPeriodic(robotMap.getLogger().getLoopTimeSecs());

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/RobotMap2017.java

@@ -9,9 +9,8 @@
 import org.usfirst.frc.team449.robot.jacksonWrappers.MappedDigitalInput;
 import org.usfirst.frc.team449.robot.jacksonWrappers.YamlCommand;
 import org.usfirst.frc.team449.robot.oi.OI;
-import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.oi.buttons.CommandButton;
-import org.usfirst.frc.team449.robot.oi.unidirectional.OIUnidirectional;
+import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.other.MotionProfileData;
 import org.usfirst.frc.team449.robot.subsystem.complex.climber.ClimberCurrentLimited;
 import org.usfirst.frc.team449.robot.subsystem.complex.intake.IntakeFixedAndActuated;

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/general/WaitForMillis.java

@@ -4,8 +4,8 @@
 import com.fasterxml.jackson.annotation.JsonIdentityInfo;
 import com.fasterxml.jackson.annotation.JsonProperty;
 import com.fasterxml.jackson.annotation.ObjectIdGenerators;
-import org.usfirst.frc.team449.robot.Robot;
 import org.usfirst.frc.team449.robot.jacksonWrappers.YamlCommandWrapper;
+import org.usfirst.frc.team449.robot.other.Clock;
 
 /**
  * A command that does nothing and finishes after a set number of milliseconds. For use to create a delay in sequential
@@ -39,7 +39,7 @@ public WaitForMillis(@JsonProperty(required = true) long time) {
 	 */
 	@Override
 	protected void initialize() {
-		startTime = Robot.currentTimeMillis();
+		startTime = Clock.currentTimeMillis();
 	}
 
 	/**
@@ -49,6 +49,6 @@ protected void initialize() {
 	 */
 	@Override
 	protected boolean isFinished() {
-		return Robot.currentTimeMillis() - startTime >= timeout;
+		return Clock.currentTimeMillis() - startTime >= timeout;
 	}
 }

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/FieldOrientedUnidirectionalDriveCommandShifting.java

@@ -6,6 +6,7 @@
 import org.usfirst.frc.team449.robot.components.AutoshiftComponent;
 import org.usfirst.frc.team449.robot.drive.shifting.DriveShiftable;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
+import org.usfirst.frc.team449.robot.generalInterfaces.shiftable.Shiftable;
 import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
 import org.usfirst.frc.team449.robot.oi.fieldoriented.OIFieldOriented;
 import org.usfirst.frc.team449.robot.other.Logger;
@@ -18,7 +19,7 @@
  */
 @JsonTypeInfo(use = JsonTypeInfo.Id.CLASS, include = JsonTypeInfo.As.WRAPPER_OBJECT, property = "@class")
 @JsonIdentityInfo(generator = ObjectIdGenerators.StringIdGenerator.class)
-public class FieldOrientedUnidirectionalDriveCommandShifting <T extends YamlSubsystem & DriveUnidirectional & SubsystemNavX & DriveShiftable>
+public class FieldOrientedUnidirectionalDriveCommandShifting<T extends YamlSubsystem & DriveUnidirectional & SubsystemNavX & DriveShiftable>
 		extends FieldOrientedUnidirectionalDriveCommand {
 
 	/**
@@ -33,25 +34,31 @@ public class FieldOrientedUnidirectionalDriveCommandShifting <T extends YamlSubs
 	@NotNull
 	private final AutoshiftComponent autoshiftComponent;
 
+	/**
+	 * The coefficient to multiply the loop output by in high gear. Defaults to 1.
+	 */
+	private final double highGearAngularCoefficient;
+
 	/**
 	 * Default constructor
 	 *
-	 * @param toleranceBuffer    How many consecutive loops have to be run while within tolerance to be considered on
-	 *                           target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0.
-	 * @param absoluteTolerance  The maximum number of degrees off from the target at which we can be considered within
-	 *                           tolerance.
-	 * @param minimumOutput      The minimum output of the loop. Defaults to zero.
-	 * @param maximumOutput      The maximum output of the loop. Can be null, and if it is, no maximum output is used.
-	 * @param deadband           The deadband around the setpoint, in degrees, within which no output is given to the
-	 *                           motors. Defaults to zero.
-	 * @param inverted           Whether the loop is inverted. Defaults to false.
-	 * @param kP                 Proportional gain. Defaults to zero.
-	 * @param kI                 Integral gain. Defaults to zero.
-	 * @param kD                 Derivative gain. Defaults to zero.
-	 * @param subsystem          The drive to execute this command on.
-	 * @param oi                 The OI controlling the robot.
-	 * @param snapPoints         The points to snap the PID controller input to.
-	 * @param autoshiftComponent The helper object for autoshifting.
+	 * @param toleranceBuffer            How many consecutive loops have to be run while within tolerance to be considered on
+	 *                                   target. Multiply by loop period of ~20 milliseconds for time. Defaults to 0.
+	 * @param absoluteTolerance          The maximum number of degrees off from the target at which we can be considered within
+	 *                                   tolerance.
+	 * @param minimumOutput              The minimum output of the loop. Defaults to zero.
+	 * @param maximumOutput              The maximum output of the loop. Can be null, and if it is, no maximum output is used.
+	 * @param deadband                   The deadband around the setpoint, in degrees, within which no output is given to the
+	 *                                   motors. Defaults to zero.
+	 * @param inverted                   Whether the loop is inverted. Defaults to false.
+	 * @param kP                         Proportional gain. Defaults to zero.
+	 * @param kI                         Integral gain. Defaults to zero.
+	 * @param kD                         Derivative gain. Defaults to zero.
+	 * @param subsystem                  The drive to execute this command on.
+	 * @param oi                         The OI controlling the robot.
+	 * @param snapPoints                 The points to snap the PID controller input to.
+	 * @param autoshiftComponent         The helper object for autoshifting.
+	 * @param highGearAngularCoefficient The coefficient to multiply the loop output by in high gear. Defaults to 1.
 	 */
 	@JsonCreator
 	public FieldOrientedUnidirectionalDriveCommandShifting(@JsonProperty(required = true) double absoluteTolerance,
@@ -65,11 +72,13 @@ public FieldOrientedUnidirectionalDriveCommandShifting(@JsonProperty(required =
 	                                                       @NotNull @JsonProperty(required = true) T subsystem,
 	                                                       @NotNull @JsonProperty(required = true) OIFieldOriented oi,
 	                                                       @Nullable List<AngularSnapPoint> snapPoints,
-	                                                       @NotNull @JsonProperty(required = true) AutoshiftComponent autoshiftComponent) {
+	                                                       @NotNull @JsonProperty(required = true) AutoshiftComponent autoshiftComponent,
+	                                                       @Nullable Double highGearAngularCoefficient) {
 		//Assign stuff
 		super(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, inverted, kP, kI, kD, subsystem, oi, snapPoints);
 		this.subsystem = subsystem;
 		this.autoshiftComponent = autoshiftComponent;
+		this.highGearAngularCoefficient = highGearAngularCoefficient != null ? highGearAngularCoefficient : 1;
 	}
 
 	/**
@@ -83,6 +92,16 @@ protected void execute() {
 		super.execute();
 	}
 
+	/**
+	 * Give the correct output to the motors based on the PID output and velocity input.
+	 *
+	 * @param output The output of the angular PID loop.
+	 */
+	@Override
+	protected void usePIDOutput(double output) {
+		super.usePIDOutput(output * (subsystem.getGear() == Shiftable.gear.HIGH.getNumVal() ? highGearAngularCoefficient : 1));
+	}
+
 	/**
 	 * Log when this command ends
 	 */

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/NavXDriveStraight.java

@@ -8,8 +8,8 @@
 import org.jetbrains.annotations.Nullable;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
 import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
-import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.oi.unidirectional.tank.OITank;
+import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.commands.PIDAngleCommand;
 

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/NavXTurnToAngle.java

@@ -7,9 +7,9 @@
 import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
-import org.usfirst.frc.team449.robot.Robot;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
 import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
+import org.usfirst.frc.team449.robot.other.Clock;
 import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.commands.PIDAngleCommand;
@@ -112,7 +112,7 @@ protected void usePIDOutput(double output) {
 	@Override
 	protected void initialize() {
 		//Set up start time
-		this.startTime = Robot.currentTimeMillis();
+		this.startTime = Clock.currentTimeMillis();
 		this.setSetpoint(setpoint);
 		//Make sure to enable the controller!
 		this.getPIDController().enable();
@@ -126,7 +126,7 @@ protected void initialize() {
 	@Override
 	protected boolean isFinished() {
 		//The PIDController onTarget() is crap and sometimes never returns true because of floating point errors, so we need a timeout
-		return this.getPIDController().onTarget() || Robot.currentTimeMillis() - startTime > timeout;
+		return this.getPIDController().onTarget() || Clock.currentTimeMillis() - startTime > timeout;
 	}
 
 	/**

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/NavXTurnToAngleRelative.java

@@ -6,9 +6,9 @@
 import com.fasterxml.jackson.annotation.ObjectIdGenerators;
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
-import org.usfirst.frc.team449.robot.Robot;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
 import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
+import org.usfirst.frc.team449.robot.other.Clock;
 import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
 
@@ -59,7 +59,7 @@ public NavXTurnToAngleRelative(@JsonProperty(required = true) double absoluteTol
 	@Override
 	protected void initialize() {
 		//Setup start time
-		this.startTime = Robot.currentTimeMillis();
+		this.startTime = Clock.currentTimeMillis();
 		Logger.addEvent("NavXRelativeTurnToAngle init.", this.getClass());
 		//Do math to setup the setpoint.
 		this.setSetpoint(clipTo180(((SubsystemNavX) subsystem).getGyroOutput() + setpoint));

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/UnidirectionalNavXDefaultDrive.java

@@ -5,9 +5,9 @@
 import org.jetbrains.annotations.Nullable;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
 import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
-import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.oi.unidirectional.OIUnidirectional;
 import org.usfirst.frc.team449.robot.other.BufferTimer;
+import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.commands.PIDAngleCommand;
 
@@ -175,6 +175,11 @@ protected void usePIDOutput(double output) {
 			//Process the output (minimumOutput, deadband, etc.)
 			output = processPIDOutput(output);
 
+			//Deadband if we're stationary
+			if(oi.getLeftOutput() == 0 || oi.getRightOutput() == 0){
+				output=deadbandOutput(output);
+			}
+
 			//Adjust the heading according to the PID output, it'll be positive if we want to go right.
 			subsystem.setOutput(oi.getLeftOutput() - output, oi.getRightOutput() + output);
 		}

RoboRIO/src/main/java/org/usfirst/frc/team449/robot/commands/multiInterface/drive/UnidirectionalNavXShiftingDefaultDrive.java

@@ -6,10 +6,11 @@
 import org.usfirst.frc.team449.robot.components.AutoshiftComponent;
 import org.usfirst.frc.team449.robot.drive.shifting.DriveShiftable;
 import org.usfirst.frc.team449.robot.drive.unidirectional.DriveUnidirectional;
+import org.usfirst.frc.team449.robot.generalInterfaces.shiftable.Shiftable;
 import org.usfirst.frc.team449.robot.jacksonWrappers.YamlSubsystem;
-import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.oi.unidirectional.OIUnidirectional;
 import org.usfirst.frc.team449.robot.other.BufferTimer;
+import org.usfirst.frc.team449.robot.other.Logger;
 import org.usfirst.frc.team449.robot.subsystem.interfaces.navX.SubsystemNavX;
 
 /**
@@ -32,6 +33,11 @@ public class UnidirectionalNavXShiftingDefaultDrive<T extends YamlSubsystem & Dr
 	@NotNull
 	protected final AutoshiftComponent autoshiftComponent;
 
+	/**
+	 * The coefficient to multiply the loop output by in high gear.
+	 */
+	private final double highGearAngularCoefficient;
+
 	/**
 	 * Default constructor
 	 *
@@ -55,6 +61,7 @@ public class UnidirectionalNavXShiftingDefaultDrive<T extends YamlSubsystem & Dr
 	 * @param subsystem                   The drive to execute this command on.
 	 * @param oi                          The OI controlling the robot.
 	 * @param autoshiftComponent          The helper object for autoshifting.
+	 * @param highGearAngularCoefficient The coefficient to multiply the loop output by in high gear. Defaults to 1.
 	 */
 	@JsonCreator
 	public UnidirectionalNavXShiftingDefaultDrive(@JsonProperty(required = true) double absoluteTolerance,
@@ -69,11 +76,13 @@ public UnidirectionalNavXShiftingDefaultDrive(@JsonProperty(required = true) dou
 	                                              @NotNull @JsonProperty(required = true) BufferTimer driveStraightLoopEntryTimer,
 	                                              @NotNull @JsonProperty(required = true) T subsystem,
 	                                              @NotNull @JsonProperty(required = true) OIUnidirectional oi,
-	                                              @NotNull @JsonProperty(required = true) AutoshiftComponent autoshiftComponent) {
+	                                              @NotNull @JsonProperty(required = true) AutoshiftComponent autoshiftComponent,
+	                                              @Nullable Double highGearAngularCoefficient) {
 		super(absoluteTolerance, toleranceBuffer, minimumOutput, maximumOutput, deadband, maxAngularVelToEnterLoop,
 				inverted, kP, kI, kD, driveStraightLoopEntryTimer, subsystem, oi);
 		this.autoshiftComponent = autoshiftComponent;
 		this.subsystem = subsystem;
+		this.highGearAngularCoefficient = highGearAngularCoefficient != null ? highGearAngularCoefficient : 1;
 	}
 
 	/**
@@ -105,4 +114,14 @@ protected void interrupted() {
 		Logger.addEvent("ShiftingUnidirectionalNavXArcadeDrive Interrupted! Stopping the robot.", this.getClass());
 		subsystem.fullStop();
 	}
+
+	/**
+	 * Give the correct output to the motors based on whether we're in free drive or drive straight.
+	 *
+	 * @param output The output of the angular PID loop.
+	 */
+	@Override
+	protected void usePIDOutput(double output) {
+		super.usePIDOutput(output * (subsystem.getGear() == Shiftable.gear.HIGH.getNumVal() ? highGearAngularCoefficient : 1));
+	}
 }