Changes sitting on netbooknetbook-workspace2

9 files changed, 612 insertions, 811 deletions

diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java b/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
deleted file mode 100644
index 756c33d..0000000
--- a/src/org/usfirst/frc/team4272/robot2016/Autonomous.java
+++ /dev/null
@@ -1,33 +0,0 @@
-package org.usfirst.frc.team4272.robot2016;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-public class Autonomous {
-	private final Timer time = new Timer();
-	private final HwRobot robot;
-
-	public Autonomous(HwRobot robot) {
-		this.robot = robot;
-		time.reset();
-		time.start();
-	}
-
-	public Control run(Control c) {
-		if (robot.armL.get()) {
-			c.arm = -0.5;
-		} else {
-			c.arm = 0;
-			robot.armE.reset();
-		}
-
-		if (time.get() < 4) {
-			c.lDrive1 = c.rDrive1 = -0.4;
-		} else {
-			c.lDrive1 = c.rDrive1 = 0;
-		}
-		SmartDashboard.putNumber("autoTime", time.get());
-
-		return c;
-	}
-}
diff --git a/src/org/usfirst/frc/team4272/robot2016/Autonomus.java b/src/org/usfirst/frc/team4272/robot2016/Autonomus.java
new file mode 100644
index 0000000..f3c4483
--- /dev/null
+++ b/src/org/usfirst/frc/team4272/robot2016/Autonomus.java
@@ -0,0 +1,243 @@
+/**

+* Copyright (c) 2017 Luke Shumaker.

+* Copyright (c) 2017 Thomas Griffith.

+* 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.

+*/

+package org.usfirst.frc.team4272.robot2017;

+

+import edu.wpi.first.wpilibj.Timer;

+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;

+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

+

