diff options
22 files changed, 1197 insertions, 0 deletions
diff --git a/.classpath b/.classpath new file mode 100644 index 0000000..daf6ebb --- /dev/null +++ b/.classpath @@ -0,0 +1,7 @@ +<classpath> + <classpathentry kind="src" path="src"/> + <classpathentry kind="var" path="wpilib" sourcepath="wpilib.sources"/> + <classpathentry kind="var" path="networktables" sourcepath="networktables.sources"/> + <classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-1.6"/> + <classpathentry kind="output" path="bin"/> +</classpath> diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..373e0e2 --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +/.settings/ +/bin/ +/build/ +/dist/ +/doc/ diff --git a/.project b/.project new file mode 100644 index 0000000..fb46bdc --- /dev/null +++ b/.project @@ -0,0 +1,18 @@ +<?xml version="1.0" encoding="UTF-8"?> +<projectDescription> + <name>frc4272-robot2015-3</name> + <comment></comment> + <projects> + </projects> + <buildSpec> + <buildCommand> + <name>org.eclipse.jdt.core.javabuilder</name> + <arguments> + </arguments> + </buildCommand> + </buildSpec> + <natures> + <nature>org.eclipse.jdt.core.javanature</nature> + <nature>edu.wpi.first.wpilib.plugins.core.nature.FRCProjectNature</nature> + </natures> +</projectDescription> diff --git a/build.properties b/build.properties new file mode 100644 index 0000000..0d5fd76 --- /dev/null +++ b/build.properties @@ -0,0 +1,4 @@ +# Project specific information +package=org.usfirst.frc.team4272.robot2015 +robot.class=${package}.Robot +simulation.world.file=/usr/share/frcsim/worlds/GearsBotDemo.world
\ No newline at end of file diff --git a/build.xml b/build.xml new file mode 100644 index 0000000..2182d37 --- /dev/null +++ b/build.xml @@ -0,0 +1,30 @@ +<?xml version="1.0" encoding="UTF-8"?> + +<project name="FRC Deployment" default="deploy"> + + <!-- + The following properties can be defined to override system level + settings. These should not be touched unless you know what you're + doing. The primary use is to override the wpilib version when + working with older robots that can't compile with the latest + libraries. + --> + + <!-- By default the system version of WPI is used --> + <!-- <property name="version" value=""/> --> + + <!-- By default the system team number is used --> + <!-- <property name="team-number" value=""/> --> + + <!-- By default the target is set to 10.TE.AM.2 --> + <!-- <property name="target" value=""/> --> + + <!-- Any other property in build.properties can also be overridden. --> + + <property file="${user.home}/wpilib/wpilib.properties"/> + <property file="build.properties"/> + <property file="${user.home}/wpilib/java/${version}/ant/build.properties"/> + + <import file="${wpilib.ant.dir}/build.xml"/> + +</project> diff --git a/src/org/mckenzierobotics/lib/robot/CommandBase.java b/src/org/mckenzierobotics/lib/robot/CommandBase.java new file mode 100644 index 0000000..0e70893 --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/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.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 <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/mckenzierobotics/lib/robot/CommandRobot.java b/src/org/mckenzierobotics/lib/robot/CommandRobot.java new file mode 100644 index 0000000..35534c2 --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/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.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 <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/mckenzierobotics/lib/robot/FeedForward.java b/src/org/mckenzierobotics/lib/robot/FeedForward.java new file mode 100644 index 0000000..a275366 --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/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.mckenzierobotics.lib.robot; + +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.isEnable()) { + 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 new file mode 100644 index 0000000..dc2301a --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/PIDController.java @@ -0,0 +1,104 @@ +/** + * 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.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 <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.isEnable(); + 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.isEnable(); + 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); + } +} diff --git a/src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java b/src/org/mckenzierobotics/lib/robot/PIDOutputSplitter.java new file mode 100644 index 0000000..b4e7713 --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/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.mckenzierobotics.lib.robot; + +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/mckenzierobotics/lib/robot/PIDServo.java b/src/org/mckenzierobotics/lib/robot/PIDServo.java new file mode 100644 index 0000000..fdbd1ce --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/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.mckenzierobotics.lib.robot; + +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/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java b/src/org/mckenzierobotics/lib/robot/RateLimitedPIDOutput.java new file mode 100644 index 0000000..ed36345 --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/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.mckenzierobotics.lib.robot; + +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/mckenzierobotics/lib/robot/RollingAvg.java b/src/org/mckenzierobotics/lib/robot/RollingAvg.java new file mode 100644 index 0000000..fcbf8e4 --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/RollingAvg.java @@ -0,0 +1,75 @@ +/** + * 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.mckenzierobotics.lib.robot; + +import edu.wpi.first.wpilibj.PIDSource; +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()); + return get(); + } + + public void pidWrite(double output) { + push(output); + } +} diff --git a/src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java b/src/org/mckenzierobotics/lib/robot/SendablePIDOutput.java new file mode 100644 index 0000000..43852b1 --- /dev/null +++ b/src/org/mckenzierobotics/lib/robot/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.mckenzierobotics.lib.robot; + +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); + } +} diff --git a/src/org/usfirst/frc/team4272/robot2015/HwOI.java b/src/org/usfirst/frc/team4272/robot2015/HwOI.java new file mode 100644 index 0000000..9d09b65 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/HwOI.java @@ -0,0 +1,11 @@ +package org.usfirst.frc.team4272.robot2015; + +import org.usfirst.frc.team4272.robotlib.Xbox360Controller; + +import edu.wpi.first.wpilibj.Joystick; + +public class HwOI { + public Joystick lStick = new Joystick(0); + public Joystick rStick = new Joystick(1); + public Xbox360Controller xbox = new Xbox360Controller(2); +} diff --git a/src/org/usfirst/frc/team4272/robot2015/HwRobot.java b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java new file mode 100644 index 0000000..22a7afc --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/HwRobot.java @@ -0,0 +1,75 @@ +package org.usfirst.frc.team4272.robot2015; + +import org.mckenzierobotics.lib.robot.PIDController; +import org.mckenzierobotics.lib.robot.PIDOutputSplitter; +import org.usfirst.frc.team4272.robotlib.DoubleSolenoid; +import org.usfirst.frc.team4272.robotlib.LimitSwitchedPIDOutput; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.PIDSource; +import edu.wpi.first.wpilibj.PIDSource.PIDSourceParameter; +import edu.wpi.first.wpilibj.Relay; +import edu.wpi.first.wpilibj.Talon; + +public class HwRobot { + /* Relay == a Spike */ + /* PCM = Pneumatics Control Module */ + + /* All of the numbered inputs are in the classes: + * - DIO: 0-9 + * - Relay: 0-3 + * - Analog In: 0-3 + * - PWM: 0-9 + * - PCM: 0-7 (the PCM is connected via CAN). + * - CAN + * + * For completeness, the roboRIO also has: i2c, RS-232, SPI, + * RSL, 2xUSB-A, an accelerometer, and an expansion port. + * + * And, for communication: USB-B and Ethernet. + */ + + static class OutDumper implements PIDOutput { + private String p; + public OutDumper(String prefix) { p = prefix; } + public void pidWrite(double v) { + System.out.println(p+": "+v); + } + } + static class InDumper implements PIDSource { + private String p; + public final PIDSource r; + public InDumper(String prefix, PIDSource real) { p = prefix; r = real; } + public double pidGet() { + double v = r.pidGet(); + System.out.println(p+": "+v); + return v; + } + } + public Encoder lDriveE = new Encoder(/*DIO*/2,/*DIO*/3); + public PIDOutput lDriveM = new PIDOutputSplitter(new Talon(/*PWM*/0), + new Talon(/*PWM*/1)); + public Encoder rDriveE = new Encoder(/*DIO*/0,/*DIO*/1, true); + public PIDOutput rDriveM = new PIDOutputSplitter(new Talon(/*PWM*/2), + new Talon(/*PWM*/3)); + public Encoder winchE = new Encoder(/*DIO*/4,/*DIO*/5);//), + public PIDOutput winchM = new LimitSwitchedPIDOutput( + new PIDOutputSplitter(new Talon(/*PWM*/4), + new Talon(/*PWM*/5)), + /*bottom*/new DigitalInput(/*DIO*/6), + /*top*/ new DigitalInput(/*DIO*/7), + true); + public DoubleSolenoid grab = new DoubleSolenoid(/*PCM*/2,/*PCM*/3); + public DoubleSolenoid push = new DoubleSolenoid(/*PCM*/0,/*PCM*/1); + public Relay lIntake = new Relay(/*Relay*/1, Relay.Direction.kBoth); + public Relay rIntake = new Relay(/*Relay*/0, Relay.Direction.kBoth); + public Relay lights = new Relay(/*Relay*/2, Relay.Direction.kForward); + + public HwRobot() { + lDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); + rDriveE.setPIDSourceParameter(PIDSourceParameter.kRate); + winchE.setPIDSourceParameter(PIDSourceParameter.kRate); + } +} diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java new file mode 100644 index 0000000..95a47b7 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java @@ -0,0 +1,105 @@ + +package org.usfirst.frc.team4272.robot2015; + +import org.usfirst.frc.team4272.robotlib.Toggler; +import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis; +import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button; + +import edu.wpi.first.wpilibj.IterativeRobot; +import edu.wpi.first.wpilibj.Relay; + +/** + * 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. + */ +public class Robot extends IterativeRobot { + private HwRobot robot = new HwRobot(); + private HwOI oi; + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. + */ + public void robotInit() { + + } + + public void autonomousInit() { + + } + public void autonomousPeriodic() { + + } + + private Toggler grabButton = new Toggler(); + private Toggler pushButton = new Toggler(); + private Toggler ledsButton = new Toggler(); + public void teleopInit() { + //robot.lDrive.reset(); + //robot.rDrive.reset(); + //robot.winch.reset(); + //robot.lDrive.enable(); + //robot.rDrive.enable(); + //robot.winch.enable(); + robot.grab.setEnabled(true); + robot.push.setEnabled(true); + oi = new HwOI(); + grabButton.update(false); + pushButton.update(false); + ledsButton.update(false); + } + public void teleopPeriodic() { + /* Drive */ + robot.lDriveM.pidWrite(-oi.lStick.getY()*Math.abs(oi.lStick.getY())); + robot.rDriveM.pidWrite( oi.rStick.getY()*Math.abs(oi.rStick.getY())); + + /* Winch */ + double w = oi.xbox.getAxis(Axis.LY); + if (Math.abs(w) < 0.1) { w = 0; } + robot.winchM.pidWrite(-w); + + /* left intake */ + if (oi.xbox.getButton(Button.LB)) { + robot.lIntake.set(Relay.Value.kReverse); + } else if (oi.xbox.getAxis(Axis.LT) > 0.75) { + robot.lIntake.set(Relay.Value.kForward); + } else { + robot.lIntake.set(Relay.Value.kOff); + } + /* right intake */ + if (oi.xbox.getButton(Button.RB)) { + robot.rIntake.set(Relay.Value.kReverse); + } else if (oi.xbox.getAxis(Axis.RT) > 0.75) { + robot.rIntake.set(Relay.Value.kForward); + } else { + robot.rIntake.set(Relay.Value.kOff); + } + + /* grab */ + robot.grab.setForward(grabButton.update(oi.xbox.getButton(Button.A))); + robot.push.setForward(pushButton.update(oi.xbox.getButton(Button.B))); + if (ledsButton.update(oi.xbox.getButton(Button.RT))) { + robot.lights.set(Relay.Value.kForward); + } else { + robot.lights.set(Relay.Value.kOff); + } + } + + public void disabledInit() { + //robot.lDrive.reset(); + //robot.rDrive.reset(); + //robot.winch.reset(); + robot.grab.setEnabled(false); + robot.push.setEnabled(false); + } + + /** + * This function is called periodically during test mode + */ + public void testPeriodic() { + + } + +} diff --git a/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java b/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java new file mode 100644 index 0000000..8698277 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/DoubleSolenoid.java @@ -0,0 +1,39 @@ +package org.usfirst.frc.team4272.robotlib; + +public class DoubleSolenoid extends edu.wpi.first.wpilibj.DoubleSolenoid { + private boolean enabled = true; + private Value value; + + public DoubleSolenoid(int forwardChannel, int reverseChannel) { + super(forwardChannel, reverseChannel); + value = get(); + } + public DoubleSolenoid(int moduleNumber, int forwardChannel, int reverseChannel) { + super(moduleNumber, forwardChannel, reverseChannel); + value = get(); + } + + public void setEnabled(boolean enabled) { + this.enabled = enabled; + if (enabled) { + set(value); + } else { + set(Value.kOff); + } + } + + public void setForward(boolean forward) { + set(forward ? Value.kForward : Value.kReverse); + } + + public boolean getForward() { + return value == Value.kForward; + } + + public void set(Value value) { + this.value = value; + if (enabled) { + super.set(value); + } + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java new file mode 100644 index 0000000..73073c2 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/LimitSwitchedPIDOutput.java @@ -0,0 +1,38 @@ +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.PIDOutput; + +public class LimitSwitchedPIDOutput implements PIDOutput { + private final PIDOutput out; + private final DigitalInput forward; + private final DigitalInput backward; + private final boolean i_dont_know_which_way_is_which; + + public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward, boolean i_dont_know_which_way_is_which) { + this.out = out; + this.forward = forward; + this.backward = backward; + this.i_dont_know_which_way_is_which = i_dont_know_which_way_is_which; + } + + public LimitSwitchedPIDOutput(PIDOutput out, DigitalInput forward, DigitalInput backward) { + this(out, forward,backward, false); + } + + public void pidWrite(double v) { + if (i_dont_know_which_way_is_which) { + if (forward.get() || backward.get()) { + v = 0; + } + } else { + if (forward.get()) { + v = Math.min(v, 0); + } + if (backward.get()) { + v = Math.max(v, 0); + } + } + out.pidWrite(v); + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/Toggler.java b/src/org/usfirst/frc/team4272/robotlib/Toggler.java new file mode 100644 index 0000000..03fffec --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/Toggler.java @@ -0,0 +1,13 @@ +package org.usfirst.frc.team4272.robotlib; + +public class Toggler { + private boolean prev = false; + private boolean state = false; + public boolean update(boolean next) { + if (next && ! prev) { + state = !state; + } + prev = next; + return state; + } +} diff --git a/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java b/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java new file mode 100644 index 0000000..d04fa23 --- /dev/null +++ b/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java @@ -0,0 +1,251 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */ +/* Copyright (c) Luke Shumaker 2015. 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. */ +/*----------------------------------------------------------------------------*/ +package org.usfirst.frc.team4272.robotlib; + +import edu.wpi.first.wpilibj.Joystick; + +/** + * Handle input from a wired Xbox 360 controller connected to the + * Driver Station. + */ +public class Xbox360Controller extends Joystick { + /* Constants ************************************************/ + + /** + * Represents an analog axis on an Xbox 360 controller. + */ + public static enum Axis { + LX(0), LY(1), /** left trigger */ LT(2), + RX(4), RY(5), /** right trigger */ RT(3), + /** D-Pad X */ DX(6), /** D-Pad Y */ DY(7); + + private final int id; + private Axis(int id) { this.id = id; } + public int getId() { return id; } + } + + /** + * Represents a digital button on Xbox 360 controller. + */ + public static enum Button { + A(0), B(1), + X(2), Y(3), + /** left bumper */ LB(4), /** right bumper */RB( 5), + Back(6), Start(7), /*Home(8),*/ + /** left thumb */ LT(8), /** right thumb */ RT(9); + + public final int id; + private Button(int id) { this.id = id+1; } + } + + /* Constructor **********************************************/ + + /** + * Construct an instance of a joystick. + * The joystick index is the USB port on the drivers station. + * + * @param port The port on the driver station that the joystick is plugged into. + */ + public Xbox360Controller(final int port) { + super(port); + } + + /* Core functions *******************************************/ + + /** + * Get the value of an axis base on an enumerated type. + * + * @param axis The axis to read. + * @return The value of the axis. + */ + public double getAxis(Axis axis) { + return getRawAxis(axis.id); + } + + /** + * Get buttons based on an enumerated type. + * + * @param button The button to read. + * @return The state of the button. + */ + public boolean getButton(Button button) { + return getRawButton(button.id); + } + + + /* The actual code is done. The rest is boilerplate. See, + * this is why Java is terrible. Just 30% of the file + * actually doing useful stuff, the rest just filling + * interfaces. */ + /* Stupid little wrappers ***********************************/ + + /** + * Get the X value of a thumb-stick. + * + * @param hand Left stick or right? + * @return The X value of the joystick. + */ + public double getX(final Hand hand) { + if (hand.value == Hand.kLeft.value) + return getAxis(Axis.LX); + if (hand.value == Hand.kRight.value) + return getAxis(Axis.RX); + return 0.0; + } + + /** + * Get the Y value of a thumb-stick. + + * @param hand Left stick or right? + * @return The Y value of the joystick. + */ + public double getY(final Hand hand) { + if (hand.value == Hand.kLeft.value) + return getAxis(Axis.LY); + if (hand.value == Hand.kRight.value) + return getAxis(Axis.RY); + return 0.0; + } + + /** + * Get the value of a trigger. + * + * @param hand Left trigger or right? + * @return The trigger value. + */ + public double getZ(final Hand hand) { + if (hand.value == Hand.kLeft.value) + return getAxis(Axis.LT); + if (hand.value == Hand.kRight.value) + return getAxis(Axis.RT); + return 0.0; + } + + /** + * Get the state of a bumper. + * + * @param hand Left trigger or right? + * @return the state of the bumper. + */ + public boolean getBumper(Hand hand) { + if (hand.value == Hand.kLeft.value) + return getButton(Button.LB); + if (hand.value == Hand.kRight.value) + return getButton(Button.RB); + return false; + } + + /** + * Get the state of a thumb-stick button. + * + * @param hand Left trigger or right? + * @return the state of the button. + */ + public boolean getTop(Hand hand) { + if (hand.value == Hand.kLeft.value) + return getButton(Button.LT); + if (hand.value == Hand.kRight.value) + return getButton(Button.RB); + return false; + } + + /** + * Get the state of a trigger; whether it is more than + * half-way pressed or not. + * + * @param hand Left trigger or right? + * @return The state of the trigger. + */ + public boolean getTrigger(Hand hand) { + return getZ(hand) > 0.75; + } + + /** + * Get the magnitude of the direction vector formed by the thumb-stick's + * current position relative to its origin + * + * @return The magnitude of the direction vector + */ + public double getMagnitude(Hand hand) { + return Math.sqrt(Math.pow(getX(hand), 2) + Math.pow(getY(hand), 2)); + } + + public double getMagnitude() { + return getMagnitude(Hand.kRight); + } + + /** + * Get the direction of the vector formed by the thumb-stick and its origin + * in radians + * + * @return The direction of the vector in radians + */ + public double getDirectionRadians(Hand hand) { + return Math.atan2(getX(hand), -getY(hand)); + } + + public double getDirectionRadians() { + return getDirectionRadians(Hand.kRight); + } + + /** + * Get the direction of the vector formed by the thumb-stick and its origin + * in degrees + * + * uses acos(-1) to represent Pi due to absence of readily accessable Pi + * constant in C++ + * + * @return The direction of the vector in degrees + */ + public double getDirectionDegrees(Hand hand) { + return Math.toDegrees(getDirectionRadians(hand)); + } + + public double getDirectionDegrees() { + return Math.toDegrees(getDirectionRadians(Hand.kRight)); + } + + + /* Unused wrappers for GenericHID/Joystick ******************/ + + /** + * This method is only here to complete the GenericHID interface. + * + * @return Always 0.0 + */ + public double getTwist() { + return 0.0; + } + + /** + * This method is only here to complete the GenericHID interface. + * + * @return Always 0.0 + */ + public double getThrottle() { + return 0.0; + } + + /** + * This method is only here to complete the Joystick interface. + * + * @param axis unused + * @return Always 0 + */ + public int getAxisChannel(AxisType axis) { + return 0; + } + + /** + * This method is only here to complete the Joystick interface. + * + * @param axis unused + * @param channel unused + */ + public void setAxisChannel(AxisType axis, int channel) {} +} diff --git a/sysProps.xml b/sysProps.xml Binary files differnew file mode 100644 index 0000000..05b01bc --- /dev/null +++ b/sysProps.xml |