diff --git a/src/org/usfirst/frc/team4737/lib/Gamepad.java b/src/org/usfirst/frc/team4737/lib/Gamepad.java new file mode 100644 index 0000000..495f93b --- /dev/null +++ b/src/org/usfirst/frc/team4737/lib/Gamepad.java @@ -0,0 +1,329 @@ +package org.usfirst.frc.team4737.lib; + +import java.util.HashMap; +import java.util.Map; + +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.buttons.Button; +import edu.wpi.first.wpilibj.buttons.JoystickButton; + +public abstract class Gamepad { + + public class GamepadButton extends JoystickButton { + + private String name; + + public GamepadButton(Gamepad gamepad, String name, int buttonNumber) { + super(gamepad.gamepad, buttonNumber); + + this.name = name; + + gamepad.registerButton(this, name); + } + + public String getNameID() { + return name; + } + + } + + private abstract class Axis { + + protected final Gamepad gamepad; + + protected String name; + + public Axis(Gamepad gamepad, String name) { + this.gamepad = gamepad; + + this.name = name; + + gamepad.registerAxis(this, name); + } + + public abstract double get(); + + public String getName() { + return name; + } + + } + + public class GamepadAxis extends Axis { + + private final int axis; + + private double deadzone; + + public GamepadAxis(Gamepad gamepad, String name, int axis) { + super(gamepad, name); + this.axis = axis; + + gamepad.registerAxis(this, name); + } + + public void setDeadzone(double radius) { + if (radius < 0) + throw new IllegalArgumentException("Deadzone cannot be less than 0."); + this.deadzone = radius; + } + + public double getRaw() { + return gamepad.gamepad.getRawAxis(axis); + } + + @Override + public double get() { + double raw = getRaw(); + if (raw < -deadzone) { + return scale(raw, -1, -deadzone, -1, 0); + } else if (raw > deadzone) { + return scale(raw, deadzone, 1, 0, 1); + } else { + return 0; + } + } + + private double scale(double val, double valLow, double valHigh, double newLow, double newHigh) { + double reduced = (val - valLow) / (valHigh - valLow); + return reduced * (newHigh - newLow) + newLow; + } + + } + + public class DPad { + + private class DPadButton extends Button { + + private final DPad dpad; + private final int degree; + + private String name; + + public DPadButton(DPad dpad, String name, int degree) { + this.dpad = dpad; + this.degree = degree; + + this.name = name; + + dpad.gamepad.registerButton(this, name); + } + + @Override + public boolean get() { + return gamepad.gamepad.getPOV(dpad.id) == degree; + } + + public String getNameID() { + return name; + } + + } + + private class DPadAxis extends Axis { + + private DPadButton[] negative; + private DPadButton[] positive; + + public DPadAxis(Gamepad gamepad, String name, DPadButton[] negative, DPadButton[] positive) { + super(gamepad, name); + this.negative = negative; + this.positive = positive; + } + + @Override + public double get() { + boolean neg = false; + for (int i = 0; i < negative.length; i++) { + if (negative[i].get()) { + neg = true; + break; + } + } + boolean pos = false; + for (int i = 0; i < positive.length; i++) { + if (positive[i].get()) { + pos = true; + break; + } + } + + if (neg && pos) + return 0; + else if (neg) + return -1; + else if (pos) + return 1; + else + return 0; + } + + } + + private final Gamepad gamepad; + private final int id; + + private String name; + + public final DPadButton UP; + public final DPadButton UP_RIGHT; + public final DPadButton RIGHT; + public final DPadButton DOWN_RIGHT; + public final DPadButton DOWN; + public final DPadButton DOWN_LEFT; + public final DPadButton LEFT; + public final DPadButton UP_LEFT; + public final DPadAxis X; + public final DPadAxis Y; + + public DPad(Gamepad gamepad, String name, int id) { + this.gamepad = gamepad; + this.id = id; + + this.name = name; + + UP = new DPadButton(this, name + "_UP", 0); + UP_RIGHT = new DPadButton(this, name + "_UP_RIGHT", 45); + RIGHT = new DPadButton(this, name + "_RIGHT", 90); + DOWN_RIGHT = new DPadButton(this, name + "_DOWN_RIGHT", 135); + DOWN = new DPadButton(this, name + "_DOWN", 180); + DOWN_LEFT = new DPadButton(this, name + "_DOWN_LEFT", 225); + LEFT = new DPadButton(this, name + "_LEFT", 270); + UP_LEFT = new DPadButton(this, name + "_UP_LEFT", 315); + X = new DPadAxis(gamepad, name + "_X", new DPadButton[] { LEFT, DOWN_LEFT, UP_LEFT }, + new DPadButton[] { RIGHT, DOWN_RIGHT, UP_RIGHT }); + Y = new DPadAxis(gamepad, name + "_Y", new DPadButton[] { DOWN, DOWN_LEFT, DOWN_RIGHT }, + new DPadButton[] { UP, UP_LEFT, UP_RIGHT }); + } + + public int getDegree() { + return gamepad.gamepad.getPOV(id); + } + + public String getName() { + return name; + } + + } + + public class Thumbstick { + + public final GamepadAxis X, Y; + + private String name; + + public Thumbstick(Gamepad gamepad, String name, int axisX, int axisY) { + X = new GamepadAxis(gamepad, name + "_X", axisX); + Y = new GamepadAxis(gamepad, name + "_Y", axisY); + + this.name = name; + + gamepad.registerThumbstick(this, name); + } + + public String getName() { + return name; + } + + } + + public final Joystick gamepad; + + private String name; + + private Map buttonMap; + private Map axisMap; + private Map thumbstickMap; + private Map dpadMap; + + public Gamepad(int usbPort, String name) { + gamepad = new Joystick(usbPort); + + this.name = name; + + buttonMap = new HashMap<>(); + axisMap = new HashMap<>(); + thumbstickMap = new HashMap<>(); + dpadMap = new HashMap<>(); + } + + protected void registerButton(Button button, String name) { + buttonMap.put(name.toLowerCase(), button); + } + + protected void registerAxis(Axis axis, String name) { + axisMap.put(name.toLowerCase(), axis); + } + + protected void registerThumbstick(Thumbstick thumbstick, String name) { + thumbstickMap.put(name.toLowerCase(), thumbstick); + } + + protected void registerDPad(DPad dpad, String name) { + dpadMap.put(name.toLowerCase(), dpad); + } + + public void setRumble(double rumble) { + this.setRumble(rumble, rumble); + } + + public void setRumble(double left, double right) { + gamepad.setRumble(RumbleType.kLeftRumble, left); + gamepad.setRumble(RumbleType.kRightRumble, right); + } + + public String getName() { + return name; + } + + /** + * Enables retrieving buttons by name without knowing which controller is being + * used. + * + * @param name + * - The name of the button + * @return Returns a button mapped with the given name + */ + public Button getButton(String name) { + return buttonMap.get(name); + } + + /** + * Enables retrieving axes by name without knowing which controller is being + * used. + * + * @param name + * - The name of the axis + * @return Returns an axis mapped with the given name + */ + public Axis getAxis(String name) { + return axisMap.get(name); + } + + /** + * Enables retrieving thumbsticks by name without knowing which controller is + * being used. + * + * @param name + * - The name of the thumbstick (usually just "LS" or + * "RS") + * @return Returns a thumbstick mapped with the given name + */ + public Thumbstick getThumbstick(String name) { + return thumbstickMap.get(name); + } + + /** + * Enables retrieving POV DPads by name without knowing which controller is + * being used. + * + * @param name + * - The name of the DPad (usually just "DPAD") + * @return Returns a button mapped with the given name + */ + public DPad getDPad(String name) { + return dpadMap.get(name); + } + +} diff --git a/src/org/usfirst/frc/team4737/lib/LogitechController.java b/src/org/usfirst/frc/team4737/lib/LogitechController.java deleted file mode 100644 index bc02705..0000000 --- a/src/org/usfirst/frc/team4737/lib/LogitechController.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.usfirst.frc.team4737.lib; - -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.buttons.JoystickButton; - -public class LogitechController { - - public class Axis { - private final Joystick controller; - private final int axis; - - private double deadzone; - - public Axis(Joystick controller, int axis) { - this.controller = controller; - this.axis = axis; - } - - public void setDeadzone(double radius) { - if (radius < 0) throw new IllegalArgumentException("Deadzone cannot be less than 0."); - this.deadzone = radius; - } - - public double getRaw() { - return controller.getRawAxis(axis); - } - - public double get() { - double raw = getRaw(); - if (raw < -deadzone) { - return scale(raw, -1, -deadzone, -1, 0); - } else if (raw > deadzone) { - return scale(raw, deadzone, 1, 0, 1); - } else { - return 0; - } - } - - private double scale(double val, double valLow, double valHigh, double newLow, double newHigh) { - double reduced = (val - valLow) / (valHigh - valLow); - return reduced * (newHigh - newLow) + newLow; - } - } - - public class Thumbstick { - public final Axis X, Y; - - public Thumbstick(Joystick controller, int axisX, int axisY) { - X = new Axis(controller, axisX); - Y = new Axis(controller, axisY); - } - } - - public final Joystick controller; - public final JoystickButton A; - public final JoystickButton B; - public final JoystickButton X; - public final JoystickButton Y; - public final JoystickButton RB; - public final JoystickButton LB; - public final Thumbstick LS; - public final Thumbstick RS; - public final Axis LT; - public final Axis RT; - - public LogitechController(int usbPort) { - controller = new Joystick(usbPort); - A = new JoystickButton(controller, -1); // TODO map the correct button IDs - B = new JoystickButton(controller, -1); - X = new JoystickButton(controller, -1); - Y = new JoystickButton(controller, -1); - RB = new JoystickButton(controller, -1); - LB = new JoystickButton(controller, -1); - LS = new Thumbstick(controller, -1, -1); - RS = new Thumbstick(controller, -1, -1); - LT = new Axis(controller, -1); - RT = new Axis(controller, -1); - } - - // TODO add rumble if possible - -} diff --git a/src/org/usfirst/frc/team4737/lib/LogitechGamepad.java b/src/org/usfirst/frc/team4737/lib/LogitechGamepad.java new file mode 100644 index 0000000..335001a --- /dev/null +++ b/src/org/usfirst/frc/team4737/lib/LogitechGamepad.java @@ -0,0 +1,72 @@ +package org.usfirst.frc.team4737.lib; + +public class LogitechGamepad extends Gamepad { + + private class LogitechGamepadTriggerAxis extends GamepadAxis { + + private final boolean positive; + + public LogitechGamepadTriggerAxis(Gamepad gamepad, String name, int axis, boolean positive) { + super(gamepad, name, axis); + + this.positive = positive; + } + + @Override + public double get() { + double value = super.get(); + if (value > 0 && positive) + return value; + else if (value < 0 && !positive) + return -value; + else + return 0; + } + + } + + public final GamepadButton A; + public final GamepadButton B; + public final GamepadButton X; + public final GamepadButton Y; + public final GamepadButton LB; + public final GamepadButton RB; + public final GamepadButton BACK; + public final GamepadButton START; + public final GamepadButton L3; + public final GamepadButton R3; + + public final Thumbstick LS; + public final Thumbstick RS; + + public final LogitechGamepadTriggerAxis LT; + public final LogitechGamepadTriggerAxis RT; + + public final DPad DPAD; + + public LogitechGamepad(int usbPort) { + super(usbPort, "LogitechGamepad"); + A = new GamepadButton(this, "A", 1); + B = new GamepadButton(this, "B", 2); + X = new GamepadButton(this, "X", 3); + Y = new GamepadButton(this, "Y", 4); + LB = new GamepadButton(this, "LB", 5); + RB = new GamepadButton(this, "RB", 6); + BACK = new GamepadButton(this, "BACK", 7); + registerButton(BACK, "SELECT"); + START = new GamepadButton(this, "START", 8); + L3 = new GamepadButton(this, "L3", 9); + registerButton(L3, "LS"); + R3 = new GamepadButton(this, "R3", 10); + registerButton(R3, "RS"); + + LS = new Thumbstick(this, "LS", 1, 2); + RS = new Thumbstick(this, "RS", 4, 5); + + LT = new LogitechGamepadTriggerAxis(this, "LT", 3, true); + RT = new LogitechGamepadTriggerAxis(this, "RT", 3, false); + + DPAD = new DPad(this, "DPAD", 0); + } + +} diff --git a/src/org/usfirst/frc/team4737/lib/XboxController.java b/src/org/usfirst/frc/team4737/lib/XboxController.java new file mode 100644 index 0000000..00ee6e9 --- /dev/null +++ b/src/org/usfirst/frc/team4737/lib/XboxController.java @@ -0,0 +1,49 @@ +package org.usfirst.frc.team4737.lib; + +public class XboxController extends Gamepad { + + public final GamepadButton A; + public final GamepadButton B; + public final GamepadButton X; + public final GamepadButton Y; + public final GamepadButton LB; + public final GamepadButton RB; + public final GamepadButton SELECT; + public final GamepadButton START; + public final GamepadButton L3; + public final GamepadButton R3; + + public final Thumbstick LS; + public final Thumbstick RS; + + public final GamepadAxis LT; + public final GamepadAxis RT; + + public final DPad DPAD; + + public XboxController(int usbPort) { + super(usbPort, "XboxController"); + A = new GamepadButton(this, "A", 1); + B = new GamepadButton(this, "B", 2); + X = new GamepadButton(this, "X", 3); + Y = new GamepadButton(this, "Y", 4); + LB = new GamepadButton(this, "LB", 5); + RB = new GamepadButton(this, "RB", 6); + SELECT = new GamepadButton(this, "SELECT", 7); + registerButton(SELECT, "BACK"); + START = new GamepadButton(this, "START", 8); + L3 = new GamepadButton(this, "L3", 9); + registerButton(L3, "LS"); + R3 = new GamepadButton(this, "R3", 10); + registerButton(R3, "RS"); + + LS = new Thumbstick(this, "LS", 0, 1); + RS = new Thumbstick(this, "RS", 4, 5); + + LT = new GamepadAxis(this, "LT", 2); + RT = new GamepadAxis(this, "RT", 3); + + DPAD = new DPad(this, "DPAD", 0); + } + +} diff --git a/src/org/usfirst/frc/team4737/robot/OI.java b/src/org/usfirst/frc/team4737/robot/OI.java index 52cfda5..27eec7e 100644 --- a/src/org/usfirst/frc/team4737/robot/OI.java +++ b/src/org/usfirst/frc/team4737/robot/OI.java @@ -7,7 +7,8 @@ package org.usfirst.frc.team4737.robot; -import org.usfirst.frc.team4737.lib.LogitechController; +import org.usfirst.frc.team4737.lib.Gamepad; +import org.usfirst.frc.team4737.lib.LogitechGamepad; import org.usfirst.frc.team4737.robot.commands.ReverseIntake; import org.usfirst.frc.team4737.robot.commands.RunIntake; @@ -45,13 +46,13 @@ public class OI { // until it is finished as determined by it's isFinished method. // button.whenReleased(new ExampleCommand()); - public LogitechController controller; + public Gamepad controller; public OI() { - controller = new LogitechController(0); + controller = new LogitechGamepad(0); - controller.A.whileHeld(new RunIntake()); - controller.B.whileHeld(new ReverseIntake()); + controller.getButton("A").whileHeld(new RunIntake()); + controller.getButton("B").whileHeld(new ReverseIntake()); } diff --git a/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java b/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java index 81be37e..7f9721a 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java +++ b/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java @@ -21,8 +21,8 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.DRIVETRAIN.tankDrive( - Robot.OI.controller.LS.Y.get(), - Robot.OI.controller.RS.Y.get()); + Robot.OI.controller.getThumbstick("LS").Y.get(), + Robot.OI.controller.getThumbstick("RS").Y.get()); } // Make this return true when this Command no longer needs to run execute()