summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/CommandBase.java73
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/CommandRobot.java65
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/FeedForward.java77
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/PIDController.java153
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;
}
}