diff options
Diffstat (limited to 'src/org/usfirst/frc/team4272/robotlib')
9 files changed, 626 insertions, 0 deletions
diff --git a/src/org/usfirst/frc/team4272/robotlib/CommandBase.java b/src/org/usfirst/frc/team4272/robotlib/CommandBase.java new file mode 100644 index 0000000..0519a2a --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/CommandBase.java @@ -0,0 +1,73 @@ +/** + * 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 new file mode 100644 index 0000000..306a09e --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/CommandRobot.java @@ -0,0 +1,65 @@ +/** + * 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 new file mode 100644 index 0000000..00b2d14 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/FeedForward.java @@ -0,0 +1,76 @@ +/** + * 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.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 new file mode 100644 index 0000000..3a867bb --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/PIDController.java @@ -0,0 +1,114 @@ +/** + * Copyright (c) 2011, 2012, 2015 Luke Shumaker + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the FIRST nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * @author Luke Shumaker <lukeshu@sbcglobal.net> + */ +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.SpeedController; +//import edu.wpi.first.wpilibj.PIDController; + +/** + * TODO: Write JavaDocs + * @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; + public final PIDSource source; + public final PIDOutput output; + public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) { + super(Kp, Ki, Kd, source, output); + this.source = source; + this.output = output; + } + public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) { + super(Kp, Ki, Kd, source, output, period); + this.source = source; + this.output = output; + } + + public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, boolean autodisable) { + super(Kp, Ki, Kd, source, output); + this.autodisable = autodisable; + enabled = this.isEnabled(); + this.source = source; + this.output = output; + } + 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; + enabled = this.isEnabled(); + this.source = source; + this.output = output; + } + + public void setSetpoint(double output) { + if ((output == 0) && autodisable) { + disable(); + enabled = false; + } else { + if (!enabled) { + enable(); + enabled = true; + } + super.setSetpoint(output); + } + } + + public void pidWrite(double output) { + setSetpoint(output); + } + + public void set(double output) { + setSetpoint(output); + } + + /** + * Don't use this; it is leaking up from CANJaguar + * @param output + * @param syncGroup + */ + public void set(double output, byte syncGroup) { + setSetpoint(output); + } + + public void setInverted(boolean isInverted) { + // TODO Auto-generated method stub + + } + + public boolean getInverted() { + // TODO Auto-generated method stub + return false; + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/PIDOutputSplitter.java b/src/org/usfirst/frc/team4272/robotlib/PIDOutputSplitter.java new file mode 100644 index 0000000..c750659 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/PIDOutputSplitter.java @@ -0,0 +1,80 @@ +/** + * Copyright (c) 2011, 2012, 2015 Luke Shumaker + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the FIRST nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * @author Luke Shumaker <lukeshu@sbcglobal.net> + */ + +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.PIDOutput; + +/** + * TODO: Write JavaDocs + * @author Luke Shumaker <lukeshu@sbcglobal.net> + */ +public class PIDOutputSplitter implements PIDOutput { + private final PIDOutput[] outputs; + private final double[] scalars; + + public PIDOutputSplitter(PIDOutput[] outputs, double[] scalars) { + this.outputs = outputs; + this.scalars = scalars; + if (outputs.length != scalars.length) { + throw new RuntimeException("outputs and scalars must be same len"); + } + } + + public PIDOutputSplitter(PIDOutput... outputs) { + double[] s = new double[outputs.length]; + for (int i=0; i<s.length; i++) { + s[i]=1; + } + this.outputs = outputs; + this.scalars = s; + } + + public void pidWrite(double output) { + for(int i=0; i<outputs.length; i++) { + try { + outputs[i].pidWrite(output*scalars[i]); + } catch (Exception e) { + error(e); + } + } + } + + /** + * Handles exceptions from pidWrite(). + * + * By default it writes to System.out; you may extend+override this to + * change the behavior. + * + * @param e Exception + */ + public void error(Exception e) { + e.printStackTrace(); + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/PIDServo.java b/src/org/usfirst/frc/team4272/robotlib/PIDServo.java new file mode 100644 index 0000000..2a19a51 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/PIDServo.java @@ -0,0 +1,36 @@ +/** + * 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.Servo; +import edu.wpi.first.wpilibj.PIDOutput; + +/** + * TODO: Write JavaDocs + * @author Luke Shumaker <lukeshu@sbcglobal.net> + */ +public class PIDServo extends Servo implements PIDOutput { + public PIDServo(int channel) { + super(channel); + } + + public void pidWrite(double degrees) { + setAngle(getAngle()+degrees); + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/RateLimitedPIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/RateLimitedPIDOutput.java new file mode 100644 index 0000000..e54ae27 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/RateLimitedPIDOutput.java @@ -0,0 +1,50 @@ +/** + * 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.PIDOutput; +import edu.wpi.first.wpilibj.Timer; + +/** + * + * @author Luke Shumaker <lukeshu@sbcglobal.net> + */ +public class RateLimitedPIDOutput implements PIDOutput { + PIDOutput o; + double m; + double prev_rate = 0; + double prev_time = 0; + + public RateLimitedPIDOutput(PIDOutput out, double maxChange) { + o = out; + m = maxChange; + } + + public void pidWrite(double rate) { + double time = Timer.getFPGATimestamp(); + double dTime = time-prev_time; + double dRate = rate-prev_rate; + if (Math.abs(dRate/dTime)>m) { + int sign = (dRate<0?-1:1); + rate = prev_rate+(m*dTime*sign); + } + o.pidWrite(rate); + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/RollingAvg.java b/src/org/usfirst/frc/team4272/robotlib/RollingAvg.java new file mode 100644 index 0000000..665e7bf --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/RollingAvg.java @@ -0,0 +1,90 @@ +/** + * Copyright (c) 2011 Luke Shumaker + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the FIRST nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR + * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * @author Luke Shumaker <lukeshu@sbcglobal.net> + */ + +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSourceType; +import edu.wpi.first.wpilibj.PIDOutput; + +/** + * TODO: Write JavaDocs + * @author Luke Shumaker <lukeshu@sbcglobal.net> + */ +public class RollingAvg implements PIDSource, PIDOutput { + private PIDSource source = null; + private double[] points; + private double avg; + private int i; + + public RollingAvg(int len) { + points = new double[len]; + i = 0; + avg = 0; + } + public RollingAvg(int len, PIDSource src) { + this(len); + source = src; + } + + public double push(double v) { + avg -= points[i]; + points[i] = v/points.length; + avg += points[i]; + i++; i %= points.length; + return avg; + } + + public double get() { + return avg; + } + + public double pidGet() { + if (source!=null) + return push(source.pidGet()); + else + return get(); + } + + public void pidWrite(double output) { + push(output); + } + + public void setPIDSourceType(PIDSourceType srcType) { + if (source!=null) + source.setPIDSourceType(srcType); + } + + public PIDSourceType getPIDSourceType() { + if (source!=null) + return source.getPIDSourceType(); + else + return null; + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/SendablePIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/SendablePIDOutput.java new file mode 100644 index 0000000..a6886fa --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/SendablePIDOutput.java @@ -0,0 +1,42 @@ +/** + * Copyright (c) 2012, 2015 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.PIDOutput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * + * @author Luke Shumaker <lukeshu@sbcglobal.net> + */ +public class SendablePIDOutput implements PIDOutput { + PIDOutput o; + String n; + + public SendablePIDOutput(String name, PIDOutput out) { + n = name; + o = out; + } + + public void pidWrite(double val) { + o.pidWrite(val); + SmartDashboard.putNumber(n, val); + } +} |