diff options
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; } } |