+public class Autonomous {

+        private static interface Command {

+                /** Mutate the @{code Control} (possible because it is

+                 * passed by reference), and return whether or not

+                 * this command has finished yet.

+                 *

+                 * @param c The structure to control the robot

+                 * @return Whether the command has finished.

+                 */

+                public boolean execute(Control c);

+        }

+

+        private static class DriveDistance implements Command {

+                private static final double pctTaper = 1.0/6;

+                private static final double pctTolerance = 0.002;

+

+                private final HwRobot robot;

+                private final double pctThrottle;

+                private final double lSpeed, lDistTarget;

+                private final double rSpeed, rDistTarget;

+                private double lMin = 0, rMin = 0;

+                public DriveDistance(HwRobot robot, double pctThrottle,

+                                     double lSpeed, double lDistTarget,

+                                     double rSpeed, double rDistTarget) {

+                        this.robot = robot;

+                        this.pctThrottle = pctThrottle;

+                        this.lSpeed = lSpeed;

+                        this.lDistTarget = lDistTarget;

+                        this.rSpeed = rSpeed;

+                        this.rDistTarget = rDistTarget;

+                }

+                private boolean initialized = false;

+                public boolean execute(Control c) {

+                        if (!initialized) {

+                                robot.lDriveE.reset();

+                                robot.rDriveE.reset();

+                                initialized = true;

+                        }

+                        double lDistCurr = robot.lDriveE.getDistance();

+                        double rDistCurr = robot.rDriveE.getDistance();

+                        double lPct = lDistCurr/lDistTarget;

+                        double rPct = rDistCurr/rDistTarget;

+

+                        /* Left side */

+                        c.lDrive = lSpeed;

+                        if (Math.abs(1-lPct) < pctTaper) {

+                                /* Taper off, slowing down as we approach the designed target */

+                                c.lDrive *= Math.abs((1-lPct)/pctTaper);

+                                if (Math.abs(robot.lRate.pidGet()) < 0.5) {

+                                        /* oops, tapered off too quickly */

+                                        lMin = c.lDrive = Math.max(c.lDrive, lMin) * 1.05; /* Multiplier */

+                                }

+                                if (c.lDrive > lSpeed) { /* Speed too high */

+                                        c.lDrive = lSpeed;

+                                }

+                        }

+                        c.lDrive *= Math.signum(lDistTarget) * Math.signum(1-lPct);

+

+                        /* Right side */

+                        c.rDrive = rSpeed;

+                        if (Math.abs(1-rPct) < pctTaper) {

+                                /* Taper off, slowing down as we approach the designated target */

+                                c.rDrive *= Math.abs((1-rPct)/pctTaper);

+                                if (Math.abs(robot.rRate.pidGet()) < 0.5) {

+                                        /* oops, tapered off too quickly */

+                                        rMin = c.rDrive = Math.max(c.rDrive, rMin) * 1.05; /* Multiplier */

+                                }

+                                if (c.rDrive > rSpeed) { /* Speed too high */

+                                        c.rDrive = rSpeed;

+                                }

+                        }

+                        c.rDrive *= Math.signum(rDistTarget) * Math.signum(1-rPct);

+

+                        /* Make sure the left and right sides are even */

+                        if (Math.abs(lPct - rPct) > pctTolerance) {

+                                if (lPct > rPct) {

+                                        c.lDrive *= pctThrottle;

+                                } else {

+                                        c.rDrive *= pctThrottle;

+                                }

+                        }

+

+                        SmartDashboard.putNumber("lPct", lPct);

+                        SmartDashboard.putNumber("rPct", rPct);

+                        SmartDashboard.putNumber("lMin", lMin);

+                        SmartDashboard.putNumber("rMin", rMin);

+                        SmartDashboard.putNumber("lDrive", c.lDrive);

+                        SmartDashboard.putNumber("rDrive", c.rDrive);

+                        return lPct >= 1 && rPct >= 1; 

+                }

+        }

+        private static class Stop implements Command {

+                public boolean execute(Control c) {

+                        c.lDrive = c.rDrive = 0;

+                        return true;

+                }

+        }

+        private static class Init implements Command {

+                public boolean execute(Control c) {

+                        c.lDrive = c.rDrive = 0;

+                        c.highGear = c.gedOut = false;

+                        return true;

+                }

+        }

+        private static class TimedCommand implements Command {

+                private final Command inner;

+                private final double secs;

+                private final Timer t = new Timer();

+                private boolean initialized = false;

+                public TimedCommand(Command inner, double secs) {

+                        this.inner = inner;

+                        this.secs = secs;

+                }

+                public boolean execute(Control c) {

+                        if (!initialized) {

+                                t.reset();

+                                t.start();

+                                initialized = true;

+                        }

+                        if (t.get() < secs) {

+                                return inner.execute(c);

+                        } else {

+                                return true;

+                        }

+                }

+        }

+

+        private static final double axleWidth = 2; // in feet

+        private static Command driveDistance(HwRobot robot, double speed, double dist) {

+                return new DriveDistance(robot, 0.2,

+                                         speed, dist,

+                                         speed, dist);

+        }

+        private static Command turnRadians(HwRobot robot, double speed, double rad) {

+                return new DriveDistance(robot, 0.6,

+                                         speed, Math.copySign(rad*axleWidth/2, rad),

+                                         speed, Math.copySign(rad*axleWidth/2, -rad));

+        }

+        private static Command turnDegrees(HwRobot robot, double speed, double deg) {

+                return turnRadians(robot, speed, deg * Math.PI/180);

+        }

+        private static Command stop(HwRobot robot) {

+                return new Stop();

+        }

+

+        private final HwRobot robot;

+        private final Command[] commands;

+        private int step = 0;

+        private static final SendableChooser modeChooser = new SendableChooser();

+

+        public static void robotInit() {

+                modeChooser.addObject("Center Peg", "Center Peg");

+                modeChooser.addObject("Left Peg", "Left Peg");

+                modeChooser.addObject("Right Peg", "Right Peg");

+                modeChooser.addObject("Stand Still", "Stand Still");

+                SmartDashboard.putData("Autonomous Mode", modeChooser);

+        }

+

+        public Autonomous(HwRobot robot) {

+                this.robot = robot;

+

+                String mode = (String) modeChooser.getSelected();

+                SmartDashboard.putString("read mode", mode);

+

+                switch (mode) {

+                case "Center Peg":

+                        commands = new Command[]{

+                                new Init(),

+                                new TimedCommand(driveDistance(robot, 0.4, 8.75), 6),

+                                stop(robot),

+                                new Command() {public boolean execute(Control c) {

+                                        c.gedOut = true;

+                                        return true;

+                                }},

+                                driveDistance(robot, 0.4, -3),

+                                stop(robot),

+                        };

+                        break;

+                case "Left Peg":

+                        commands = new Command[]{

+                                new Init(),

+                                driveDistance(robot, 0.4, 3.5),

+                                turnDegrees(robot, 0.4, 60),

+                                driveDistance(robot, 0.4, 8.3),

+                                stop(robot),

+                        };

+                        break;

+                case "Right Peg":

+                        commands = new Command[]{

+                                new Init(),

+                                driveDistance(robot, 0.4, 3.5),

+                                turnDegrees(robot, 0.4, -60),

+                                driveDistance(robot, 0.4, 8.3),

+                                stop(robot),

+                        };

+                        break;

+                default:

+                        commands = new Command[]{stop(robot)};

+                }

+        }

+

+        public Control run(Control c) {

+                if (step < commands.length) {

+                        if (commands[step].execute(c))

+                                step++;

+                }

+                SmartDashboard.putNumber("step", step);

+                return c;

+        }

+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team4272/robot2016/Control.java b/src/org/usfirst/frc/team4272/robot2016/Control.java
index 40fcb5a..da706fc 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Control.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Control.java
@@ -1,13 +1,10 @@
-package org.usfirst.frc.team4272.robot2016;
-
-public class Control {
-	double lDrive1 = 0,
-	       lDrive2 = 0,
-	       rDrive1 = 0,
-	       rDrive2 = 0,
-	       fBall   = 0,
-	       bBall   = 0,
-	       arm     = 0;
-
-	public Control() {}
-}
+public class Control {

+        double lDrive = 0,

+               rDrive = 0,

+               camRotate = 0,

+               camTilt = 0,

+               climber = 0;

+        boolean highGear = false,

+                gedOut = false;

+        public Control() {}

+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwOI.java b/src/org/usfirst/frc/team4272/robot2016/HwOI.java
index fc5cc78..3f05243 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwOI.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwOI.java
@@ -1,11 +1,11 @@
-package org.usfirst.frc.team4272.robot2016;
-
-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);
-}
+// This file is not sufficiently creative to qualify for copyright.

