From 7fad9de275f72354deb1a86ee0e8312a420bf783 Mon Sep 17 00:00:00 2001 From: Luke Shumaker Date: Sat, 16 Jan 2016 15:14:37 -0500 Subject: mv src/org/mkenzierobotics/lib/robot/* src/org/usfirst/frc/team4272/robotlib --- .../mckenzierobotics/lib/robot/CommandBase.java | 73 ------------- .../mckenzierobotics/lib/robot/CommandRobot.java | 65 ------------ .../mckenzierobotics/lib/robot/FeedForward.java | 76 -------------- .../mckenzierobotics/lib/robot/PIDController.java | 114 --------------------- .../lib/robot/PIDOutputSplitter.java | 80 --------------- src/org/mckenzierobotics/lib/robot/PIDServo.java | 36 ------- .../lib/robot/RateLimitedPIDOutput.java | 50 --------- src/org/mckenzierobotics/lib/robot/RollingAvg.java | 90 ---------------- .../lib/robot/SendablePIDOutput.java | 42 -------- .../usfirst/frc/team4272/robot2016/HwRobot.java | 2 +- .../usfirst/frc/team4272/robotlib/CommandBase.java | 73 +++++++++++++ .../frc/team4272/robotlib/CommandRobot.java | 65 ++++++++++++ .../usfirst/frc/team4272/robotlib/FeedForward.java | 76 ++++++++++++++ .../frc/team4272/robotlib/PIDController.java | 114 +++++++++++++++++++++ .../frc/team4272/robotlib/PIDOutputSplitter.java | 80 +++++++++++++++ .../usfirst/frc/team4272/robotlib/PIDServo.java | 36 +++++++ .../team4272/robotlib/RateLimitedPIDOutput.java | 50 +++++++++ .../usfirst/frc/team4272/robotlib/RollingAvg.java | 90 ++++++++++++++++ .../frc/team4272/robotlib/SendablePIDOutput.java | 42 ++++++++ 19 files changed, 627 insertions(+), 627 deletions(-) delete mode 100644 src/org/mckenzierobotics/lib/robot/CommandBase.java delete mode 100644 src/org/mckenzierobotics/lib/robot/CommandRobot.java delete mode 100644 src/org/mckenzierobotics/lib/robot/FeedForward.java delete mode 100644 src/org/mckenzierobotics/lib/robot/PIDController.java delete mode 100644 src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java delete mode 100644 src/org/mckenzierobotics/lib/robot/PIDServo.java delete mode 100644 src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java delete mode 100644 src/org/mckenzierobotics/lib/robot/RollingAvg.java delete mode 100644 src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/CommandBase.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/CommandRobot.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/FeedForward.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/PIDController.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/PIDOutputSplitter.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/PIDServo.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/RateLimitedPIDOutput.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/RollingAvg.java create mode 100644 src/org/usfirst/frc/team4272/robotlib/SendablePIDOutput.java diff --git a/src/org/mckenzierobotics/lib/robot/CommandBase.java b/src/org/mckenzierobotics/lib/robot/CommandBase.java deleted file mode 100644 index a0fb5a2..0000000 --- a/src/org/mckenzierobotics/lib/robot/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 . - * - * @author Luke Shumaker - */ -package org.mckenzierobotics.lib.robot; - -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 - */ -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/mckenzierobotics/lib/robot/CommandRobot.java b/src/org/mckenzierobotics/lib/robot/CommandRobot.java deleted file mode 100644 index ecbd879..0000000 --- a/src/org/mckenzierobotics/lib/robot/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 . - * - * @author Luke Shumaker - */ - -package org.mckenzierobotics.lib.robot; - -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 - */ -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/mckenzierobotics/lib/robot/FeedForward.java b/src/org/mckenzierobotics/lib/robot/FeedForward.java deleted file mode 100644 index c7a06b8..0000000 --- a/src/org/mckenzierobotics/lib/robot/FeedForward.java +++ /dev/null @@ -1,76 +0,0 @@ -/** - * 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 . - * - * @author Luke Shumaker - */ -package org.mckenzierobotics.lib.robot; - -import edu.wpi.first.wpilibj.PIDOutput; -import edu.wpi.first.wpilibj.PIDSource; - -/** - * - * @author Luke Shumaker - */ -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/mckenzierobotics/lib/robot/PIDController.java b/src/org/mckenzierobotics/lib/robot/PIDController.java deleted file mode 100644 index b0cf99f..0000000 --- a/src/org/mckenzierobotics/lib/robot/PIDController.java +++ /dev/null @@ -1,114 +0,0 @@ -/** - * 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 - */ -package org.mckenzierobotics.lib.robot; - -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 - */ -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/mckenzierobotics/lib/robot/PIDOutputSplitter.java b/src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java deleted file mode 100644 index c689bab..0000000 --- a/src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java +++ /dev/null @@ -1,80 +0,0 @@ -/** - * 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 - */ - -package org.mckenzierobotics.lib.robot; - -import edu.wpi.first.wpilibj.PIDOutput; - -/** - * TODO: Write JavaDocs - * @author Luke Shumaker - */ -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. - * - * @author Luke Shumaker - */ -package org.mckenzierobotics.lib.robot; - -import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.PIDOutput; - -/** - * TODO: Write JavaDocs - * @author Luke Shumaker - */ -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/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java b/src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java deleted file mode 100644 index 10f01ce..0000000 --- a/src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java +++ /dev/null @@ -1,50 +0,0 @@ -/** - * 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 . - * - * @author Luke Shumaker - */ - -package org.mckenzierobotics.lib.robot; - -import edu.wpi.first.wpilibj.PIDOutput; -import edu.wpi.first.wpilibj.Timer; - -/** - * - * @author Luke Shumaker - */ -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/mckenzierobotics/lib/robot/RollingAvg.java b/src/org/mckenzierobotics/lib/robot/RollingAvg.java deleted file mode 100644 index 3001b8a..0000000 --- a/src/org/mckenzierobotics/lib/robot/RollingAvg.java +++ /dev/null @@ -1,90 +0,0 @@ -/** - * 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 - */ - -package org.mckenzierobotics.lib.robot; - -import edu.wpi.first.wpilibj.PIDSource; -import edu.wpi.first.wpilibj.PIDSourceType; -import edu.wpi.first.wpilibj.PIDOutput; - -/** - * TODO: Write JavaDocs - * @author Luke Shumaker - */ -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/mckenzierobotics/lib/robot/SendablePIDOutput.java b/src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java deleted file mode 100644 index 679c6e1..0000000 --- a/src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java +++ /dev/null @@ -1,42 +0,0 @@ -/** - * 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 . - * - * @author Luke Shumaker - */ - -package org.mckenzierobotics.lib.robot; - -import edu.wpi.first.wpilibj.PIDOutput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * - * @author Luke Shumaker - */ -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); - } -} diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java index da4c1b4..b2c7150 100644 --- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java +++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java @@ -1,6 +1,6 @@ package org.usfirst.frc.team4272.robot2016; -import org.mckenzierobotics.lib.robot.PIDOutputSplitter; +import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PIDOutput; 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 . + * + * @author Luke Shumaker + */ +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 + */ +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 . + * + * @author Luke Shumaker + */ + +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 + */ +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 . + * + * @author Luke Shumaker + */ +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; + +/** + * + * @author Luke Shumaker + */ +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 + */ +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 + */ +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 + */ + +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.PIDOutput; + +/** + * TODO: Write JavaDocs + * @author Luke Shumaker + */ +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. + * + * @author Luke Shumaker + */ +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.PIDOutput; + +/** + * TODO: Write JavaDocs + * @author Luke Shumaker + */ +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 . + * + * @author Luke Shumaker + */ + +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.Timer; + +/** + * + * @author Luke Shumaker + */ +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 + */ + +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 + */ +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 . + * + * @author Luke Shumaker + */ + +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * + * @author Luke Shumaker + */ +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); + } +} -- cgit v1.2.3