From 4e2b11e4df2f724a0e2d66c28bc465126a1c9d37 Mon Sep 17 00:00:00 2001 From: Deveveloper Date: Thu, 23 Mar 2017 01:47:29 -0400 Subject: Changes sitting on netbook --- .../usfirst/frc/team4272/robot2016/Autonomous.java | 33 --- .../usfirst/frc/team4272/robot2016/Autonomus.java | 243 +++++++++++++++++ .../usfirst/frc/team4272/robot2016/Control.java | 23 +- src/org/usfirst/frc/team4272/robot2016/HwOI.java | 22 +- .../usfirst/frc/team4272/robot2016/HwRobot.java | 215 +++++++++------ src/org/usfirst/frc/team4272/robot2016/Robot.java | 149 +++++----- src/org/usfirst/frc/team4272/robot2016/Teleop.java | 223 ++++++++------- .../frc/team4272/robotlib/PIDController.java | 216 --------------- .../frc/team4272/robotlib/Xbox360Controller.java | 299 --------------------- 9 files changed, 612 insertions(+), 811 deletions(-) delete mode 100644 src/org/usfirst/frc/team4272/robot2016/Autonomous.java create mode 100644 src/org/usfirst/frc/team4272/robot2016/Autonomus.java delete mode 100644 src/org/usfirst/frc/team4272/robotlib/PIDController.java delete mode 100644 src/org/usfirst/frc/team4272/robotlib/Xbox360Controller.java 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 - */ -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). - *

- * It is enhanced from - * {@link edu.wpi.first.wpilibj.PIDController wpilibj PIDController} - * in that it that also: - *

- */ -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 - */ -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; - } -} -- cgit v1.2.3-54-g00ecf