+package org.usfirst.frc.team4272.robot2017;

+

+import edu.wpi.first.wpilibj.Joystick;

+import edu.wpi.first.wpilibj.XboxController;

+

+public class HwOI {

+        public Joystick lStick = new Joystick(0);

+        public Joystick rStick = new Joystick(1);

+        public XboxController xbox = new XboxController(2);

+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
index e61d87f..23ae62a 100644
--- a/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/HwRobot.java
@@ -1,84 +1,131 @@
-package org.usfirst.frc.team4272.robot2016;
-
-import edu.wpi.first.wpilibj.networktables.NetworkTable;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PIDOutput;
-import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.PowerDistributionPanel;
-import edu.wpi.first.wpilibj.Talon;
-
-import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;
-
-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.
-	 */
-
-	// naming convention: {side_letter}{Thing}{E|M (encoder/motor)}
-
-	private final PIDOutput
-	    rDriveM1 = new Talon(/*PWM*/2), /* PDP 2, 3 */
-	    rDriveM2 = new Talon(/*PWM*/4), /* PDP 4 */
-	    lDriveM1 = new Talon(/*PWM*/1), /* PDP 12, 13 */
-	    lDriveM2 = new Talon(/*PWM*/0), /* PDP 11 */
-	    armM     = new Talon(/*PWM*/8), /* PDP 15 */
-	    rfBallM  = new Talon(/*PWM*/3), /* PDP 0 */
-	    lfBallM  = new Talon(/*PWM*/9), /* PDP 14 */
-	    fBallM   = new PIDOutputSplitter(rfBallM, lfBallM),
-	    bBallM   = new Talon(/*PWM*/6); /* PDP 10, 5 */
-
-	public final Encoder   armE = new Encoder(/*DIO*/0, /*DIO*/1, /*DIO*/2, true);
-	public final DigitalInput armL = new DigitalInput(/*DIO*/3);
-	public final DigitalInput ballL = new DigitalInput(/*DIO*/4);
-	public final Encoder   rDriveE = new Encoder(/*DIO*/5,/*DIO*/6);
-	public final Encoder   lDriveE = new Encoder(/*DIO*/7,/*DIO*/8, true);
-	public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */
-
-	public HwRobot() {
-		armE.setPIDSourceType(PIDSourceType.kDisplacement);
-		lDriveE.setPIDSourceType(PIDSourceType.kRate);
-		rDriveE.setPIDSourceType(PIDSourceType.kRate);
-		PDP.initTable(NetworkTable.getTable("PDP"));
-	}
-
-	public void run(Control c) {
-		lDriveM1.pidWrite( c.lDrive1);
-		lDriveM2.pidWrite( c.lDrive2);
-		rDriveM1.pidWrite(-c.rDrive1);
-		rDriveM2.pidWrite(-c.rDrive2);
-		armM.pidWrite(c.arm * 0.5);
-		fBallM.pidWrite(-c.fBall);
-		bBallM.pidWrite(-c.bBall);
-
-		SmartDashboard.putNumber("fBall", c.fBall);
-		SmartDashboard.putNumber("bBall", c.bBall);
-		SmartDashboard.putNumber("armM", c.arm);
-
-		SmartDashboard.putNumber("armE", armE.pidGet());
-		SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());
-		SmartDashboard.putNumber("lDriveE rate", lDriveE.getRate());
-		SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());
-		SmartDashboard.putNumber("rDriveE rate", rDriveE.getRate());
-		SmartDashboard.putBoolean("ballL", ballL.get());
-		SmartDashboard.putBoolean("armL", armL.get());
-
-		PDP.updateTable();
-		SmartDashboard.putData("PDP", PDP);
-	}
-}
+/**

+* Copyright (c) 2015-2017 Luke Shumaker.

+* Copyright (c) 2017 Cameron Richards.

+* Copyright (c) 2017 Noah Gaeta.

+* Copyright (c) 2017 Thomas Griffith.

+* 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.

+*/

+package org.usfirst.frc.team4272.robot2017;

+

+import edu.wpi.first.wpilibj.DigitalInput;

+import edu.wpi.first.wpilibj.DoubleSolenoid;

+import edu.wpi.first.wpilibj.DriverStation;

+import edu.wpi.first.wpilibj.Encoder;

+import edu.wpi.first.wpilibj.PIDOutput;

+import edu.wpi.first.wpilibj.PIDSource;

