summaryrefslogtreecommitdiff
path: root/src/org/usfirst
diff options
context:
space:
mode:
Diffstat (limited to 'src/org/usfirst')
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Autonomous.java33
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Autonomus.java243
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Control.java23
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/HwOI.java22
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/HwRobot.java215
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Robot.java149
-rw-r--r--src/org/usfirst/frc/team4272/robot2016/Teleop.java223
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/PIDController.java216
-rw-r--r--src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java299
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;
- }
-}