code cleanup

4 files changed, 118 insertions, 250 deletions

diff --git a/src/org/usfirst/frc/team4272/robotlib/CommandBase.java b/src/org/usfirst/frc/team4272/robotlib/CommandBase.java
deleted file mode 100644
index 0519a2a..0000000
--- a/src/org/usfirst/frc/team4272/robotlib/CommandBase.java
+++ /dev/null
@@ -1,73 +0,0 @@
-/**
- * CommandBase - defines robot hardware; superclass of all commands.
- *
- * Copyright (c) FIRST 2008. All Rights Reserved.
- * Open Source Software - may be modified and shared by FRC teams. The code
- * must be accompanied by the FIRST BSD license file in the root directory of
- * the project.
- *
- * Copyright (c) 2012 Precise Path Robotics, Inc
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Luke Shumaker <lukeshu@sbcglobal.net>
- */
-package org.usfirst.frc.team4272.robotlib;
-
-import edu.wpi.first.wpilibj.CANJaguar;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * The base for all commands, also the class defining robot hardware/config.
- * All atomic commands should subclass CommandBase. CommandBase stores creates
- * and stores each control system.
- *
- * @author Luke Shumaker <lukeshu@sbcglobal.net>
- */
-public abstract class CommandBase extends Command {
-
-	private boolean has_subsystems = true;
-	protected synchronized void requires(Subsystem subsystem) {
-		if (subsystem == null) {
-			has_subsystems = false;
-		}
-		super.requires(subsystem);
-	}
-
-	public synchronized void start() {
-		if (has_subsystems) { super.start(); }
-	}
-
-	/**
-	 * Instantiate a CANJaguar, handling exceptions.
-	 *
-	 * @param id CANJaguar ID to use.
-	 * @return the CANJaguar
-	 */
-	public static CANJaguar getJaguar(int id) {
-		CANJaguar ret = null;
-		try {
-			ret = new CANJaguar(id);
-		} catch(Exception e) {
-			e.printStackTrace();
-		}
-		return ret;
-	}
-
-	public CommandBase() { super(); }
-	public CommandBase(double timeout) { super(timeout); }
-	public CommandBase(String name) { super(name); }
-	public CommandBase(String name, double timeout) { super(name, timeout); }
-}
diff --git a/src/org/usfirst/frc/team4272/robotlib/CommandRobot.java b/src/org/usfirst/frc/team4272/robotlib/CommandRobot.java
deleted file mode 100644
index 306a09e..0000000
--- a/src/org/usfirst/frc/team4272/robotlib/CommandRobot.java
+++ /dev/null
@@ -1,65 +0,0 @@
-/**
- * CommandFramework base class for FRC 1024.
- *
- * Copyright (c) 2012 Precise Path Robotics, Inc
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Luke Shumaker <lukeshu@sbcglobal.net>
- */
-
-package org.usfirst.frc.team4272.robotlib;
-
-import edu.wpi.first.wpilibj.IterativeRobot;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Scheduler;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the IterativeRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the manifest file in the resource
- * directory.
- *
- * @author Luke Shumaker <lukeshu@sbcglobal.net>
- */
-public abstract class CommandRobot extends IterativeRobot {
-
-	protected Command autonomousCommand;
-	protected Command teleopCommand;
-
-	public abstract void robotInit();
-
-	public void autonomousInit() {
-		autonomousCommand.start();
-	}
-
-	public void autonomousPeriodic() {
-		Scheduler.getInstance().run();
-	}
-
-	public void teleopInit() {
-		autonomousCommand.cancel();
-		teleopCommand.start();
-	}
-
-	public void teleopPeriodic() {
-		Scheduler.getInstance().run();
-	}
-
-	public void disabledInit() {
-		autonomousCommand.cancel();
-		teleopCommand.cancel();
-	}
-}
diff --git a/src/org/usfirst/frc/team4272/robotlib/FeedForward.java b/src/org/usfirst/frc/team4272/robotlib/FeedForward.java
deleted file mode 100644
index a387b5e..0000000
--- a/src/org/usfirst/frc/team4272/robotlib/FeedForward.java
+++ /dev/null
@@ -1,77 +0,0 @@
-/**
- * Copyright (c) 2012 Precise Path Robotics, Inc
- * Copyright (c) 2016 Luke Shumaker
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Luke Shumaker <lukeshu@sbcglobal.net>
- */
-package org.usfirst.frc.team4272.robotlib;
-
-import edu.wpi.first.wpilibj.PIDOutput;
-import edu.wpi.first.wpilibj.PIDSource;
-
-/**
- *
- * @author Luke Shumaker <lukeshu@sbcglobal.net>
- */
-public abstract class FeedForward implements PIDOutput {
-	private class PIDTrigger implements PIDOutput {
-		private FeedForward ff;
-		public PIDTrigger(FeedForward ff) {
-			this.ff = ff;
-		}
-		public void pidWrite(double output) {
-			ff.update(output);
-		}
-	}
-	private PIDController pid;
-	private PIDOutput pidOutput;
-	public FeedForward(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
-		pidOutput = output;
-		pid = new PIDController(Kp, Ki, Kd, source, new PIDTrigger(this));
-	}
-	public FeedForward(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
-		pidOutput = output;
-		pid = new PIDController(Kp, Ki, Kd, source, new PIDTrigger(this), period);
-	}
-
-	public FeedForward(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, boolean autodisable) {
-		pidOutput = output;
-		pid = new PIDController(Kp, Ki, Kd, source, new PIDTrigger(this), autodisable);
-	}
-	public FeedForward(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period, boolean autodisable) {
-		pidOutput = output;
-		pid = new PIDController(Kp, Ki, Kd, source, new PIDTrigger(this), period, autodisable);
-	}
-
-	public PIDController getPID() {
-		return pid;
-	}
-
-	public void pidWrite(double output) {
-		pid.setSetpoint(output);
-	}
-
-	public void update(double pidResult) {
-		if (pid.isEnabled()) {
-			double ffResult = calculate(pid.getSetpoint());
-			pidOutput.pidWrite(ffResult+pidResult);
-		} else {
-			pidOutput.pidWrite(0);
-		}
-	}
-
-	public abstract double calculate(double setpoint);
-}
diff --git a/src/org/usfirst/frc/team4272/robotlib/PIDController.java b/src/org/usfirst/frc/team4272/robotlib/PIDController.java
index 6650ad4..14c26ce 100644
--- a/src/org/usfirst/frc/team4272/robotlib/PIDController.java
+++ b/src/org/usfirst/frc/team4272/robotlib/PIDController.java
@@ -1,4 +1,9 @@
 /**
+ * Copyright (c) FIRST 2008-2012. All Rights Reserved.
+ * Open Source Software - may be modified and shared by FRC teams. The code
+ * must be accompanied by the FIRST BSD license file in the root directory of
+ * the project.
+ *
  * Copyright (c) 2011-2012, 2015-2016 Luke Shumaker
  * All rights reserved.
  *
@@ -34,7 +39,13 @@ import edu.wpi.first.wpilibj.SpeedController;
 //import edu.wpi.first.wpilibj.PIDController;
 
 /**
- * A variant of edu.wpi.first.wpilibj.PIDController that also:
+ * An enhanced variant of {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController}
+ * (implements a PID Control Loop).
+ *
+ * It is enhanced from
+ * {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController}
+ * in that it that also:
+ *
  * <ul>
  *  <li> implements SpeedController (and therefore PIDOutput)
  *  <li> has auto-enable/disable functionality to avoid "locking" the
@@ -43,77 +54,149 @@ import edu.wpi.first.wpilibj.SpeedController;
  * @author Luke Shumaker <lukeshu@sbcglobal.net>
  */
 public class PIDController extends edu.wpi.first.wpilibj.PIDController implements SpeedController {
-	private boolean autodisable = false;
-	/**
-	 * Default to true, so if we don't use autodisable, we don't enable when we
-	 * shouldn't, as we will think it's already enabled.
-	 */
-	private boolean enabled = true;
+	private boolean NaNDisabled = false;
+	private boolean inverted = false;
 	public final PIDSource source;
 	public final PIDOutput output;
 
-	/* Constructors *******************************************************/
-
-	public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
-		this(Kp, Ki, Kd, source, output, kDefaultPeriod, false);
+	/**
+	 * Allocate a PID object with the given constants for P, I, D, and F
+	 *$
+	 * @param Kp the proportional coefficient
+	 * @param Ki the integral coefficient
+	 * @param Kd the derivative coefficient
+	 * @param Kf the feed forward term
+	 * @param source The PIDSource object that is used to get values
+	 * @param output The PIDOutput object that is set to the output percentage
+	 * @param period the loop time for doing calculations. This particularly
+	 *        effects calculations of the integral and differential terms. The
+	 *        default is 50ms.
+	 */
+	public PIDController(double Kp, double Ki, double Kd, double Kf,
+	                     PIDSource source, PIDOutput output,
+	                     double period) {
+		super(Kp, Ki, Kd, Kf, source, output, period);
+		this.source = source;
+		this.output = output;
 	}
-	public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
-		this(Kp, Ki, Kd, source, output, period, false);
+
+	/* Convenience constructors *******************************************/
+	/* `Kf`, and `period` are optional */
+
+	/**
+	 * Convenience constructor with {@code Kf} defaulting to
+	 * {@code 0.0}.
+	 */
+	public PIDController(double Kp, double Ki, double Kd,
+	                     PIDSource source, PIDOutput output,
+	                     double period) {
+		this(Kp, Ki, Kd, 0.0, source, output, period);
 	}
-	public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, boolean autodisable) {
-		this(Kp, Ki, Kd, source, output, kDefaultPeriod, autodisable);
+
+	/**
+	 * Convenience constructor with {@code period} defaulting to
+	 * {@link #kDefaultPeriod}.
+	 */
+	public PIDController(double Kp, double Ki, double Kd, double Kf,
+	                     PIDSource source, PIDOutput output) {
+		this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
 	}
-	public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period, boolean autodisable) {
-		super(Kp, Ki, Kd, source, output, period);
-		this.autodisable = autodisable;
-		this.enabled = this.isEnabled();
-		this.source = source;
-		this.output = output;
+
+	/**
+	 * Convenience constructor with {@code Kf} defaulting to
+	 * {@code 0.0}, and {@code period} defaulting to
+	 * {@link #kDefaultPeriod}.
+	 */
+	public PIDController(double Kp, double Ki, double Kd,
+	                     PIDSource source, PIDOutput output) {
+		this(Kp, Ki, Kd, 0.0, source, output, kDefaultPeriod);
 	}
 
-	/* Mimic the four PIDController constructors **************************/
+	/* Override to auto-disable if setpoint is NaN, and invert ************/
 
-	public synchronized void setSetpoint(double output) {
-		if ((output == 0) && autodisable) {
-			disable();
-			enabled = false;
+	/**
+	 * {@inheritDoc}
+	 */
+	@Override
+	public synchronized void setSetpoint(double setpoint) {
+		if (Double.isNaN(setpoint)) {
+			NaNDisabled = true;
+			super.disable();
 		} else {
-			if (!enabled) {
+			if (NaNDisabled && !isEnabled()) {
 				enable();
-				enabled = true;
 			}
-			super.setSetpoint(output);
+			super.setSetpoint((inverted ? -1 : 1) * setpoint);
 		}
 	}
 
+	/**
+	 * {@inheritDoc}
+	 */
+	@Override
+	public synchronized void enable() {
+		NaNDisabled = false;
+		super.enable();
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	@Override
+	public synchronized void disable() {
+		NaNDisabled = false;
+		super.disable();
+	}
+
 	/* Implement PIDOutput (a parent of SpeedController) *******************/
 
+	/**
+	 * {@inheritDoc}
+	 */
+	@Override
 	public void pidWrite(double output) {
 		setSetpoint(output);
 	}
 
 	/* Implement SpeedController *******************************************/
 
+	/**
+	 * {@inheritDoc}
+	 */
+	@Override
 	public void set(double output) {
 		setSetpoint(output);
 	}
 
 	/**
-	 * Don't use this; it is leaking up from CANJaguar
+	 * Don't use this; it is leaking up from
+	 * {@link edu.wpi.first.wpilibj.CANJaguar CANJaguar} through
+	 * {@link edu.wpi.first.wpilibj.SpeedController SpeedController}
+	 *
 	 * @param output
 	 * @param syncGroup
+	 *
+	 * @deprecated Don't use this, it is leaking up from
+	 *   {@link edu.wpi.first.wpilibj.CANJaguar CANJaguar}
 	 */
+	@Override
 	public void set(double output, byte syncGroup) {
-		setSetpoint(output);
+		set(output);
 	}
 
+	/**
+	 * {@inheritDoc}
+	 */
+	@Override
 	public void setInverted(boolean isInverted) {
-		// TODO Auto-generated method stub
-
+		inverted = isInverted;
 	}
 
+	/**
+	 * {@inheritDoc}
+	 */
+	@Override
 	public boolean getInverted() {
-		// TODO Auto-generated method stub
-		return false;
+		return inverted;
 	}
 }