+import edu.wpi.first.wpilibj.PIDSourceType;

+import edu.wpi.first.wpilibj.PowerDistributionPanel;

+import edu.wpi.first.wpilibj.Talon;

+import edu.wpi.first.wpilibj.networktables.NetworkTable;

+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

+

+import org.usfirst.frc.team4272.robotlib.PIDOutputSplitter;

+import org.usfirst.frc.team4272.robotlib.PIDServo;

+import org.usfirst.frc.team4272.robotlib.RollingAvg;

+

+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.

+         */

+

+        // naming convention: {side_letter}{Thing}{E|M (encoder/motor)}

+

+        private final PIDOutput

+            rDriveM = new Talon(/*PWM*/0), /* PDP 2, 3 */

+            lDriveM = new Talon(/*PWM*/1), /* PDP 12, 13 */

+            climber = new Talon(/*PWM*/3), /* PDP 4 */

+            camRotate = new PIDServo(8),

+            camTilt = new PIDServo(9);

+        public final Encoder   lDriveE = new Encoder(/*DIO*/0,/*DIO*/1, /*reverse*/false);

+        public final Encoder   rDriveE = new Encoder(/*DIO*/2,/*DIO*/3, /*reverse*/true);

+        public final PIDSource lRate, rRate;

+        public final PowerDistributionPanel PDP = new PowerDistributionPanel(); /* via CAN */

+        public final DriverStation ds = DriverStation.getInstance();

+

+        public DoubleSolenoid

+                shifter = new DoubleSolenoid(/*PCM*/4, /*PCM*/5),

+                ged     = new DoubleSolenoid(/*PCM*/6, /*PCM*/7);

+        

+        private double maxRate = 0;

+        private double minBatt = 20;

+

+        public HwRobot() {

+                lDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);

+                rDriveE.setDistancePerPulse(10.0/*feet*/ / 1865.75/*pulses*/);

+

+                lDriveE.setPIDSourceType(PIDSourceType.kRate);

+                rDriveE.setPIDSourceType(PIDSourceType.kRate);

+                PDP.initTable(NetworkTable.getTable("PDP"));

+

+                lRate = new RollingAvg(5, lDriveE);

+                rRate = new RollingAvg(5, rDriveE);

+        }

+

+        public void run(Control c) {

+                lDriveM.pidWrite(c.lDrive);

+                rDriveM.pidWrite(-c.rDrive);

+                climber.pidWrite(c.climber);

+                camRotate.pidWrite(c.camRotate);

+                camTilt.pidWrite(c.camTilt);

+

+                shifter.set(c.highGear ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);

+                ged.set(c.gedOut ? DoubleSolenoid.Value.kReverse : DoubleSolenoid.Value.kForward);

+

+                double rate = Math.abs((lDriveE.getRate()+rDriveE.getRate())/2);

+                SmartDashboard.putNumber("lDriveE distance", lDriveE.getDistance());

+                SmartDashboard.putNumber("lDriveE rate", Math.abs(lDriveE.getRate()));

+                SmartDashboard.putNumber("rDriveE distance", rDriveE.getDistance());

+                SmartDashboard.putNumber("rDriveE rate", Math.abs(rDriveE.getRate()));

+                SmartDashboard.putNumber("rate", rate);

+                if (rate > maxRate)

+                        maxRate = rate;

+                SmartDashboard.putNumber("maxRate", maxRate);

+

+                double batt = ds.getBatteryVoltage();

+                SmartDashboard.putNumber("batt", batt);

+                if (batt < minBatt)

+                        minBatt = batt;

+                SmartDashboard.putNumber("minBatt", minBatt);

+

+                SmartDashboard.putBoolean("highGear", c.highGear);

+                SmartDashboard.putBoolean("gedOut", c.gedOut);

+

+                PDP.updateTable();

+                SmartDashboard.putData("PDP", PDP);

+        }

+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team4272/robot2016/Robot.java b/src/org/usfirst/frc/team4272/robot2016/Robot.java
index bb15ccc..a07a1c7 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Robot.java
@@ -1,61 +1,88 @@
-
-package org.usfirst.frc.team4272.robot2016;
-
-import edu.wpi.first.wpilibj.IterativeRobot;
-
-/**
- * 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 final HwRobot robot = new HwRobot();
-	private final HwOI oi = new HwOI();
-	private final Control control = new Control();
-
-	private Autonomous auto;
-	private Teleop teleop;
-	/**
-	 * 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() {
-		try {
-			auto = new Autonomous(robot);
-		} catch (Exception e) {}
-	}
-	public void autonomousPeriodic() {
-		try {
-			robot.run(auto.run(control));
-		} catch (Exception e) {}
-	}
-
-	public void teleopInit() {
-		try {
-			teleop = new Teleop(robot);
-		} catch (Exception e) {}
-	}
-
-	public void teleopPeriodic() {
-		try {
-			robot.run(teleop.run(control, oi));
-		} catch (Exception e) {}
-	}
-
-	public void disabledInit() {
-	}
-
-	public void disabledPeriodic() {
-	}
-
-	/**
-	 * This function is called periodically during test mode
-	 */
-	public void testPeriodic() {
-	}
-}
+/**

+* Copyright (c) 2010-2017 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.

+*/

