diff --git a/src/main/java/common/core/NAR_Robot.java b/src/main/java/common/core/NAR_Robot.java index 01b999f..8e6ff2a 100644 --- a/src/main/java/common/core/NAR_Robot.java +++ b/src/main/java/common/core/NAR_Robot.java @@ -19,7 +19,7 @@ * @author Mason Lam */ public class NAR_Robot extends IterativeRobotBase { - @SuppressWarnings("MemberName") + @SuppressWarnings("MemberName") static class Callback implements Comparable { public Runnable func; public double period; diff --git a/src/main/java/common/core/subsystems/NAR_StateSpaceSubsystem.java b/src/main/java/common/core/subsystems/NAR_StateSpaceSubsystem.java deleted file mode 100644 index 6ce15f4..0000000 --- a/src/main/java/common/core/subsystems/NAR_StateSpaceSubsystem.java +++ /dev/null @@ -1,172 +0,0 @@ -package common.core.subsystems; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.system.LinearSystemLoop; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -/** - * Team 3128's State Space control class. - * @since 2023 CHARGED UP - * @author Teja Yaramada, Mason Lam - */ -@SuppressWarnings({"rawtypes", "unchecked"}) -public abstract class NAR_StateSpaceSubsystem extends SubsystemBase{ - - public enum System { - POSITION, - VELOCITY - } - - protected final LinearSystemLoop loop; - protected final TrapezoidProfile.Constraints constraints; - private TrapezoidProfile.State lastProfiledReference; - private System system; - protected boolean m_enabled; - private double tolerance; - private double setpoint; - - /** - * Create a new NAR_StateSpaceSubsystem object - * - * @param loop LinearSystemLoop representing the physical subsystem - * @param constraints Trapezoid constraints with max velocity and accelaration - * @param system Type of Linear System setpoint: (Position/Velocity) - */ - public NAR_StateSpaceSubsystem(LinearSystemLoop loop, TrapezoidProfile.Constraints constraints, System system) { - this.loop = loop; - this.constraints = constraints; - this.system = system; - reset(); - } - - /** - * Create a new NAR_StateSpaceSubsystem object - * - * @param loop LinearSystemLoop representing the physical subsystem - * @param system Type of Linear System (Velocity/Position) - */ - public NAR_StateSpaceSubsystem(LinearSystemLoop loop, System system) { - this(loop, new TrapezoidProfile.Constraints(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY), system); - } - - /** - * Sets the setpoint for the subsystem. - * - * @param setpoint the setpoint for the subsystem - */ - public void startSSC(double setpoint) { - this.setpoint = setpoint; - enable(); - } - - /** Enables State Space control. Resets the controller. */ - public void enable(){ - reset(); - m_enabled = true; - } - - /** Disables State Space control. Sets output to zero. */ - public void disable(){ - m_enabled = false; - useOutput(0); - } - - /** - * Sets the setpoint for the subsystem. - * - * @param setpoint the setpoint for the subsystem - */ - public void setSetpoint(double setpoint){ - this.setpoint = setpoint; - } - - /** - * Gets the setpoint for the subsystem. - * - * @return the setpoint for the subsystem - */ - public double getSetpoint() { - return setpoint; - } - - /** Set the tolerance for the subsystem */ - public void setTolerance(double tolerance){ - this.tolerance = tolerance; - } - - /** - * Returns whether the subsystem is at setpoint - * - * @return true if at setpoint and false if not - */ - public boolean atSetpoint() { - double measurement = system == System.POSITION ? setpoint - getPosition() : setpoint - getVelocity(); - return Math.abs(measurement) < tolerance; - } - - @Override - public void periodic() { - if(m_enabled){ - double output = calculate(setpoint); - useOutput(output); - } - } - - /** - * Uses the output from the LinearSystemLoop. - * - * @param output the output of the LinearSystemLoop - */ - protected abstract void useOutput(double output); - - /** - * Returns the current position of the linear system. - * Not needed if the system type is velocity based. - */ - protected abstract double getPosition(); - - /** - * Returns the current velocity of the linear system - */ - protected abstract double getVelocity(); - - /** - * Calculates the desired output voltage for the motor(s) - * @param desiredSetpoint the setpoint for the linearSystem - * @return output in voltage - */ - public double calculate(double desiredSetpoint) { - if (system == System.POSITION) { - //Goal represented by the end position and a velocity of 0 - TrapezoidProfile.State setpoint = new TrapezoidProfile.State(desiredSetpoint, 0.0); - //Estimate the position and velocity of the system using the previous state 20ms ago - lastProfiledReference = (new TrapezoidProfile(constraints, setpoint, lastProfiledReference)).calculate(0.020); - loop.setNextR(lastProfiledReference.position, lastProfiledReference.velocity); - } - else { - //Set the desired velocity - loop.setNextR(desiredSetpoint); - } - //Update the LinearSystemLoop with encoder measurement - loop.correct(VecBuilder.fill(system == System.POSITION ? getPosition() : getVelocity())); - //Predict the state of the linear system 20ms into the future - loop.predict(0.020); - //Get the voltage applied to the motors in the linear system in the model - double nextVoltage = loop.getU(0); - return MathUtil.clamp(nextVoltage, -12.0, 12.0); - } - - /** - * Resets the linear system to the current position/velocity. - */ - protected void reset(){ - if (system == System.POSITION) { - loop.reset(VecBuilder.fill(getPosition(), getVelocity())); - lastProfiledReference = new TrapezoidProfile.State(getPosition(), getVelocity()); - } - if (system == System.VELOCITY) { - loop.reset(VecBuilder.fill(getVelocity())); - } - } -} \ No newline at end of file diff --git a/src/main/java/common/hardware/input/NAR_ButtonBoard.java b/src/main/java/common/hardware/input/NAR_ButtonBoard.java index 0b6f384..c36ad59 100644 --- a/src/main/java/common/hardware/input/NAR_ButtonBoard.java +++ b/src/main/java/common/hardware/input/NAR_ButtonBoard.java @@ -1,54 +1,46 @@ package common.hardware.input; -import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * Wrapper for the WPILib Joystick class. Works with device that can be recognized as joystick as driverstation. + * Custom GenericHID class for team 3128's button board setup * @since 2023 Charged UP - * @author Peter Ma, Arav Chadha + * @author Peter Ma, Arav Chadha, Mason Lam */ public class NAR_ButtonBoard { - private Joystick stick; + private GenericHID device; private Trigger[] buttons; - public NAR_ButtonBoard(int deviceNumber) { + /** + * Creates a new button board object. + * @param port The port on DriverStation the button board is connected to. + */ + public NAR_ButtonBoard(int port) { buttons = new Trigger[16]; - stick = new Joystick(deviceNumber); - - // 2023 btnboard has 12 buttons & 4 axis direction + device = new GenericHID(port); + // 2023 buttonBoard has 12 buttons & 4 axis direction for (int i = 0; i < 12; i++) { int buttonId = i; - buttons[buttonId] = new Trigger(() -> stick.getRawButton(buttonId + 1)); + buttons[buttonId] = new Trigger(() -> device.getRawButton(buttonId + 1)); } - buttons[12] = new Trigger(() -> stick.getX() == -1.0); - buttons[13] = new Trigger(() -> stick.getX() == 1.0); - buttons[14] = new Trigger(() -> stick.getY() == 1.0); - buttons[15] = new Trigger(() -> stick.getY() == -1.0); + buttons[12] = new Trigger(() -> device.getRawAxis(0) == -1.0); + buttons[13] = new Trigger(() -> device.getRawAxis(0) == 1.0); + buttons[14] = new Trigger(() -> device.getRawAxis(1) == 1.0); + buttons[15] = new Trigger(() -> device.getRawAxis(1) == -1.0); } + /** + * Returns a button on the board + * @param i The ID of the button + * @return Trigger to store the command to run + */ public Trigger getButton(int i) { return buttons[i-1]; } - - public Trigger getPosXButton() { - return buttons[12]; - } - - public Trigger getNegXButton() { - return buttons[13]; - } - - public Trigger getPosYButton() { - return buttons[14]; - } - - public Trigger getNegYButton() { - return buttons[15]; - } } \ No newline at end of file diff --git a/src/main/java/common/hardware/input/NAR_Joystick.java b/src/main/java/common/hardware/input/NAR_Joystick.java index c63952b..1ceb416 100644 --- a/src/main/java/common/hardware/input/NAR_Joystick.java +++ b/src/main/java/common/hardware/input/NAR_Joystick.java @@ -14,10 +14,6 @@ public class NAR_Joystick { private final Trigger[] buttons; - /** - * POV convention: 0 = up, 45 = top right, 90 = right, 135 = buttom right, 180 = down, 225 = bottom left, 270 = left, 315 = top left - * We assign indices as angle / 45 [0,7] - */ private final Trigger[] povButtons; private double xDeadband = 0.05; @@ -28,12 +24,12 @@ public class NAR_Joystick { /** * Creates a new NAR_Joystick object - * @param deviceNumber The port the joystick is connected on driver station + * @param port The port the joystick is connected on driver station */ - public NAR_Joystick(int deviceNumber) { + public NAR_Joystick(int port) { buttons = new Trigger[16]; povButtons = new Trigger[8]; - stick = new Joystick(deviceNumber); + stick = new Joystick(port); // Thrustmaster joystick has 16 buttons @@ -82,6 +78,16 @@ public double getThrottle() { return mappedThrottle; } + /** + * Sets the throttle bounds for the joystick. + * @param lowerBound [0, 1] lowest possible throttle value. + * @param upperBound [0, 1] highest possible throttle value. + */ + public void setThrottleBounds(double lowerBound, double upperBound) { + throttleLowerBound = lowerBound; + throttleUpperBound = upperBound; + } + /** * Returns the button of the Joystick * @param i The ID of the button diff --git a/src/main/java/common/hardware/input/NAR_XboxController.java b/src/main/java/common/hardware/input/NAR_XboxController.java index d207d56..885cb2f 100644 --- a/src/main/java/common/hardware/input/NAR_XboxController.java +++ b/src/main/java/common/hardware/input/NAR_XboxController.java @@ -6,112 +6,182 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; /** - * Wrapper for the WPILib XboxController class. - * To adjust for specific controller, change array for button names. + * Wrapper for the WPILib XboxController class. * @since 2022 Rapid React - * @author Arav Chadha + * @author Arav Chadha, Mason Lam */ - public class NAR_XboxController extends XboxController { - private String buttonNames[] = { - "A", - "B", - "X", - "Y", - "LeftBumper", - "RightBumper", - "Back", - "Start", - "LeftStick", - "RightStick" - }; - - private double leftXDeadband = 0.1; - private double leftYDeadband = 0.1; - private double rightXDeadband = 0.1; - private double rightYDeadband = 0.1; + private double leftXDeadband = 0.05; + private double leftYDeadband = 0.05; + private double rightXDeadband = 0.05; + private double rightYDeadband = 0.05; private HashMap buttons; private Trigger[] povButtons; + /** + * Creates an XboxController object + * @param port Port on driver station for controller + */ public NAR_XboxController(int port) { super(port); buttons = new HashMap(); + for (final Button button : Button.values()) { + buttons.put(button.toString(), new Trigger(()-> getRawButton(button.value))); + } + buttons.put("RightTrigger", new Trigger(()-> getRightTriggerAxis() >= 0.5)); + buttons.put("LeftTrigger", new Trigger(()-> getLeftTriggerAxis() >= 0.5)); + povButtons = new Trigger[8]; - for (int i = 0; i < 10; i++) { - int n = i + 1; - buttons.put(buttonNames[i], new Trigger (() -> getRawButton(n))); - } for (int i = 0; i < 8; i++) { - int n = i; + final int n = i; povButtons[i] = new Trigger (() -> getPOV() == n * 45); } - buttons.put("RightTrigger", new Trigger(()-> getRightTriggerAxis() >= 0.5)); - buttons.put("LeftTrigger", new Trigger(()-> getLeftTriggerAxis() >= 0.5)); - buttons.put("LeftPosY", new Trigger(()-> getLeftY() >= 0.5)); - buttons.put("LeftNegY", new Trigger(()-> getLeftY() <= -0.5)); - buttons.put("RightPosY", new Trigger(()-> getRightY() >= 0.5)); - buttons.put("RightNegY", new Trigger(()-> getRightY() <= -0.5)); } + /** + * Returns the XboxController Button. + * @param buttonName Name of the button. + * @return Trigger object to add commands to. + */ public Trigger getButton(String buttonName) { return buttons.get(buttonName); } - public Trigger getLeftTrigger() { - return new Trigger (() -> getLeftTriggerAxis() >= 0.5); + /** + * Returns the XboxController Button + * @param button Type of button. + * @return Trigger object to add commands to. + */ + public Trigger getButton(Button button) { + return getButton(button.toString()); } - public Trigger getRightTrigger() { - return new Trigger (() -> getRightTriggerAxis() >= 0.5); - } - - public Trigger getPOVButton(int i) { - return povButtons[i]; + /** + * Returns a POV button on the controller + * @param index The angle of the POV button in increments of 45 degrees, positive direction is clockwise + * @return Trigger to store the command to run + */ + public Trigger getPOVButton(int index) { + return povButtons[index]; } + /** + * Returns the 0 degree or top POV button. + * @return Trigger object to add commands to. + */ public Trigger getUpPOVButton() { return getPOVButton(0); } - public Trigger getDownPOVButton() { - return getPOVButton(4); - } - - public Trigger getLeftPOVButton() { - return getPOVButton(6); - } - + /** + * Returns the 90 degree or right POV button. + * @return Trigger object to add commands to. + */ public Trigger getRightPOVButton() { return getPOVButton(2); } - public void startVibrate() { - setRumble(RumbleType.kBothRumble, 0.8); + /** + * Returns the 180 degree or bottom POV button. + * @return Trigger object to add commands to. + */ + public Trigger getDownPOVButton() { + return getPOVButton(4); } - public void stopVibrate() { - setRumble(RumbleType.kBothRumble, 0); + /** + * Returns the 270 degree or left POV button. + * @return Trigger object to add commands to. + */ + public Trigger getLeftPOVButton() { + return getPOVButton(6); } - @Override + /** + * Returns the x-axis value of the right joystick. + * @return Right joystick X on [-1, 1], -1 is left, 1 is right. + */ + @Override public double getRightX() { - return Math.abs(super.getRightX()) > rightXDeadband ? super.getRightX():0; + return Math.abs(super.getRightX()) > rightXDeadband ? super.getRightX() : 0; } + /** + * Returns the y-axis value of the right joystick. + * @return Right joystick Y on [-1, 1], -1 is down, 1 is up. + */ @Override public double getRightY() { - return Math.abs(super.getRightY()) > rightYDeadband ? -super.getRightY():0; + return Math.abs(super.getRightY()) > rightYDeadband ? -super.getRightY() : 0; } + /** + * Returns the x-axis value of the left joystick. + * @return Left joystick X on [-1, 1], -1 is left, 1 is right. + */ @Override public double getLeftX() { - return Math.abs(super.getLeftX()) > leftXDeadband ? super.getLeftX():0; + return Math.abs(super.getLeftX()) > leftXDeadband ? super.getLeftX() : 0; } + /** + * Returns the y-axis value of the left joystick. + * @return Left joystick Y on [-1, 1], -1 is down, 1 is up. + */ @Override public double getLeftY() { - return Math.abs(super.getLeftY()) > leftYDeadband ? -super.getLeftY():0; + return Math.abs(super.getLeftY()) > leftYDeadband ? -super.getLeftY() : 0; + } + + /** + * Sets the horizontal deadband value for the right joystick. + *

Deadband reduces noise by requiring input above a certain threshold. + * @param deadband Value for deadband to be set to + */ + public void setRightXDeadband(double deadband) { + rightXDeadband = deadband; + } + + /** + * Sets the vertical deadband value for the right joystick. + *

Deadband reduces noise by requiring input above a certain threshold. + * @param deadband Value for deadband to be set to + */ + public void setRightYDeadband(double deadband) { + rightYDeadband = deadband; + } + + /** + * Sets the horizontal deadband value for the left joystick. + *

Deadband reduces noise by requiring input above a certain threshold. + * @param deadband Value for deadband to be set to + */ + public void setLeftXDeadband(double deadband) { + leftXDeadband = deadband; + } + + /** + * Sets the vertical deadband value for the left joystick. + *

Deadband reduces noise by requiring input above a certain threshold. + * @param deadband Value for deadband to be set to + */ + public void setLeftYDeadband(double deadband) { + leftYDeadband = deadband; + } + + /** + * Vibrates the controller + */ + public void startVibrate() { + setRumble(RumbleType.kBothRumble, 0.8); + } + + /** + * Stops vibrating the controller + */ + public void stopVibrate() { + setRumble(RumbleType.kBothRumble, 0); } } \ No newline at end of file diff --git a/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java b/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java index 779bd0b..1ddcb98 100644 --- a/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java +++ b/src/main/java/common/hardware/motorcontroller/NAR_CANSparkMax.java @@ -86,6 +86,7 @@ public NAR_CANSparkMax(int deviceNumber, MotorType type, EncoderType encoderType if (encoderType == EncoderType.Relative) { relativeEncoder = (SparkMaxRelativeEncoder) motor.getEncoder(); + //No clue what this does, but Mechanical Advantage does this so it must be good relativeEncoder.setAverageDepth(2); relativeEncoder.setMeasurementPeriod(10); } diff --git a/src/main/java/common/utility/NAR_Shuffleboard.java b/src/main/java/common/utility/NAR_Shuffleboard.java index 2ebb8eb..548e294 100644 --- a/src/main/java/common/utility/NAR_Shuffleboard.java +++ b/src/main/java/common/utility/NAR_Shuffleboard.java @@ -5,7 +5,6 @@ import java.util.function.Supplier; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; @@ -14,7 +13,7 @@ import edu.wpi.first.wpilibj.shuffleboard.SimpleWidget; /** - * Wrapper for {@link Shuffleboard} + * Team 3128's wrapper class for {@link Shuffleboard} * @since 2022 RAPID REACT * @author Mason Lam */