+package org.usfirst.frc.team4272.robot2017;

+

+import edu.wpi.first.wpilibj.IterativeRobot;

+

+/**

+* 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 final HwRobot robot = new HwRobot();

+        private final HwOI oi = new HwOI();

+        private final Control control = new Control();

+

+        private Autonomous auto;

+        private Teleop teleop;

+

+        /**

+         * This function is run when the robot is first started up and should be

+         * used for any initialization code.

+         */

+        public void robotInit() {

+                Autonomous.robotInit();

+        }

+

+        public void autonomousInit() {

+                try {

+                        auto = new Autonomous(robot);

+                } catch (Exception e) {}

+        }

+        public void autonomousPeriodic() {

+                try {

+                        robot.run(auto.run(control));

+                } catch (Exception e) {}

+        }

+

+        public void teleopInit() {

+                try {

+                        teleop = new Teleop(robot);

+                } catch (Exception e) {}

+        }

+

+        public void teleopPeriodic() {

+                try {

+                        robot.run(teleop.run(control, oi));

+                } catch (Exception e) {}

+        }

+

+        public void disabledInit() {

+        }

+

+        public void disabledPeriodic() {

+        }

+

+        /**

+         * This function is called periodically during test mode

+         */

+        public void testPeriodic() {

+        }

+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team4272/robot2016/Teleop.java b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
index 73b7b60..e0b5119 100644
--- a/src/org/usfirst/frc/team4272/robot2016/Teleop.java
+++ b/src/org/usfirst/frc/team4272/robot2016/Teleop.java
@@ -1,94 +1,129 @@
-package org.usfirst.frc.team4272.robot2016;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.Timer;
-import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
-import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;
-import org.usfirst.frc.team4272.robotlib.PushButton;
-
-public class Teleop {
-	private final HwRobot robot;
-	private final double armSoftStop = 95;//295;
-
-	private final PushButton shootButton = new PushButton();
-	private final Timer time = new Timer();
-
-	public Teleop(HwRobot robot) {
-		this.robot = robot;
-		robot.armE.reset();
-	}
-
-	private static double jsScale(Joystick j) {
-		double y = -j.getY();/* +:forward; -:backward */
-		double z = -j.getZ();/* +:more-sensitive; -:less-sensitive */
-		return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
-	}
-
-	private static double bound(double min, double v, double max) {
-		return Math.min(Math.max(v, min), max);
-	}
-
-	private static double oneIf(boolean b) {
-		return b ? 1 : 0;
-	}
-
-	public Control run(Control control, HwOI oi) {
-		/* Drive */
-		control.lDrive1 = jsScale(oi.lStick);
-		control.rDrive1 = jsScale(oi.rStick);
-		control.lDrive2 = oi.lStick.getTrigger() ? control.lDrive1 : 0;
-		control.rDrive2 = oi.rStick.getTrigger() ? control.rDrive1 : 0;
-		control.arm = -oi.xbox.getAxis(Axis.LY);
-		if (control.arm < 0)
-			control.arm = control.arm*0.2;
-		
-		double fBall = oi.xbox.getAxis(Axis.RTrigger) - oi.xbox.getAxis(Axis.LTrigger);
-		double bBall = bound(-1, fBall, 0) + oneIf(oi.xbox.getButton(Button.A)) - oneIf(oi.xbox.getButton(Button.B));
-
-		boolean ballL = robot.ballL.get();
-		control.fBall = bound(-oneIf(ballL), fBall < 0 ? 0.5*fBall : fBall, 1);
-		control.bBall = bound(-oneIf(ballL), bBall < 0 ? 0.5*bBall : bBall, 1);
-
-		/* I'm eyeballing 10 degrees ≈ 50 clicks */
-		if (robot.armE.pidGet() < 50 && !robot.armL.get()) {
-			robot.armE.reset();
-		}
-
-		if (oi.xbox.getButton(Button.LBumper))
-			control.arm = 1-(robot.armE.pidGet()/armSoftStop);
-		else if (oi.xbox.getButton(Button.RBumper))
-			control.arm = robot.armE.pidGet()/armSoftStop;
-
-		if (shootButton.update(oi.xbox.getButton(Button.X))) {
-			time.reset();
-			time.start();
-		}
-		if (oi.xbox.getButton(Button.X)) {
-			if (time.get() < 0.5) {
-				control.fBall = control.bBall = -0.5;
-			} else if (time.get() < 2.5) {
-				control.bBall = -0.5;
-				control.fBall = 1;
-			} else {
-				control.bBall = 1;
-				control.fBall = 1;
-			}
-		}
-
-		//if (robot.armE.pidGet() > armSoftStop && control.arm > 0)
-		//	control.arm = 0;
-
-		/* Take pictures */
-		/*
-		if (camButton.update(oi.xbox.getButton(Button.RT))) {
-			try {
-				ProcessBuilder pb = new ProcessBuilder("/home/lvuser/tweeterbot/bin/snapPhoto");
-				pb.redirectErrorStream(true);
-				pb.start();
-			} catch (Exception e) {};
-		}
-		*/
-
-		return control;
-	}
-}
+/**

+* Copyright (c) 2015-2017 Luke Shumaker.

+* Copyright (c) 2017 Cameron Richards.

+* Copyright (c) 2017 Noah Gaeta.

+* Copyright (c) 2017 Thomas Griffith.

+* 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.

+*/

+package org.usfirst.frc.team4272.robot2017;

+

+import edu.wpi.first.wpilibj.GenericHID.Hand;

+import edu.wpi.first.wpilibj.Joystick;

+import edu.wpi.first.wpilibj.Timer;

+import edu.wpi.first.wpilibj.Preferences;

+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

+

+import org.usfirst.frc.team4272.robotlib.PushButton;

+

+public class Teleop {

+        private final HwRobot robot;

+        private final Timer timer = new Timer();

+        private final Preferences prefs = Preferences.getInstance();

+

+        private double lastTime;

+        private double currentTime;

+        private boolean state = false;

+

+        public Teleop(HwRobot robot) {

+                this.robot = robot;

+                timer.start();

+        }

+

+        private static double jsScale(Joystick j) {

+                double y = j.getY();/* +:forward; -:backward */

+                double z = j.getZ();/* +:more-sensitive; -:less-sensitive */

+                return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);

+        }

+

+        private static double bound(double min, double v, double max) {

+                return Math.min(Math.max(v, min), max);

+        }

+

+        private static double oneIf(boolean b) {

+                return b ? 1 : 0;

+        }

+        

+        public Control run(Control control, HwOI oi) {

+                /* drive */

+                control.lDrive = -jsScale(oi.lStick);

+                control.rDrive = -jsScale(oi.rStick);

+

+                /* shifting */

+                double shiftUp = prefs.getDouble("shiftUp", 3.3*3.28);

+                double shiftDown = prefs.getDouble("shiftDown", 2.7*3.28);

+                if (oi.rStick.getTrigger()) {

+                        control.highGear = true;

+                } else if (oi.lStick.getTrigger()) {

+                        control.highGear = false;

+                } else {

+                        double speed = Math.abs((robot.lRate.pidGet() + robot.rRate.pidGet()) / 2);

+                        if (!control.highGear) {

+                                if (speed > shiftUp)

+                                        control.highGear = true;

+                        } else {

+                                if (speed < shiftDown)

+                                        control.highGear = false;

+                        }

+                }

+

+                /* auto gear wiggle */

+                if (oi.rStick.getRawButton(2) || oi.lStick.getRawButton(2)) {

+                        if (timer.get() > 0.30) {

+                                timer.reset();

+                        }

+                        if (timer.get() < 0.15) {

+                                control.lDrive = 0.5;

+                                control.rDrive = 0;

+                        } else {

+                                control.lDrive = 0;

+                                control.rDrive = 0.5;

+                        }

+                }

+

+                /* climber */

+                control.climber = oi.xbox.getY(Hand.kLeft);

+                if (control.climber <= 0.2) {

+                        control.climber = 0;

+                }

+

+                /* GED */

+                control.gedOut = oi.xbox.getTriggerAxis(Hand.kRight) > 0.5;

+

+                /* camera */

+                // AnnaLaura wants the GED on the trigger, so we'll

+                // have to find somewhere else for the camera.

+                /*

+                if (oi.xbox.getTriggerAxis(Hand.kRight) > 0) {

+                        control.camRotate = 1;

+                } else if (oi.xbox.getTriggerAxis(Hand.kLeft) > 0) {

+                        control.camRotate = -1;

+                } else {

+                        control.camRotate = 0;

+                }

+                control.camTilt = oi.xbox.getX(Hand.kRight);

+                */

+                return control;

+        }

+}
\ No newline at end of file
diff --git a/src/org/usfirst/frc/team4272/robotlib/PIDController.java b/src/org/usfirst/frc/team4272/robotlib/PIDController.java
deleted file mode 100644
index 13f5666..0000000
--- a/src/org/usfirst/frc/team4272/robotlib/PIDController.java
+++ /dev/null
@@ -1,216 +0,0 @@
-/**
- * Copyright (c) FIRST 2008-2012. All Rights Reserved.
- * Open Source Software - may be modified and shared by FRC teams. The code
- * must be accompanied by the FIRST BSD license file in the root directory of
- * the project.
- *
- * Copyright (c) 2011-2012, 2015-2016 Luke Shumaker
- * All rights reserved.
- *
- * 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;
-
-/**
- * An enhanced variant of {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController}
- * (implements a PID Control Loop).
- * <p>
- * It is enhanced from
- * {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController}
- * in that it that also:
- * <ul>
- *  <li> implements SpeedController (and therefore PIDOutput)
- *  <li> disables self if the setpoint is NaN (see {@link PIDNanny}).
- * </ul>
- */
-public class PIDController extends edu.wpi.first.wpilibj.PIDController implements SpeedController {
-	private boolean NaNDisabled = false;
-	private boolean inverted = false;
-	public final PIDSource source;
-	public final PIDOutput output;
-
-	/**
-	 * Allocate a PID object with the given constants for P, I, D, and F
-	 *$
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param Kf the feed forward term
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 * @param period the loop time for doing calculations. This particularly
-	 *        effects calculations of the integral and differential terms. The
-	 *        default is 50ms.
-	 */
-	public PIDController(double Kp, double Ki, double Kd, double Kf,
-	                     PIDSource source, PIDOutput output,
-	                     double period) {
-		super(Kp, Ki, Kd, Kf, source, output, period);
-		this.source = source;
-		this.output = output;
-	}
-
-	/* Convenience constructors *******************************************/
-	/* `Kf`, and `period` are optional */
-
-	/**
-	 * Convenience constructor with {@code Kf} defaulting to
-	 * {@code 0.0}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 * @param period the loop time for doing calculations. This particularly
-	 *        effects calculations of the integral and differential terms. The
-	 *        default is 50ms.
-	 */
-	public PIDController(double Kp, double Ki, double Kd,
-	                     PIDSource source, PIDOutput output,
-	                     double period) {
-		this(Kp, Ki, Kd, 0.0, source, output, period);
-	}
-
-	/**
-	 * Convenience constructor with {@code period} defaulting to
-	 * {@link #kDefaultPeriod}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param Kf the feed forward term
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 */
-	public PIDController(double Kp, double Ki, double Kd, double Kf,
-	                     PIDSource source, PIDOutput output) {
-		this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
-	}
-
-	/**
-	 * Convenience constructor with {@code Kf} defaulting to
-	 * {@code 0.0}, and {@code period} defaulting to
-	 * {@link #kDefaultPeriod}.
-	 *
-	 * @param Kp the proportional coefficient
-	 * @param Ki the integral coefficient
-	 * @param Kd the derivative coefficient
-	 * @param source The PIDSource object that is used to get values
-	 * @param output The PIDOutput object that is set to the output percentage
-	 */
-	public PIDController(double Kp, double Ki, double Kd,
-	                     PIDSource source, PIDOutput output) {
-		this(Kp, Ki, Kd, 0.0, source, output, kDefaultPeriod);
-	}
-
-	/* Override to auto-disable if setpoint is NaN, and invert ************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void setSetpoint(double setpoint) {
-		if (Double.isNaN(setpoint)) {
-			NaNDisabled = true;
-			super.disable();
-		} else {
-			if (NaNDisabled && !isEnabled()) {
-				enable();
-			}
-			super.setSetpoint((inverted ? -1 : 1) * setpoint);
-		}
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void enable() {
-		NaNDisabled = false;
-		super.enable();
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public synchronized void disable() {
-		NaNDisabled = false;
-		super.disable();
-	}
-
-	/* Implement PIDOutput (a parent of SpeedController) *******************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void pidWrite(double output) {
-		setSetpoint(output);
-	}
-
-	/* Implement SpeedController *******************************************/
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void set(double output) {
-		setSetpoint(output);
-	}
-
-	/**
-	 * Don't use this.
-	 * @deprecated Don't use this; it is leaking up from
-	 *   {@link edu.wpi.first.wpilibj.CANJaguar} through
-	 *   {@link edu.wpi.first.wpilibj.SpeedController}
-	 */
-	@Override
-	public void set(double output, byte syncGroup) {
-		set(output);
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public void setInverted(boolean isInverted) {
-		inverted = isInverted;
-	}
-
-	/**
-	 * {@inheritDoc}
-	 */
-	@Override
-	public boolean getInverted() {
-		return inverted;
-	}
-}
diff --git a/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java b/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java
deleted file mode 100644
index b5c1d13..0000000
--- a/src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java
+++ /dev/null
@@ -1,299 +0,0 @@
-/**
- * Copyright (c) FIRST 2008-2012. All Rights Reserved.
- * Open Source Software - may be modified and shared by FRC teams. The code
- * must be accompanied by the FIRST BSD license file in the root directory of
- * the project.
- *
- * Copyright (c) 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.GenericHID;
-import edu.wpi.first.wpilibj.Joystick;
-
-/**
- * Handle input from a wired Xbox 360 controller connected to the
- * Driver Station.
- */
-public class Xbox360Controller extends GenericHID {
-	/* Constants ************************************************/
-
-	/**
-	 * Represents an analog axis on an Xbox 360 controller.
-	 */
-	public static enum Axis {
-		LX(0), LY(1), LTrigger(2),
-		RX(4), RY(5), RTrigger(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),
-		LBumper(4), RBumper( 5),
-		Back(6), Start(7), /*Home(8),*/
-		LThumb(8), RThumb(9);
-
-		public final int id;
-		private Button(int id) { this.id = id+1; }
-	}
-
-	/* Constructor **********************************************/
-	private final Joystick joystick;
-
-	/**
-	 * Construct an instance of a joystick.
-	 * The joystick index is the USB port on the Driver Station.
-	 *
-	 * @param port The port on the driver station that the joystick is plugged into
-	 */
-	public Xbox360Controller(final int port) {
-		joystick = new Joystick(port);
-	}
-
-	/* Core functions *******************************************/
-
-	/**
-	 * Get the raw axis
-	 *$
-	 * @param axis Index of the axis
-	 * @return The raw value of the selected axis
-	 */
-	@Override
-	public double getRawAxis(int axis) {
-		return joystick.getRawAxis(axis);
-	}
-
-	/**
-	 * Is the given button pressed
-	 *$
-	 * @param button Index of the button
-	 * @return True if the button is pressed
-	 */
-	@Override
-	public boolean getRawButton(int button) {
-		return joystick.getRawButton(button);
-	}
-
-	/**
-	 * 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(final 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(final Button button) {
-		return getRawButton(button.id);
-	}
-
-	/**
-	 * Ask the Driver Station if this USB joystick is in fact an
-	 * Xbox controller.
-	 *
-	 * @return Whether the controller is an Xbox controller
-	 */
-	public boolean getIsXbox() {
-		return joystick.getIsXbox();
-	}
-
-	/* TODO: Outputs? (Rumble, LEDs) */
-
-	/* Stupid little mathy wrappers *****************************/
-
-	/**
-	 * Get the magnitude of the direction vector formed by the thumb-stick's
-	 * current position relative to its origin
-	 *
-	 * @param hand Left stick or right?
-	 * @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));
-	}
-
-	/**
-	 * Get the direction of the vector formed by the thumb-stick and its origin
-	 * in radians
-	 *
-	 * @param hand Left stick or right?
-	 * @return The direction of the vector in radians
-	 */
-	public double getDirectionRadians(Hand hand) {
-		return Math.atan2(getX(hand), -getY(hand));
-	}
-
-	/**
-	 * 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++
-	 *
-	 * @param hand Left stick or right?
-	 * @return The direction of the vector in degrees
-	 */
-	public double getDirectionDegrees(Hand hand) {
-		return Math.toDegrees(getDirectionRadians(hand));
-	}
-
-	/* Stupid aliases for getAxis/getButton *********************/
-
-	/**
-	 * Get the state of a trigger; whether it is mostly pressed or
-	 * not.
-	 *
-	 * @param hand Left trigger or right?
-	 * @return The state of the trigger.
-	 */
-	@Override
-	public boolean getTrigger(Hand hand) {
-		return getZ(hand) > 0.75;
-	}
-
-	/**
-	 * Get the state of a bumper.
-	 *
-	 * @param hand Left bumper or right?
-	 * @return Whether the bumper is pressed
-	 * @deprecated This method is only here to complete the {@link GenericHID} abstract class.
-	 */
-	@Override
-	public boolean getBumper(Hand hand) {
-		if (hand.value == Hand.kLeft.value)
-			return getButton(Button.LBumper);
-		if (hand.value == Hand.kRight.value)
-			return getButton(Button.RBumper);
-		return false;
-	}
-
-	/**
-	 * Get the state of a thumb-stick button.
-	 *
-	 * @param hand Left trigger or right?
-	 * @return the state of the button.
-	 */
-	@Override
-	public boolean getTop(Hand hand) {
-		if (hand.value == Hand.kLeft.value)
-			return getButton(Button.LThumb);
-		if (hand.value == Hand.kRight.value)
-			return getButton(Button.RThumb);
-		return false;
-	}
-
-	/**
-	 * Get the X value of a thumb-stick.
-	 *
-	 * @param hand Left stick or right?
-	 * @return The X value of the thumb-stick.
-	 */
-	@Override
-	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 thumb-stick.
-	 */
-	@Override
-	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.
-	 */
-	@Override
-	public double getZ(final Hand hand) {
-		if (hand.value == Hand.kLeft.value)
-			return getAxis(Axis.LTrigger);
-		if (hand.value == Hand.kRight.value)
-			return getAxis(Axis.RTrigger);
-		return 0.0;
-	}
-
-	/* Stupid things I have to implement ************************/
-
-	/**
-	 * @return Always 0.0
-	 * @deprecated This method is only here to complete the {@link GenericHID} abstract class.
-	 */
-	@Override
-	public double getTwist() {
-		return 0.0;
-	}
-
-	/**
-	 * @return Always 0.0
-	 * @deprecated This method is only here to complete the {@link GenericHID} abstract class.
-	 */
-	@Override
-	public double getThrottle() {
-		return 0.0;
-	}
-
-	/**
-	 * @param pov Unused
-	 * @return Always 0
-	 * @deprecated This method is only here to complete the {@link GenericHID} abstract class.
-	 */
-	@Override
-	public int getPOV(int pov) {
-		return 0;
-	}
-}