diff --git a/FRC_20230330_223449.wpilog b/FRC_20230330_223449.wpilog new file mode 100644 index 0000000..c0663e3 Binary files /dev/null and b/FRC_20230330_223449.wpilog differ diff --git a/simgui.json b/simgui.json index 72f68dd..fc4c75d 100644 --- a/simgui.json +++ b/simgui.json @@ -65,6 +65,7 @@ "/Shuffleboard/Subsystems/PDP": "Subsystem", "/Shuffleboard/Subsystems/PID": "Subsystem", "/Shuffleboard/Subsystems/PlaceConeSecondLevelCommand": "Command", + "/Shuffleboard/Subsystems/PlaceCubeSecondLevelCommand": "Command", "/Shuffleboard/Subsystems/PlaceGamePieceCommand": "Command", "/Shuffleboard/Subsystems/SwitchIdleModeCommmand": "Command", "/Shuffleboard/Subsystems/Tank": "Subsystem", @@ -81,9 +82,6 @@ "/SmartDashboard/Field": { "Robot": { "style": "Track" - }, - "window": { - "visible": true } }, "/SmartDashboard/Scheduler": { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7290622..66f220b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,11 +8,15 @@ import edu.wpi.first.wpilibj.XboxController; /** - * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean - * constants. This class should not be used for any other purpose. All constants should be declared + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared * globally (i.e. public static). Do not put anything functional in this class. * - *

It is advised to statically import this class (or one of its inner classes) wherever the + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the * constants are needed, to reduce verbosity. */ public final class Constants { diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 09cfa4f..b2375a4 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -30,344 +30,344 @@ * methods to flip these values based on the current alliance color. */ public final class FieldConstants { - public static final boolean isWPIField = false; // Red alliance + public static final boolean isWPIField = false; // Red alliance - public static final double fieldLength = Units.inchesToMeters(651.25); - public static final double fieldWidth = Units.inchesToMeters(315.5) - + (isWPIField ? Units.inchesToMeters(3.0) : 0.0); - public static final double tapeWidth = Units.inchesToMeters(2.0); + public static final double fieldLength = Units.inchesToMeters(651.25); + public static final double fieldWidth = Units.inchesToMeters(315.5) + + (isWPIField ? Units.inchesToMeters(3.0) : 0.0); + public static final double tapeWidth = Units.inchesToMeters(2.0); - // Dimensions for community and charging station, including the tape. - public static final class Community { - // Region dimensions - public static final double innerX = 0.0; - public static final double midX = Units.inchesToMeters(132.375); // Tape to the left of charging station - public static final double outerX = Units.inchesToMeters(193.25); // Tape to the right of charging - // station - public static final double leftY = Units.feetToMeters(18.0); - public static final double midY = leftY - Units.inchesToMeters(59.39) + tapeWidth; - public static final double rightY = 0.0; - public static final Translation2d[] regionCorners = new Translation2d[] { - new Translation2d(innerX, rightY), - new Translation2d(innerX, leftY), - new Translation2d(midX, leftY), - new Translation2d(midX, midY), - new Translation2d(outerX, midY), - new Translation2d(outerX, rightY), - }; + // Dimensions for community and charging station, including the tape. + public static final class Community { + // Region dimensions + public static final double innerX = 0.0; + public static final double midX = Units.inchesToMeters(132.375); // Tape to the left of charging station + public static final double outerX = Units.inchesToMeters(193.25); // Tape to the right of charging + // station + public static final double leftY = Units.feetToMeters(18.0); + public static final double midY = leftY - Units.inchesToMeters(59.39) + tapeWidth; + public static final double rightY = 0.0; + public static final Translation2d[] regionCorners = new Translation2d[] { + new Translation2d(innerX, rightY), + new Translation2d(innerX, leftY), + new Translation2d(midX, leftY), + new Translation2d(midX, midY), + new Translation2d(outerX, midY), + new Translation2d(outerX, rightY), + }; - // Charging station dimensions - public static final double chargingStationLength = Units.inchesToMeters(76.125); - public static final double chargingStationWidth = Units.inchesToMeters(97.25); - public static final double chargingStationOuterX = outerX - tapeWidth; - public static final double chargingStationInnerX = chargingStationOuterX - chargingStationLength; - public static final double chargingStationLeftY = midY - tapeWidth; - public static final double chargingStationRightY = chargingStationLeftY - chargingStationWidth; - public static final Translation2d[] chargingStationCorners = new Translation2d[] { - new Translation2d(chargingStationInnerX, chargingStationRightY), - new Translation2d(chargingStationInnerX, chargingStationLeftY), - new Translation2d(chargingStationOuterX, chargingStationRightY), - new Translation2d(chargingStationOuterX, chargingStationLeftY) - }; + // Charging station dimensions + public static final double chargingStationLength = Units.inchesToMeters(76.125); + public static final double chargingStationWidth = Units.inchesToMeters(97.25); + public static final double chargingStationOuterX = outerX - tapeWidth; + public static final double chargingStationInnerX = chargingStationOuterX - chargingStationLength; + public static final double chargingStationLeftY = midY - tapeWidth; + public static final double chargingStationRightY = chargingStationLeftY - chargingStationWidth; + public static final Translation2d[] chargingStationCorners = new Translation2d[] { + new Translation2d(chargingStationInnerX, chargingStationRightY), + new Translation2d(chargingStationInnerX, chargingStationLeftY), + new Translation2d(chargingStationOuterX, chargingStationRightY), + new Translation2d(chargingStationOuterX, chargingStationLeftY) + }; - // Cable bump - public static final double cableBumpInnerX = innerX + Grids.outerX + Units.inchesToMeters(95.25); - public static final double cableBumpOuterX = cableBumpInnerX + Units.inchesToMeters(7); - public static final Translation2d[] cableBumpCorners = new Translation2d[] { - new Translation2d(cableBumpInnerX, 0.0), - new Translation2d(cableBumpInnerX, chargingStationRightY), - new Translation2d(cableBumpOuterX, 0.0), - new Translation2d(cableBumpOuterX, chargingStationRightY) - }; - } + // Cable bump + public static final double cableBumpInnerX = innerX + Grids.outerX + Units.inchesToMeters(95.25); + public static final double cableBumpOuterX = cableBumpInnerX + Units.inchesToMeters(7); + public static final Translation2d[] cableBumpCorners = new Translation2d[] { + new Translation2d(cableBumpInnerX, 0.0), + new Translation2d(cableBumpInnerX, chargingStationRightY), + new Translation2d(cableBumpOuterX, 0.0), + new Translation2d(cableBumpOuterX, chargingStationRightY) + }; + } - // Dimensions for grids and nodes - public static final class Grids { - // X layout - public static final double outerX = Units.inchesToMeters(54.25); - public static final double lowX = outerX - (Units.inchesToMeters(14.25) / 2.0); // Centered when under - // cube - // nodes - public static final double midX = outerX - Units.inchesToMeters(22.75); - public static final double highX = outerX - Units.inchesToMeters(39.75); + // Dimensions for grids and nodes + public static final class Grids { + // X layout + public static final double outerX = Units.inchesToMeters(54.25); + public static final double lowX = outerX - (Units.inchesToMeters(14.25) / 2.0); // Centered when under + // cube + // nodes + public static final double midX = outerX - Units.inchesToMeters(22.75); + public static final double highX = outerX - Units.inchesToMeters(39.75); - // Y layout - public static final int nodeRowCount = 9; - public static final double[] nodeY = isWPIField - ? new double[] { - Units.inchesToMeters(20.19 + 22.0 * 0), - Units.inchesToMeters(20.19 + 22.0 * 1), - Units.inchesToMeters(20.19 + 22.0 * 2), - Units.inchesToMeters(20.19 + 22.0 * 3), - Units.inchesToMeters(20.19 + 22.0 * 4), - Units.inchesToMeters(20.19 + 22.0 * 5), - Units.inchesToMeters(20.19 + 22.0 * 6), - Units.inchesToMeters(20.19 + 22.0 * 7), - Units.inchesToMeters(20.19 + 22.0 * 8 + 2.5) - } - : new double[] { - Units.inchesToMeters(20.19 + 22.0 * 0), - Units.inchesToMeters(20.19 + 22.0 * 1), - Units.inchesToMeters(20.19 + 22.0 * 2), - Units.inchesToMeters(20.19 + 22.0 * 3), - Units.inchesToMeters(20.19 + 22.0 * 4), - Units.inchesToMeters(20.19 + 22.0 * 5), - Units.inchesToMeters(20.19 + 22.0 * 6), - Units.inchesToMeters(20.19 + 22.0 * 7), - Units.inchesToMeters(20.19 + 22.0 * 8) - }; + // Y layout + public static final int nodeRowCount = 9; + public static final double[] nodeY = isWPIField + ? new double[] { + Units.inchesToMeters(20.19 + 22.0 * 0), + Units.inchesToMeters(20.19 + 22.0 * 1), + Units.inchesToMeters(20.19 + 22.0 * 2), + Units.inchesToMeters(20.19 + 22.0 * 3), + Units.inchesToMeters(20.19 + 22.0 * 4), + Units.inchesToMeters(20.19 + 22.0 * 5), + Units.inchesToMeters(20.19 + 22.0 * 6), + Units.inchesToMeters(20.19 + 22.0 * 7), + Units.inchesToMeters(20.19 + 22.0 * 8 + 2.5) + } + : new double[] { + Units.inchesToMeters(20.19 + 22.0 * 0), + Units.inchesToMeters(20.19 + 22.0 * 1), + Units.inchesToMeters(20.19 + 22.0 * 2), + Units.inchesToMeters(20.19 + 22.0 * 3), + Units.inchesToMeters(20.19 + 22.0 * 4), + Units.inchesToMeters(20.19 + 22.0 * 5), + Units.inchesToMeters(20.19 + 22.0 * 6), + Units.inchesToMeters(20.19 + 22.0 * 7), + Units.inchesToMeters(20.19 + 22.0 * 8) + }; - // Z layout - public static final double cubeEdgeHigh = Units.inchesToMeters(3.0); - public static final double highCubeZ = Units.inchesToMeters(35.5) - cubeEdgeHigh; - public static final double midCubeZ = Units.inchesToMeters(23.5) - cubeEdgeHigh; - public static final double highConeZ = Units.inchesToMeters(46.0); - public static final double midConeZ = Units.inchesToMeters(34.0); + // Z layout + public static final double cubeEdgeHigh = Units.inchesToMeters(3.0); + public static final double highCubeZ = Units.inchesToMeters(35.5) - cubeEdgeHigh; + public static final double midCubeZ = Units.inchesToMeters(23.5) - cubeEdgeHigh; + public static final double highConeZ = Units.inchesToMeters(46.0); + public static final double midConeZ = Units.inchesToMeters(34.0); - // Translations (all nodes in the same column/row have the same X/Y coordinate) - public static final Translation2d[] lowTranslations = new Translation2d[nodeRowCount]; - public static final Translation3d[] low3dTranslations = new Translation3d[nodeRowCount]; - public static final Translation2d[] midTranslations = new Translation2d[nodeRowCount]; - public static final Translation3d[] mid3dTranslations = new Translation3d[nodeRowCount]; - public static final Translation2d[] highTranslations = new Translation2d[nodeRowCount]; - public static final Translation3d[] high3dTranslations = new Translation3d[nodeRowCount]; + // Translations (all nodes in the same column/row have the same X/Y coordinate) + public static final Translation2d[] lowTranslations = new Translation2d[nodeRowCount]; + public static final Translation3d[] low3dTranslations = new Translation3d[nodeRowCount]; + public static final Translation2d[] midTranslations = new Translation2d[nodeRowCount]; + public static final Translation3d[] mid3dTranslations = new Translation3d[nodeRowCount]; + public static final Translation2d[] highTranslations = new Translation2d[nodeRowCount]; + public static final Translation3d[] high3dTranslations = new Translation3d[nodeRowCount]; - static { - for (int i = 0; i < nodeRowCount; i++) { - boolean isCube = i == 1 || i == 4 || i == 7; - lowTranslations[i] = new Translation2d(lowX, nodeY[i]); - low3dTranslations[i] = new Translation3d(lowX, nodeY[i], 0.0); - midTranslations[i] = new Translation2d(midX, nodeY[i]); - mid3dTranslations[i] = new Translation3d(midX, nodeY[i], isCube ? midCubeZ : midConeZ); - highTranslations[i] = new Translation2d(highX, nodeY[i]); - high3dTranslations[i] = new Translation3d(highX, nodeY[i], - isCube ? highCubeZ : highConeZ); - } - } + static { + for (int i = 0; i < nodeRowCount; i++) { + boolean isCube = i == 1 || i == 4 || i == 7; + lowTranslations[i] = new Translation2d(lowX, nodeY[i]); + low3dTranslations[i] = new Translation3d(lowX, nodeY[i], 0.0); + midTranslations[i] = new Translation2d(midX, nodeY[i]); + mid3dTranslations[i] = new Translation3d(midX, nodeY[i], isCube ? midCubeZ : midConeZ); + highTranslations[i] = new Translation2d(highX, nodeY[i]); + high3dTranslations[i] = new Translation3d(highX, nodeY[i], + isCube ? highCubeZ : highConeZ); + } + } - // Complex low layout (shifted to account for cube vs cone rows and wide edge - // nodes) - public static final double complexLowXCones = outerX - Units.inchesToMeters(16.0) / 2.0; // Centered X - // under - // cone nodes - public static final double complexLowXCubes = lowX; // Centered X under cube nodes - public static final double complexLowOuterYOffset = nodeY[0] - - (Units.inchesToMeters(3.0) + (Units.inchesToMeters(25.75) / 2.0)); + // Complex low layout (shifted to account for cube vs cone rows and wide edge + // nodes) + public static final double complexLowXCones = outerX - Units.inchesToMeters(16.0) / 2.0; // Centered X + // under + // cone nodes + public static final double complexLowXCubes = lowX; // Centered X under cube nodes + public static final double complexLowOuterYOffset = nodeY[0] + - (Units.inchesToMeters(3.0) + (Units.inchesToMeters(25.75) / 2.0)); - public static final Translation2d[] complexLowTranslations = new Translation2d[] { - new Translation2d(complexLowXCones, nodeY[0] - complexLowOuterYOffset), - new Translation2d(complexLowXCubes, nodeY[1]), - new Translation2d(complexLowXCones, nodeY[2]), - new Translation2d(complexLowXCones, nodeY[3]), - new Translation2d(complexLowXCubes, nodeY[4]), - new Translation2d(complexLowXCones, nodeY[5]), - new Translation2d(complexLowXCones, nodeY[6]), - new Translation2d(complexLowXCubes, nodeY[7]), - new Translation2d(complexLowXCones, nodeY[8] + complexLowOuterYOffset), - }; + public static final Translation2d[] complexLowTranslations = new Translation2d[] { + new Translation2d(complexLowXCones, nodeY[0] - complexLowOuterYOffset), + new Translation2d(complexLowXCubes, nodeY[1]), + new Translation2d(complexLowXCones, nodeY[2]), + new Translation2d(complexLowXCones, nodeY[3]), + new Translation2d(complexLowXCubes, nodeY[4]), + new Translation2d(complexLowXCones, nodeY[5]), + new Translation2d(complexLowXCones, nodeY[6]), + new Translation2d(complexLowXCubes, nodeY[7]), + new Translation2d(complexLowXCones, nodeY[8] + complexLowOuterYOffset), + }; - public static final Translation3d[] complexLow3dTranslations = new Translation3d[] { - new Translation3d(complexLowXCones, nodeY[0] - complexLowOuterYOffset, 0.0), - new Translation3d(complexLowXCubes, nodeY[1], 0.0), - new Translation3d(complexLowXCones, nodeY[2], 0.0), - new Translation3d(complexLowXCones, nodeY[3], 0.0), - new Translation3d(complexLowXCubes, nodeY[4], 0.0), - new Translation3d(complexLowXCones, nodeY[5], 0.0), - new Translation3d(complexLowXCones, nodeY[6], 0.0), - new Translation3d(complexLowXCubes, nodeY[7], 0.0), - new Translation3d(complexLowXCones, nodeY[8] + complexLowOuterYOffset, 0.0), - }; - } + public static final Translation3d[] complexLow3dTranslations = new Translation3d[] { + new Translation3d(complexLowXCones, nodeY[0] - complexLowOuterYOffset, 0.0), + new Translation3d(complexLowXCubes, nodeY[1], 0.0), + new Translation3d(complexLowXCones, nodeY[2], 0.0), + new Translation3d(complexLowXCones, nodeY[3], 0.0), + new Translation3d(complexLowXCubes, nodeY[4], 0.0), + new Translation3d(complexLowXCones, nodeY[5], 0.0), + new Translation3d(complexLowXCones, nodeY[6], 0.0), + new Translation3d(complexLowXCubes, nodeY[7], 0.0), + new Translation3d(complexLowXCones, nodeY[8] + complexLowOuterYOffset, 0.0), + }; + } - // Dimensions for loading zone and substations, including the tape - public static final class LoadingZone { - // Region dimensions - public static final double width = Units.inchesToMeters(99.0); - public static final double innerX = FieldConstants.fieldLength; - public static final double midX = fieldLength - Units.inchesToMeters(132.25); - public static final double outerX = fieldLength - Units.inchesToMeters(264.25); - public static final double leftY = FieldConstants.fieldWidth; - public static final double midY = leftY - Units.inchesToMeters(50.5); - public static final double rightY = leftY - width; - public static final Translation2d[] regionCorners = new Translation2d[] { - new Translation2d( - midX, rightY), // Start at lower left next to border with opponent - // community - new Translation2d(midX, midY), - new Translation2d(outerX, midY), - new Translation2d(outerX, leftY), - new Translation2d(innerX, leftY), - new Translation2d(innerX, rightY), - }; + // Dimensions for loading zone and substations, including the tape + public static final class LoadingZone { + // Region dimensions + public static final double width = Units.inchesToMeters(99.0); + public static final double innerX = FieldConstants.fieldLength; + public static final double midX = fieldLength - Units.inchesToMeters(132.25); + public static final double outerX = fieldLength - Units.inchesToMeters(264.25); + public static final double leftY = FieldConstants.fieldWidth; + public static final double midY = leftY - Units.inchesToMeters(50.5); + public static final double rightY = leftY - width; + public static final Translation2d[] regionCorners = new Translation2d[] { + new Translation2d( + midX, rightY), // Start at lower left next to border with opponent + // community + new Translation2d(midX, midY), + new Translation2d(outerX, midY), + new Translation2d(outerX, leftY), + new Translation2d(innerX, leftY), + new Translation2d(innerX, rightY), + }; - // Double substation dimensions - public static final double doubleSubstationLength = Units.inchesToMeters(14.0); - public static final double doubleSubstationX = innerX - doubleSubstationLength; - public static final double doubleSubstationShelfZ = Units.inchesToMeters(37.375); - public static final double doubleSubstationCenterY = fieldWidth - Units.inchesToMeters(49.76); + // Double substation dimensions + public static final double doubleSubstationLength = Units.inchesToMeters(14.0); + public static final double doubleSubstationX = innerX - doubleSubstationLength; + public static final double doubleSubstationShelfZ = Units.inchesToMeters(37.375); + public static final double doubleSubstationCenterY = fieldWidth - Units.inchesToMeters(49.76); - // Single substation dimensions - public static final double singleSubstationWidth = Units.inchesToMeters(22.75); - public static final double singleSubstationLeftX = FieldConstants.fieldLength - doubleSubstationLength - - Units.inchesToMeters(88.77); - public static final double singleSubstationCenterX = singleSubstationLeftX - + (singleSubstationWidth / 2.0); - public static final double singleSubstationRightX = singleSubstationLeftX + singleSubstationWidth; - public static final Translation2d singleSubstationTranslation = new Translation2d( - singleSubstationCenterX, - leftY); + // Single substation dimensions + public static final double singleSubstationWidth = Units.inchesToMeters(22.75); + public static final double singleSubstationLeftX = FieldConstants.fieldLength - doubleSubstationLength + - Units.inchesToMeters(88.77); + public static final double singleSubstationCenterX = singleSubstationLeftX + + (singleSubstationWidth / 2.0); + public static final double singleSubstationRightX = singleSubstationLeftX + singleSubstationWidth; + public static final Translation2d singleSubstationTranslation = new Translation2d( + singleSubstationCenterX, + leftY); - public static final double singleSubstationHeight = Units.inchesToMeters(18.0); - public static final double singleSubstationLowZ = Units.inchesToMeters(27.125); - public static final double singleSubstationCenterZ = singleSubstationLowZ - + (singleSubstationHeight / 2.0); - public static final double singleSubstationHighZ = singleSubstationLowZ + singleSubstationHeight; - } + public static final double singleSubstationHeight = Units.inchesToMeters(18.0); + public static final double singleSubstationLowZ = Units.inchesToMeters(27.125); + public static final double singleSubstationCenterZ = singleSubstationLowZ + + (singleSubstationHeight / 2.0); + public static final double singleSubstationHighZ = singleSubstationLowZ + singleSubstationHeight; + } - // Locations of staged game pieces - public static final class StagingLocations { - public static final double centerOffsetX = Units.inchesToMeters(47.36); - public static final double positionX = fieldLength / 2.0 - Units.inchesToMeters(47.36); - public static final double firstY = Units.inchesToMeters(36.19); - public static final double separationY = Units.inchesToMeters(48.0); - public static final Translation2d[] translations = new Translation2d[4]; + // Locations of staged game pieces + public static final class StagingLocations { + public static final double centerOffsetX = Units.inchesToMeters(47.36); + public static final double positionX = fieldLength / 2.0 - Units.inchesToMeters(47.36); + public static final double firstY = Units.inchesToMeters(36.19); + public static final double separationY = Units.inchesToMeters(48.0); + public static final Translation2d[] translations = new Translation2d[4]; - static { - for (int i = 0; i < translations.length; i++) { - translations[i] = new Translation2d(positionX, firstY + (i * separationY)); - } + static { + for (int i = 0; i < translations.length; i++) { + translations[i] = new Translation2d(positionX, firstY + (i * separationY)); + } + } } - } - // AprilTag constants - public static final double aprilTagWidth = Units.inchesToMeters(6.0); - public static final AprilTagFieldLayout aprilTags = isWPIField - ? new AprilTagFieldLayout( - List.of( - new AprilTag( - 1, - new Pose3d( - Units.inchesToMeters(610.125), - Units.inchesToMeters(43.5), - Units.inchesToMeters(19.25), - new Rotation3d(0.0, 0.0, - Math.PI))), - new AprilTag( - 2, - new Pose3d( - Units.inchesToMeters(610.375), - Units.inchesToMeters(109.5), - Units.inchesToMeters(19.25), - new Rotation3d(0.0, 0.0, - Math.PI))), - new AprilTag( - 3, - new Pose3d( - Units.inchesToMeters(610.0), - Units.inchesToMeters(176.0), - Units.inchesToMeters(19.25), - new Rotation3d(0.0, 0.0, - Math.PI))), - new AprilTag( - 4, - new Pose3d( - Units.inchesToMeters(635.375), - Units.inchesToMeters(272.0), - Units.inchesToMeters(27.25), - new Rotation3d(0.0, 0.0, - Math.PI))), - new AprilTag( - 5, - new Pose3d( - Units.inchesToMeters(14.25), - LoadingZone.doubleSubstationCenterY, - Units.inchesToMeters(27.38), - new Rotation3d())), - new AprilTag( - 6, - new Pose3d( - Units.inchesToMeters(40.45), - Grids.nodeY[7], - Units.inchesToMeters(18.22), - new Rotation3d())), - new AprilTag( - 7, - new Pose3d( - Units.inchesToMeters(40.45), - Grids.nodeY[4], - Units.inchesToMeters(18.22), - new Rotation3d())), - new AprilTag( - 8, - new Pose3d( - Units.inchesToMeters(40.45), - Grids.nodeY[1], - Units.inchesToMeters(18.22), - new Rotation3d()))), - fieldLength, - fieldWidth) - : new AprilTagFieldLayout( - List.of( - new AprilTag( - 1, - new Pose3d( - Units.inchesToMeters(610.77), - Grids.nodeY[1], - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, - Math.PI))), - new AprilTag( - 2, - new Pose3d( - Units.inchesToMeters(610.77), - Grids.nodeY[4], - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, - Math.PI))), - new AprilTag( - 3, - new Pose3d( - Units.inchesToMeters(610.77), - Grids.nodeY[7], - Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, - Math.PI))), - new AprilTag( - 4, - new Pose3d( - Units.inchesToMeters(636.96), - LoadingZone.doubleSubstationCenterY, - Units.inchesToMeters(27.38), - new Rotation3d(0.0, 0.0, - Math.PI))), - new AprilTag( - 5, - new Pose3d( - Units.inchesToMeters(14.25), - LoadingZone.doubleSubstationCenterY, - Units.inchesToMeters(27.38), - new Rotation3d())), - new AprilTag( - 6, - new Pose3d( - Units.inchesToMeters(40.45), - Grids.nodeY[7], - Units.inchesToMeters(18.22), - new Rotation3d())), - new AprilTag( - 7, - new Pose3d( - Units.inchesToMeters(40.45), - Grids.nodeY[4], - Units.inchesToMeters(18.22), - new Rotation3d())), - new AprilTag( - 8, - new Pose3d( - Units.inchesToMeters(40.45), - Grids.nodeY[1], - Units.inchesToMeters(18.22), - new Rotation3d()))), - fieldLength, - fieldWidth); + // AprilTag constants + public static final double aprilTagWidth = Units.inchesToMeters(6.0); + public static final AprilTagFieldLayout aprilTags = isWPIField + ? new AprilTagFieldLayout( + List.of( + new AprilTag( + 1, + new Pose3d( + Units.inchesToMeters(610.125), + Units.inchesToMeters(43.5), + Units.inchesToMeters(19.25), + new Rotation3d(0.0, 0.0, + Math.PI))), + new AprilTag( + 2, + new Pose3d( + Units.inchesToMeters(610.375), + Units.inchesToMeters(109.5), + Units.inchesToMeters(19.25), + new Rotation3d(0.0, 0.0, + Math.PI))), + new AprilTag( + 3, + new Pose3d( + Units.inchesToMeters(610.0), + Units.inchesToMeters(176.0), + Units.inchesToMeters(19.25), + new Rotation3d(0.0, 0.0, + Math.PI))), + new AprilTag( + 4, + new Pose3d( + Units.inchesToMeters(635.375), + Units.inchesToMeters(272.0), + Units.inchesToMeters(27.25), + new Rotation3d(0.0, 0.0, + Math.PI))), + new AprilTag( + 5, + new Pose3d( + Units.inchesToMeters(14.25), + LoadingZone.doubleSubstationCenterY, + Units.inchesToMeters(27.38), + new Rotation3d())), + new AprilTag( + 6, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[7], + Units.inchesToMeters(18.22), + new Rotation3d())), + new AprilTag( + 7, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[4], + Units.inchesToMeters(18.22), + new Rotation3d())), + new AprilTag( + 8, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[1], + Units.inchesToMeters(18.22), + new Rotation3d()))), + fieldLength, + fieldWidth) + : new AprilTagFieldLayout( + List.of( + new AprilTag( + 1, + new Pose3d( + Units.inchesToMeters(610.77), + Grids.nodeY[1], + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, + Math.PI))), + new AprilTag( + 2, + new Pose3d( + Units.inchesToMeters(610.77), + Grids.nodeY[4], + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, + Math.PI))), + new AprilTag( + 3, + new Pose3d( + Units.inchesToMeters(610.77), + Grids.nodeY[7], + Units.inchesToMeters(18.22), + new Rotation3d(0.0, 0.0, + Math.PI))), + new AprilTag( + 4, + new Pose3d( + Units.inchesToMeters(636.96), + LoadingZone.doubleSubstationCenterY, + Units.inchesToMeters(27.38), + new Rotation3d(0.0, 0.0, + Math.PI))), + new AprilTag( + 5, + new Pose3d( + Units.inchesToMeters(14.25), + LoadingZone.doubleSubstationCenterY, + Units.inchesToMeters(27.38), + new Rotation3d())), + new AprilTag( + 6, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[7], + Units.inchesToMeters(18.22), + new Rotation3d())), + new AprilTag( + 7, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[4], + Units.inchesToMeters(18.22), + new Rotation3d())), + new AprilTag( + 8, + new Pose3d( + Units.inchesToMeters(40.45), + Grids.nodeY[1], + Units.inchesToMeters(18.22), + new Rotation3d()))), + fieldLength, + fieldWidth); } \ No newline at end of file diff --git a/src/main/java/frc/robot/Interfaces/CANSparkMax.java b/src/main/java/frc/robot/Interfaces/CANSparkMax.java index 9b37903..67a8645 100644 --- a/src/main/java/frc/robot/Interfaces/CANSparkMax.java +++ b/src/main/java/frc/robot/Interfaces/CANSparkMax.java @@ -1,4 +1,4 @@ -package frc.robot.Interfaces; +package frc.robot.interfaces; import com.revrobotics.REVLibError; import edu.wpi.first.util.sendable.Sendable; @@ -11,116 +11,169 @@ public CANSparkMax(int deviceId, MotorType type) { super(deviceId, type); } - // for reference, here's all the methods that have a getter and, optionally, a setter, in the - // REVLib CANSparkMax class (brackets indicate a note, preceeding percent signs indicate a method - // that returns an object for--you guessed it-- more interfacing, a dollar sign indicates a method - // we won't be using, exclamation points indicate a method that returns an object that we will be + // for reference, here's all the methods that have a getter and, optionally, a + // setter, in the + // REVLib CANSparkMax class (brackets indicate a note, preceeding percent signs + // indicate a method + // that returns an object for--you guessed it-- more interfacing, a dollar sign + // indicates a method + // we won't be using, exclamation points indicate a method that returns an + // object that we will be // using or that we need to pay attention to) /* - * get() and set() -- these are the main methods for getting and setting the output of the motor controller - * % getAbsoluteEncoder() [no setter] -- returns an object for interfacing with a connected absolute encoder - * %$ getAlternateEncoder() [no setter] -- returns an object for interfacing with a quadrature encoder connected to the alternate encoder pins - * %$ getAnalog() [no setter] -- returns an object for interfacing with a connected analog sensor - * getAppliedOutput() [no setter] -- returns the output of the motor controller, in the range [-1, 1] - * getBusVoltage() [no setter] -- returns the bus voltage of the motor controller - * getClosedLoopRampRate() and setClosedLoopRampRate() -- gets and sets the closed loop ramp rate of the motor controller - * ! getEncoder() [no setter] -- returns an object for interfacing with a connected encoder - * getFaults() [no setter] -- returns an object for interfacing with the faults of the motor controller - * an extension of this is getStickyFaults(), which returns an object for interfacing with the sticky faults of the motor controller - * as well as getFault() and getStickyFault(), which return a boolean for whether or not a specific fault is present - * getFeedbackDeviceID() [no setter] -- returns the ID of the feedback device set on the motor controller - * % getForwardLimitSwitch() [no setter] -- returns an object for interfacing with the forward limit switch - * ! getIdleMode() and setIdleMode() -- gets and sets the idle mode of the motor controller - * getInverted() and setInverted() -- gets and sets the inversion of the motor controller + * get() and set() -- these are the main methods for getting and setting the + * output of the motor controller + * % getAbsoluteEncoder() [no setter] -- returns an object for interfacing with + * a connected absolute encoder + * %$ getAlternateEncoder() [no setter] -- returns an object for interfacing + * with a quadrature encoder connected to the alternate encoder pins + * %$ getAnalog() [no setter] -- returns an object for interfacing with a + * connected analog sensor + * getAppliedOutput() [no setter] -- returns the output of the motor controller, + * in the range [-1, 1] + * getBusVoltage() [no setter] -- returns the bus voltage of the motor + * controller + * getClosedLoopRampRate() and setClosedLoopRampRate() -- gets and sets the + * closed loop ramp rate of the motor controller + * ! getEncoder() [no setter] -- returns an object for interfacing with a + * connected encoder + * getFaults() [no setter] -- returns an object for interfacing with the faults + * of the motor controller + * an extension of this is getStickyFaults(), which returns an object for + * interfacing with the sticky faults of the motor controller + * as well as getFault() and getStickyFault(), which return a boolean for + * whether or not a specific fault is present + * getFeedbackDeviceID() [no setter] -- returns the ID of the feedback device + * set on the motor controller + * % getForwardLimitSwitch() [no setter] -- returns an object for interfacing + * with the forward limit switch + * ! getIdleMode() and setIdleMode() -- gets and sets the idle mode of the motor + * controller + * getInverted() and setInverted() -- gets and sets the inversion of the motor + * controller * getLastError() [no setter] -- returns the last error of the motor controller - * getMotorTemperature() [no setter] -- returns the temperature of the motor controller (at least, that's what we assume, the documentation is unclear) - * getOutputCurrent() [no setter] -- returns the output current of the motor controller - * % getPIDController() [no setter] -- returns an object for interfacing with the PID controller of the motor controller - * %$ getReverseLimitSwitch() [no setter] -- returns an object for interfacing with the reverse limit switch - * getSoftLimit() and setSoftLimit() -- gets and sets the soft limit of the motor controller - * getSmartCurrentLimit() and setSmartCurrentLimit() -- gets and sets the smart current limit of the motor controller - * getVoltageCompensationNomininalVoltage() and setVoltageCompensationNomininalVoltage() -- gets and sets the voltage compensation nominal voltage of the motor controller + * getMotorTemperature() [no setter] -- returns the temperature of the motor + * controller (at least, that's what we assume, the documentation is unclear) + * getOutputCurrent() [no setter] -- returns the output current of the motor + * controller + * % getPIDController() [no setter] -- returns an object for interfacing with + * the PID controller of the motor controller + * %$ getReverseLimitSwitch() [no setter] -- returns an object for interfacing + * with the reverse limit switch + * getSoftLimit() and setSoftLimit() -- gets and sets the soft limit of the + * motor controller + * getSmartCurrentLimit() and setSmartCurrentLimit() -- gets and sets the smart + * current limit of the motor controller + * getVoltageCompensationNomininalVoltage() and + * setVoltageCompensationNomininalVoltage() -- gets and sets the voltage + * compensation nominal voltage of the motor controller * - * is() statements -- these are all boolean methods that return whether or not a specific condition is true + * is() statements -- these are all boolean methods that return whether or not a + * specific condition is true * - * isFollower() [no setter] -- returns a boolean for whether or not the motor controller is a follower of another motor controller - * isSoftLimitEnabled() [no setter] -- returns a boolean for whether or not the soft limit is enabled + * isFollower() [no setter] -- returns a boolean for whether or not the motor + * controller is a follower of another motor controller + * isSoftLimitEnabled() [no setter] -- returns a boolean for whether or not the + * soft limit is enabled * * set() statements -- these are all methods that set a specific property * * setCANTimeout() -- sets the CAN timeout of the motor controller - * setVoltage() -- sets the voltage of the motor controller (This seems really dangerous, but it's in the documentation, so we'll include it) + * setVoltage() -- sets the voltage of the motor controller (This seems really + * dangerous, but it's in the documentation, so we'll include it) * - * inherited methods -- these are all methods that are inherited from various other things + * inherited methods -- these are all methods that are inherited from various + * other things * * getDeviceId() [no setter] -- returns the device ID of the motor controller - * getFirmwareString() [no setter] -- returns the firmware string of the motor controller - * getFirmwareVersion() [no setter] -- returns the firmware version of the motor controller - * getInitialMotorType() [no setter] -- returns the initial motor type of the motor controller + * getFirmwareString() [no setter] -- returns the firmware string of the motor + * controller + * getFirmwareVersion() [no setter] -- returns the firmware version of the motor + * controller + * getInitialMotorType() [no setter] -- returns the initial motor type of the + * motor controller * getMotorType() [no setter] -- returns the motor type of the motor controller - * getSafeFloat() [no setter] -- returns the safe float of the motor controller (??) - * getSerialNumber() [no setter] -- returns the serial number of the motor controller - * restoreFactoryDefaults() -- restores the factory defaults of the motor controller - * setControlFramePeriodMs() -- sets the control frame period of the motor controller - * setPeriodicFramePeriod() -- sets the periodic frame period of the motor controller - * throwIfClosed() [no setter] -- throws an exception if the motor controller is closed + * getSafeFloat() [no setter] -- returns the safe float of the motor controller + * (??) + * getSerialNumber() [no setter] -- returns the serial number of the motor + * controller + * restoreFactoryDefaults() -- restores the factory defaults of the motor + * controller + * setControlFramePeriodMs() -- sets the control frame period of the motor + * controller + * setPeriodicFramePeriod() -- sets the periodic frame period of the motor + * controller + * throwIfClosed() [no setter] -- throws an exception if the motor controller is + * closed */ - // IGNORE EVERYTHING ABOVE THIS! I rewrote the entire thing, and it's much better now! + // IGNORE EVERYTHING ABOVE THIS! I rewrote the entire thing, and it's much + // better now! - // here's all the methods found in CANSparkMaxLowLevel using the magic of regex (public [a-zA-Z]* + // here's all the methods found in CANSparkMaxLowLevel using the magic of regex + // (public [a-zA-Z]* // get[a-zA-z_]*()) /* - * getFirmwareVersion - * getFirmwareString - * getDeviceId - * getInitialMotorType - * getMotorType - * getSafeFloat - * getRaw - * - * and all the set methods (public [a-zA-Z]* set[a-zA-z_]*()) - * - * setControlFramePeriodMs - * setPeriodicFramePeriod - * - * and all the public variables that we can *technically* access, and we probably shouldn't, but we can, so safety be damned (safety? i hardly know her!) - * - * public class PeriodicStatus0 { - public double appliedOutput; - public short faults; - public short stickyFaults; - public byte lock; - public MotorType motorType; - public boolean isFollower; - public boolean isInverted; - public boolean roboRIO; - } - - public class PeriodicStatus1 { - public double sensorVelocity; - public byte motorTemperature; - public double busVoltage; - public double outputCurrent; - } - - public class PeriodicStatus2 { - public double sensorPosition; - public double iAccum; - } - * - * now, naturally, there's some things we can't access, like the CAN ID, because that's private, and the firmware version, because that's protected, but we can access the rest of it, so we're going to. see above for the list of methods we can access - * - * (water break! if you're actually reading this, you're a trooper, and i salute you. even the best of us need water sometimes, so go get some, and then come back and finish reading this. i'll wait.) - */ + * getFirmwareVersion + * getFirmwareString + * getDeviceId + * getInitialMotorType + * getMotorType + * getSafeFloat + * getRaw + * + * and all the set methods (public [a-zA-Z]* set[a-zA-z_]*()) + * + * setControlFramePeriodMs + * setPeriodicFramePeriod + * + * and all the public variables that we can *technically* access, and we + * probably shouldn't, but we can, so safety be damned (safety? i hardly know + * her!) + * + * public class PeriodicStatus0 { + * public double appliedOutput; + * public short faults; + * public short stickyFaults; + * public byte lock; + * public MotorType motorType; + * public boolean isFollower; + * public boolean isInverted; + * public boolean roboRIO; + * } + * + * public class PeriodicStatus1 { + * public double sensorVelocity; + * public byte motorTemperature; + * public double busVoltage; + * public double outputCurrent; + * } + * + * public class PeriodicStatus2 { + * public double sensorPosition; + * public double iAccum; + * } + * + * now, naturally, there's some things we can't access, like the CAN ID, because + * that's private, and the firmware version, because that's protected, but we + * can access the rest of it, so we're going to. see above for the list of + * methods we can access + * + * (water break! if you're actually reading this, you're a trooper, and i salute + * you. even the best of us need water sometimes, so go get some, and then come + * back and finish reading this. i'll wait.) + */ - // here's all the methods found in CANSparkMax using the magic of regex (public [a-zA-Z]* + // here's all the methods found in CANSparkMax using the magic of regex (public + // [a-zA-Z]* // get[a-zA-z_]*()) /* * get() * getInverted() - * getEncoder() [optionally, it takes 2 paramaters, encoderType and countsPerRev] - * getAlternateEncoder() [optionally, it takes 2 paramaters, encoderType and countsPerRev] + * getEncoder() [optionally, it takes 2 paramaters, encoderType and + * countsPerRev] + * getAlternateEncoder() [optionally, it takes 2 paramaters, encoderType and + * countsPerRev] * getAnalog(mode) [no docs on what mode is, so that's cool] * getAbsoluteEncoder() * getPIDController() @@ -144,9 +197,11 @@ public class PeriodicStatus2 { * and all the set methods (public [a-zA-Z]* set[a-zA-z_]*()) * * set() - * setVoltage() [also seems unsafe, but it's in the documentation, so we'll include it] + * setVoltage() [also seems unsafe, but it's in the documentation, so we'll + * include it] * setInverted() - * setSmartCurrentLimit() [the library actually instantiated this a bunch of times, so we'll just include the first one] + * setSmartCurrentLimit() [the library actually instantiated this a bunch of + * times, so we'll just include the first one] * setSecondaryCurrentLimit() * setIdleMode() * setOpenLoopRampRate() @@ -180,7 +235,8 @@ public class PeriodicStatus2 { * setMeasurementPeriod() * setInverted() * - * Copilot, you seem to know what you're doing. I'll let you fill in the blanks for what I'm missing. + * Copilot, you seem to know what you're doing. I'll let you fill in the blanks + * for what I'm missing. * * */ @@ -248,10 +304,13 @@ public void initSendable(SendableBuilder builder) { "Reverse Hard Limit?*", () -> getStickyFault(FaultID.kHardLimitRev), null); // brake mode - // this one is a bit weird. we have to create a switch statement to get the right value, because + // this one is a bit weird. we have to create a switch statement to get the + // right value, because // it's either gonna be kCoast or kBrake - // we do that outside the initSendable() method, because it's a bit too long to put in there, - // and I don't think it'll work. If I put outside, I'll be able to call it in other places too. + // we do that outside the initSendable() method, because it's a bit too long to + // put in there, + // and I don't think it'll work. If I put outside, I'll be able to call it in + // other places too. builder.addBooleanProperty("Brake Mode", this::isBraking, this::setBrakeMode); builder.addBooleanProperty("Coast Mode", this::isCoasting, this::setCoastMode); @@ -274,6 +333,7 @@ public boolean isBraking() { return false; } } + /** * Returns true if the motor controller is coasting for its idle mode. * diff --git a/src/main/java/frc/robot/Interfaces/Idriveable.java b/src/main/java/frc/robot/Interfaces/Idriveable.java index 072d6c3..1b2980f 100644 --- a/src/main/java/frc/robot/Interfaces/Idriveable.java +++ b/src/main/java/frc/robot/Interfaces/Idriveable.java @@ -1,4 +1,4 @@ -package frc.robot.Interfaces; +package frc.robot.interfaces; public interface Idriveable { void driveForward(); diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index dc3756f..5ed4cb7 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -7,17 +7,21 @@ import edu.wpi.first.wpilibj.RobotBase; /** - * Do NOT add any static variables to this class, or any initialization at all. Unless you know what - * you are doing, do not modify this file except to change the parameter class to the startRobot + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what + * you are doing, do not modify this file except to change the parameter class + * to the startRobot * call. */ public final class Main { - private Main() {} + private Main() { + } /** * Main initialization function. Do not perform any initialization here. * - *

If you change your main robot class, change the parameter type. + *

+ * If you change your main robot class, change the parameter type. */ public static void main(String... args) { RobotBase.startRobot(Robot::new); diff --git a/src/main/java/frc/robot/RebindHat.java b/src/main/java/frc/robot/RebindHat.java index 2868c6d..42b88f4 100644 --- a/src/main/java/frc/robot/RebindHat.java +++ b/src/main/java/frc/robot/RebindHat.java @@ -1,5 +1,6 @@ package frc.robot; - +/** @deprecated */ +@Deprecated public class RebindHat { public static double JoystickToYAxis() { if (RobotContainer.m_armController.getPOV() == -1 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 84ad454..be67fa4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,9 +12,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; /** - * The VM is configured to automatically run this class, and to call the functions corresponding to - * each mode, as described in the TimedRobot documentation. If you change the name of this class or - * the package after creating this project, you must also update the build.gradle file in the + * The VM is configured to automatically run this class, and to call the + * functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the + * name of this class or + * the package after creating this project, you must also update the + * build.gradle file in the * project. */ public class Robot extends TimedRobot { @@ -23,15 +26,18 @@ public class Robot extends TimedRobot { private RobotContainer m_robotContainer; /** - * This function is run when the robot is first started up and should be used for any + * This function is run when the robot is first started up and should be used + * for any * initialization code. */ @Override public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // Instantiate our RobotContainer. This will perform all our button bindings, + // and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - // Test values for the servo on startup (This is just a test that should be removed if it + // Test values for the servo on startup (This is just a test that should be + // removed if it // actually works) DataLogManager.start(); @@ -39,17 +45,23 @@ public void robotInit() { } /** - * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics + * This function is called every 20 ms, no matter the mode. Use this for items + * like diagnostics * that you want ran during disabled, autonomous, teleoperated and test. * - *

This runs after the mode specific periodic functions, but before LiveWindow and + *

+ * This runs after the mode specific periodic functions, but before LiveWindow + * and * SmartDashboard integrated updating. */ @Override public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic + // Runs the Scheduler. This is responsible for polling buttons, adding + // newly-scheduled + // commands, running already-scheduled commands, removing finished or + // interrupted commands, + // and running subsystem periodic() methods. This must be called from the + // robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); SmartDashboard.putData(CommandScheduler.getInstance()); @@ -57,12 +69,17 @@ public void robotPeriodic() { /** This function is called once each time the robot enters Disabled mode. */ @Override - public void disabledInit() {} + public void disabledInit() { + } @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + } - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + /** + * This autonomous runs the autonomous command selected by your + * {@link RobotContainer} class. + */ @Override public void autonomousInit() { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); @@ -75,7 +92,8 @@ public void autonomousInit() { /** This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() {} + public void autonomousPeriodic() { + } @Override public void teleopInit() { @@ -92,7 +110,8 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + } @Override public void testInit() { @@ -102,13 +121,16 @@ public void testInit() { /** This function is called periodically during test mode. */ @Override - public void testPeriodic() {} + public void testPeriodic() { + } /** This function is called once when the robot is first started up. */ @Override - public void simulationInit() {} + public void simulationInit() { + } /** This function is called periodically whilst in simulation. */ @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b278f07..61638c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -17,15 +17,23 @@ import io.github.oblarg.oblog.Logger; /** - * This class is where the bulk of the robot should be declared. Since Command-based is a - * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} - * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * This class is where the bulk of the robot should be declared. Since + * Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in + * the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of + * the robot (including * subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { public static boolean inverted = false; + // INSTANTIATES ALL JOYSTICKS + + /** @deprecated The drivers are much more comfortable with an xbox controller */ + @Deprecated public static final Joystick m_armController = new Joystick(0); + public static final XboxController m_armController2 = new XboxController(1); public static final XboxController m_driverController = new XboxController(2); @@ -45,19 +53,19 @@ public class RobotContainer { private final BalanceCommand m_BalanceCommand = new BalanceCommand(m_PID, m_Tank); private final AutoCommand m_AutoCommand = new AutoCommand(m_PID, m_Tank, m_Arm, m_Claw); private final InvertDriveCommand m_InvertDriveCommand = new InvertDriveCommand(m_Tank, this); - private final PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand = - new PlaceConeSecondLevelCommand(m_Tank, m_Arm, m_Claw); - private final IncreaseMaxSpeedCommand m_IncreaseMaxSpeedCommand = - new IncreaseMaxSpeedCommand(m_Tank); - private final DecreaseMaxSpeedCommand m_DecreaseMaxSpeedCommand = - new DecreaseMaxSpeedCommand(m_Tank); + private final PlaceConeSecondLevelCommand m_PlaceConeSecondLevelCommand = new PlaceConeSecondLevelCommand(m_Tank, + m_Arm, m_Claw); + private final IncreaseMaxSpeedCommand m_IncreaseMaxSpeedCommand = new IncreaseMaxSpeedCommand(m_Tank); + private final DecreaseMaxSpeedCommand m_DecreaseMaxSpeedCommand = new DecreaseMaxSpeedCommand(m_Tank); private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank); - private final PlaceCubeSecondLevelCommand m_PlaceCubeSecondLevelCommand = new PlaceCubeSecondLevelCommand(m_Tank, m_Arm, m_Claw); + private final PlaceCubeSecondLevelCommand m_PlaceCubeSecondLevelCommand = new PlaceCubeSecondLevelCommand(m_Tank, + m_Arm, m_Claw); - private GenericEntry timeWidget = Shuffleboard.getTab("stuff").add("time", 0).withWidget("Match Time").getEntry(); + private GenericEntry timeWidget = Shuffleboard.getTab("stuff").add("time", 0).withWidget("Match Time").getEntry(); - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ + /** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ public RobotContainer() { // Configure the button bindings configureButtonBindings(); @@ -76,9 +84,6 @@ public RobotContainer() { tab.add(m_PDP); tab.add(m_PlaceCubeSecondLevelCommand); - - - tab.add(m_PID); tab.add(m_PlaceConeSecondLevelCommand); tab.add(m_Tank); @@ -103,18 +108,23 @@ private void configureButtonBindings() { .toggleOnTrue(m_InvertDriveCommand); // new JoystickButton(m_armController2, XboxController.Button.kX.value) - // .whileTrue(m_CloseClawCommand); + // .whileTrue(m_CloseClawCommand); // new JoystickButton(m_armController2, XboxController.Button.kY.value) - // .whileTrue(m_OpenClawCommand); + // .whileTrue(m_OpenClawCommand); new JoystickButton(m_armController2, XboxController.Button.kLeftBumper.value) .whileTrue(m_FineDriveCommand); new JoystickButton(m_armController2, XboxController.Button.kY.value).whileTrue(m_PlaceConeSecondLevelCommand); // new JoystickButton(m_armController2, XboxController.Button.kB.value) - // .whileTrue(m_PlaceConeSecondLevelCommand); + // .whileTrue(m_PlaceConeSecondLevelCommand); } - public static double getDriverControllerLeftStickY() { + /** + * gets the value of the left stick on the driver controller to be used for + * turning + */ + public static double getTurningStickInput() { + // uses the left stick on the driver controller double axis = m_driverController.getRawAxis(1); // 0.60 is the minimum amount of power we need if (Math.abs(axis) < 0.01) { @@ -123,8 +133,11 @@ public static double getDriverControllerLeftStickY() { return axis * -1; } - public static double getDriverControllerLeftStickYAdjusted() { - double val = getDriverControllerLeftStickY(); + /** + * returns value used for turning + */ + public static double getAdjustedTurningStickInput() { + double val = getTurningStickInput(); if (val > 0) { return (val * val + 0.3) / 1.3; } else if (val < 0) { @@ -134,7 +147,11 @@ public static double getDriverControllerLeftStickYAdjusted() { } } - public static double getDriverControllerRightStickX() { + /** + * gets the value of the right stick on the driver controller to be used for + * forward and backward movement + */ + public static double getForwardStickInput() { double axis = m_driverController.getRawAxis(4); if (Math.abs(axis) < 0.01) { axis = 0; @@ -142,8 +159,9 @@ public static double getDriverControllerRightStickX() { return axis; } - public static double getDriverControllerRightStickXAdjusted() { - double val = getDriverControllerRightStickX(); + /** returns value used for forward and backward movement */ + public static double getAdjustedForwardStickInput() { + double val = getForwardStickInput(); if (val > 0) { return (val * val + 0.1) / 1.1; } else if (val < 0) { @@ -153,6 +171,15 @@ public static double getDriverControllerRightStickXAdjusted() { } } + /** + * This function gets the joystick value of the left stick on the arm controller + * The left stick is used to move the arm up and down + * The function returns the value of the left stick + * The value of the left stick is put on the smart dashboard + * + * @deprecated This controller is no longer used + */ + @Deprecated public static double getJoystickArmControllerLeftStickY() { double axis = m_armController.getRawAxis(1); SmartDashboard.putNumber("arm left stick y", axis); @@ -163,7 +190,19 @@ public static double getJoystickArmControllerLeftStickY() { return axis; } - public static double getJoystickArmControllerRightStickX() { + /** + * This function gets the joystick x-value of the right stick on the arm + * controller + * The right stick is used to move the arm left and right + * The function returns the value of the right stick + * The value of the right stick is put on the smart dashboard + * + * @deprecated The turret is no longer on the arm, and this controller is no + * longer used + */ + @Deprecated + public static double getJoystickArmControllerRightStickX() { // "do not forget to remove this deprecated code someday" + // lmao never double axis = m_armController.getRawAxis(4); SmartDashboard.putNumber("arm right stick x", axis); @@ -173,6 +212,11 @@ public static double getJoystickArmControllerRightStickX() { return axis; } + /** + * This function gets the y-value of the left stick on the arm controller + * The left stick is used to move the arm up and down + * The function returns the value of the left stick + */ public static double getControllerLeftStickY() { double axis = m_armController2.getRawAxis(1); if (Math.abs(axis) < 0.1) { @@ -189,7 +233,12 @@ public static double getControllerRightStickX() { return axis; } - public static double getControllerRightTrigger() { + /** + * gets value from the right trigger axis (yes, it's an axis) for grabbing up + * the game piece + */ + + public static double getClawInValue() { double axis = m_armController2.getRightTriggerAxis(); if (Math.abs(axis) < 0.01) { axis = 0; @@ -197,7 +246,11 @@ public static double getControllerRightTrigger() { return axis; } - public static double getControllerLeftTrigger() { + /** + * gets value from the left trigger axis (yes, it's an axis) for ejecting the + * game piece + */ + public static double getClawOutValue() { double axis = m_armController2.getLeftTriggerAxis(); if (Math.abs(axis) < 0.01) { axis = 0; @@ -211,9 +264,9 @@ public Command getAutonomousCommand() { } public void initTeleop() { - if(inverted){ + if (inverted) { m_Tank.setDefaultCommand(m_InvertDriveCommand); - } else{ + } else { m_Tank.setDefaultCommand(m_DriveCommand); } m_Arm.setDefaultCommand(m_MoveArmYCommand); @@ -222,7 +275,7 @@ public void initTeleop() { public void initTest() { // Set the default tank command to DriveCommand - + } public void robotPeriodic() { diff --git a/src/main/java/frc/robot/TrackingType.java b/src/main/java/frc/robot/TrackingType.java deleted file mode 100644 index a7e1d2d..0000000 --- a/src/main/java/frc/robot/TrackingType.java +++ /dev/null @@ -1,10 +0,0 @@ -package frc.robot; - -public enum TrackingType { - /** - * Literally just an enum I made because I'm too lazy to do anything else. Besides, it's good - * training. - */ - tape, - tag -} diff --git a/src/main/java/frc/robot/commands/AutoCommand.java b/src/main/java/frc/robot/commands/AutoCommand.java index f6db0b4..22a5e39 100644 --- a/src/main/java/frc/robot/commands/AutoCommand.java +++ b/src/main/java/frc/robot/commands/AutoCommand.java @@ -7,18 +7,34 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.subsystems.*; +/** + * This command will run the autonomous routine. In it, you can add commands to be run in sequence. + */ public class AutoCommand extends SequentialCommandGroup { public AutoCommand(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) { - addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw), new BalanceCommand(m_pid, m_driveBase)); - - //side autonomous - //addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw)); - //middle autonomous - //addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw), new BalanceCommand(m_pid, m_driveBase)); + //pick one + //sideAutoWCone(m_pid,m_driveBase,m_arm,m_claw); + //sideAutoWCube(m_pid,m_driveBase,m_arm,m_claw); + //middleAutoWCone(m_pid,m_driveBase,m_arm,m_claw); + //middleAutoWCube(m_pid,m_driveBase,m_arm,m_claw); - - // new BalanceCommand(m_pid, m_driveBase) - // new PlaceConeSecondLevelCommand(m_driveBase, m_arm) + } + // TODO document this + private void sideAutoWCone(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) + { + addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw)); + } + private void sideAutoWCube(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) + { + addCommands(new PlaceCubeSecondLevelCommand(m_driveBase, m_arm, m_claw)); + } + private void middleAutoWCone(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) + { + addCommands(new PlaceConeSecondLevelCommand(m_driveBase, m_arm, m_claw), new BalanceCommand(m_pid, m_driveBase)); + } + private void middleAutoWCube(PID m_pid, Tank m_driveBase, Arm m_arm, Claw m_claw) + { + addCommands(new PlaceCubeSecondLevelCommand(m_driveBase, m_arm, m_claw), new BalanceCommand(m_pid, m_driveBase)); } } diff --git a/src/main/java/frc/robot/commands/BalanceCommand.java b/src/main/java/frc/robot/commands/BalanceCommand.java index 46e0161..3b6ccc5 100644 --- a/src/main/java/frc/robot/commands/BalanceCommand.java +++ b/src/main/java/frc/robot/commands/BalanceCommand.java @@ -11,19 +11,20 @@ import frc.robot.subsystems.PID; import frc.robot.subsystems.Tank; -/** An example command that uses an example subsystem. */ +/** This command balances the robot on the charge station. Default auto command. */ public class BalanceCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final PID pid; + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) + private final PID pidLoop; private final Tank driveBase; - private double initPitch, max; + private double initPitch, maxPitch; private boolean onRamp; + /** Create a new {@link BalanceCommand}. */ public BalanceCommand(PID m_pid, Tank m_driveBase) { - pid = m_pid; + pidLoop = m_pid; driveBase = m_driveBase; - addRequirements(pid); + addRequirements(pidLoop); addRequirements(driveBase); } @@ -34,45 +35,48 @@ public void initialize() { driveBase.rightFront.setIdleMode(IdleMode.kBrake); driveBase.rightRear.setIdleMode(IdleMode.kBrake); - pid.resetPitch(); - initPitch = pid.gyro.getPitch(); - SmartDashboard.putNumber("init pitch", initPitch); - max = 0; + pidLoop.resetPitch(); + initPitch = pidLoop.gyro.getPitch(); + SmartDashboard.putNumber("inital pitch", initPitch); + maxPitch = 0; onRamp = false; + // start driving forward + driveBase.setAllMotors(0.5); } // avoid while loops inside execute @Override public void execute() { - double pitchOffset = initPitch - pid.gyro.getPitch(); + double pitchOffset = initPitch - pidLoop.gyro.getPitch(); SmartDashboard.putNumber("pitch offset", pitchOffset); - - if (pitchOffset > max) { - max = pitchOffset; + SmartDashboard.putBoolean("onRamp", onRamp); + SmartDashboard.putNumber("max offset", maxPitch); + SmartDashboard.putNumber( + "motor speed", Constants.OperatorConstants.Pconstant * (pitchOffset)); + if (pitchOffset > maxPitch) { + maxPitch = pitchOffset; } - SmartDashboard.putNumber("max offset", max); - - if (pitchOffset < 16 && !onRamp) { - driveBase.setAllMotors(0.5); - SmartDashboard.putBoolean("onRamp", onRamp); - } else { + // set boolean to true when robot is on ramp to run p loop + if (!onRamp && pitchOffset >= 16) { onRamp = true; - SmartDashboard.putBoolean("onRamp", onRamp); + } + if (onRamp) { + onRamp = true; + // dead zone of tilt if (Math.abs(pitchOffset) < 2) { driveBase.setAllMotors(0); } else { + // run p loop driveBase.setAllMotors(Constants.OperatorConstants.Pconstant * (pitchOffset)); - - SmartDashboard.putNumber( - "motor speed", Constants.OperatorConstants.Pconstant * (pitchOffset)); } } } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/DecreaseMaxSpeedCommand.java b/src/main/java/frc/robot/commands/DecreaseMaxSpeedCommand.java index 61aacc8..26c03c0 100644 --- a/src/main/java/frc/robot/commands/DecreaseMaxSpeedCommand.java +++ b/src/main/java/frc/robot/commands/DecreaseMaxSpeedCommand.java @@ -7,8 +7,9 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Tank; +/** Decrease the maximum speed of the drivebase. */ public class DecreaseMaxSpeedCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Tank m_subsystem; public DecreaseMaxSpeedCommand(Tank subsystem) { @@ -27,7 +28,8 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index f2f0127..62c7397 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -11,7 +11,7 @@ import frc.robot.subsystems.Tank; public class DriveCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Tank m_drivebase; public DriveCommand(Tank subsystem) { @@ -28,8 +28,8 @@ public void initialize() { public void execute() { // double check getMaxSpeed(), might be wrong Tank.arcadeDrive( - RobotContainer.getDriverControllerLeftStickYAdjusted() * Constants.CanConstants.maxSpeed, - RobotContainer.getDriverControllerRightStickXAdjusted() * 0.7); + /* rotation */RobotContainer.getAdjustedTurningStickInput() * Constants.CanConstants.maxSpeed, + /* speed */RobotContainer.getAdjustedForwardStickInput() * 0.7); SmartDashboard.putNumber("maxSpeed", Constants.CanConstants.maxSpeed); } diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java index 63992c9..dfcf11d 100644 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ b/src/main/java/frc/robot/commands/ExampleCommand.java @@ -8,7 +8,7 @@ import frc.robot.subsystems.ExampleSubsystem; public class ExampleCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField", "PMD.UnusedImport"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField", "PMD.UnusedImport" }) private final ExampleSubsystem m_subsystem; // NOPMD public ExampleCommand(ExampleSubsystem subsystem) { diff --git a/src/main/java/frc/robot/commands/FineDriveCommand.java b/src/main/java/frc/robot/commands/FineDriveCommand.java index 1369f1a..1370a13 100644 --- a/src/main/java/frc/robot/commands/FineDriveCommand.java +++ b/src/main/java/frc/robot/commands/FineDriveCommand.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.Tank; public class FineDriveCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Tank m_drivebase; public FineDriveCommand(Tank subsystem) { diff --git a/src/main/java/frc/robot/commands/IncreaseMaxSpeedCommand.java b/src/main/java/frc/robot/commands/IncreaseMaxSpeedCommand.java index f9be623..eb5287c 100644 --- a/src/main/java/frc/robot/commands/IncreaseMaxSpeedCommand.java +++ b/src/main/java/frc/robot/commands/IncreaseMaxSpeedCommand.java @@ -8,7 +8,7 @@ import frc.robot.subsystems.Tank; public class IncreaseMaxSpeedCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Tank m_subsystem; public IncreaseMaxSpeedCommand(Tank subsystem) { @@ -26,7 +26,8 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/InvertDriveCommand.java b/src/main/java/frc/robot/commands/InvertDriveCommand.java index 2f076b6..b4540fd 100644 --- a/src/main/java/frc/robot/commands/InvertDriveCommand.java +++ b/src/main/java/frc/robot/commands/InvertDriveCommand.java @@ -12,7 +12,7 @@ import frc.robot.subsystems.Tank; public class InvertDriveCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Tank m_drivebase; private final RobotContainer robot; @@ -33,8 +33,8 @@ public void initialize() { public void execute() { // double check getMaxSpeed(), might be wrong m_drivebase.arcadeDrive( - -RobotContainer.getDriverControllerLeftStickYAdjusted() * Constants.CanConstants.maxSpeed, - RobotContainer.getDriverControllerRightStickXAdjusted() * 0.75); + -RobotContainer.getAdjustedTurningStickInput() * Constants.CanConstants.maxSpeed, + RobotContainer.getAdjustedForwardStickInput() * 0.75); } @Override diff --git a/src/main/java/frc/robot/commands/MoveArmXCommand.java b/src/main/java/frc/robot/commands/MoveArmXCommand.java index 2049149..3098b2f 100644 --- a/src/main/java/frc/robot/commands/MoveArmXCommand.java +++ b/src/main/java/frc/robot/commands/MoveArmXCommand.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.Turret; public class MoveArmXCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Turret m_subsystem; public MoveArmXCommand(Turret subsystem) { @@ -18,7 +18,8 @@ public MoveArmXCommand(Turret subsystem) { } @Override - public void initialize() {} + public void initialize() { + } @Override public void execute() { @@ -26,7 +27,8 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/MoveArmYCommand.java b/src/main/java/frc/robot/commands/MoveArmYCommand.java index cf5c3a4..8559d65 100644 --- a/src/main/java/frc/robot/commands/MoveArmYCommand.java +++ b/src/main/java/frc/robot/commands/MoveArmYCommand.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.Arm; public class MoveArmYCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Arm m_subsystem; public MoveArmYCommand(Arm subsystem) { @@ -18,7 +18,8 @@ public MoveArmYCommand(Arm subsystem) { } @Override - public void initialize() {} + public void initialize() { + } @Override public void execute() { @@ -26,7 +27,8 @@ public void execute() { } @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/commands/MoveClawCommand.java b/src/main/java/frc/robot/commands/MoveClawCommand.java index b2a1797..5b8c433 100644 --- a/src/main/java/frc/robot/commands/MoveClawCommand.java +++ b/src/main/java/frc/robot/commands/MoveClawCommand.java @@ -9,7 +9,7 @@ import frc.robot.subsystems.Claw; public class MoveClawCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Claw m_subsystem; public MoveClawCommand(Claw subsystem) { @@ -18,14 +18,15 @@ public MoveClawCommand(Claw subsystem) { } @Override - public void initialize() {} + public void initialize() { + } @Override public void execute() { - if(Math.abs(RobotContainer.getControllerLeftTrigger()) < 0.2){ - m_subsystem.clawMotor.set(RobotContainer.getControllerRightTrigger() * 0.7); - } else{ - m_subsystem.clawMotor.set(-RobotContainer.getControllerLeftTrigger() * 0.7); + if (Math.abs(RobotContainer.getClawOutValue()) < 0.2) { + m_subsystem.clawMotor.set(RobotContainer.getClawInValue() * 0.7); + } else { + m_subsystem.clawMotor.set(-RobotContainer.getClawOutValue() * 0.7); } } diff --git a/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java b/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java index 4cc2b42..3ca2cde 100644 --- a/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java +++ b/src/main/java/frc/robot/commands/PlaceConeSecondLevelCommand.java @@ -1,6 +1,7 @@ -// Copyright (c) FIRST and other WPILib contributors. +// Copyright (c) Elliot Snyder, Parker Brownlowe, and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. +// the WTFPL license file in the internet. Any questions about the mental +// health of the authors can be shouted into the endless void of the internet. package frc.robot.commands; @@ -8,12 +9,14 @@ import frc.robot.subsystems.*; public class PlaceConeSecondLevelCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Tank drive; private final Claw claw; private final Arm arm; - + private boolean coneOut; + private boolean clawDown; private boolean firstStep = true; + private boolean armBack; public PlaceConeSecondLevelCommand(Tank drive, Arm arm, Claw claw) { this.drive = drive; @@ -31,34 +34,35 @@ public void initialize() { drive.rightRear.getEncoder().setPosition(0); arm.armMotor.getEncoder().setPosition(0); + // starting movement + claw.closeClaw(); + arm.armMotor.set(0.5); } // double check constants @Override public void execute() { - if (firstStep) { - if (Math.abs(arm.armMotor.getEncoder().getPosition()) < 115) { - claw.closeClaw(); - arm.armMotor.set(0.5); - } - - if (Math.abs(arm.armMotor.getEncoder().getPosition()) >= 115) { - firstStep = false; - arm.armMotor.set(0); - arm.armMotor.getEncoder().setPosition(0); - } - - // go over charge station - if (Math.abs(drive.getAverageDistance()) < 4.95 && firstStep == false) { - claw.stopClaw(); - drive.setAllMotors(-0.3); // move back so that cone falls in - arm.armMotor.set(-0.5); - } - if (Math.abs(drive.getAverageDistance()) >= 4.95) { - drive.setAllMotors(0); - } + if (!clawDown && Math.abs(arm.armMotor.getEncoder().getPosition()) >= 115) { + claw.stopClaw(); + arm.armMotor.set(0); + drive.setAllMotors(-0.3); // move back so that cone falls in + clawDown = true; + } + if (!coneOut && Math.abs(drive.getAverageDistance()) >= 1.00) { + coneOut = true; + arm.armMotor.set(-0.5); + } + if (coneOut && !armBack && arm.armMotor.getEncoder().getPosition() < 10) { + armBack = true; + arm.armMotor.set(0); } + // stops after reaching a certian point + /* + * if (Math.abs(drive.getAverageDistance()) >= 4.95) { + * drive.setAllMotors(0); + * } + */ } @Override @@ -71,6 +75,7 @@ public void end(boolean interrupted) { drive.setAllMotors(0); arm.armMotor.set(0); // stop the arm motor + claw.stopClaw(); } @Override diff --git a/src/main/java/frc/robot/commands/PlaceCubeSecondLevelCommand.java b/src/main/java/frc/robot/commands/PlaceCubeSecondLevelCommand.java index 56bda22..1da8e44 100644 --- a/src/main/java/frc/robot/commands/PlaceCubeSecondLevelCommand.java +++ b/src/main/java/frc/robot/commands/PlaceCubeSecondLevelCommand.java @@ -10,13 +10,15 @@ import frc.robot.subsystems.*; public class PlaceCubeSecondLevelCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" }) private final Tank drive; private final Claw claw; private final Arm arm; private GenericEntry widget; - + private int counter; + private boolean finalStep; private boolean firstStep = true; + private boolean done; public PlaceCubeSecondLevelCommand(Tank drive, Arm arm, Claw claw) { this.drive = drive; @@ -24,69 +26,73 @@ public PlaceCubeSecondLevelCommand(Tank drive, Arm arm, Claw claw) { this.claw = claw; addRequirements(drive); addRequirements(arm); - - } @Override - public void initialize() { -/* drive.leftFront.getEncoder().setPosition(0); - drive.leftRear.getEncoder().setPosition(0); - drive.rightFront.getEncoder().setPosition(0); - drive.rightRear.getEncoder().setPosition(0); */ - + public void initialize() { + /* + * drive.leftFront.getEncoder().setPosition(0); + * drive.leftRear.getEncoder().setPosition(0); + * drive.rightFront.getEncoder().setPosition(0); + * drive.rightRear.getEncoder().setPosition(0); + */ + counter = 0; + finalStep = false; + done = false; arm.armMotor.getEncoder().setPosition(0); widget = Shuffleboard.getTab("stuff").add("arm thing", arm.armMotor.getEncoder().getPosition()).getEntry(); - + claw.closeClaw(); + arm.armMotor.set(0.25); } // double check constants @Override public void execute() { widget.setDouble(arm.armMotor.getEncoder().getPosition()); - - if (firstStep) { - claw.closeClaw(); - arm.armMotor.set(0.25); - } - - - if (arm.armMotor.getEncoder().getPosition() >= 60) { - firstStep = false; - arm.armMotor.set(0); - arm.armMotor.getEncoder().setPosition(0); - claw.openClaw(); - } - - // go over charge station -/* if (Math.abs(drive.getAverageDistance()) < 4.95 && firstStep == false) { - claw.stopClaw(); - drive.setAllMotors(-0.3); // move back so that cone falls in - arm.armMotor.set(-0.5); - } - - if (Math.abs(drive.getAverageDistance()) >= 4.95) { - drive.setAllMotors(0); - } */ - + counter++; + + if (arm.armMotor.getEncoder().getPosition() >= 60 && firstStep) { + firstStep = false; + arm.armMotor.set(0); + claw.openClaw(); + int counter = 0; + } + + // go over charge station + + if (counter > 10 && !firstStep && !finalStep) { + claw.stopClaw(); + drive.setAllMotors(-0.3); + arm.armMotor.set(-0.5); + finalStep = true; + } + + if (Math.abs(drive.getAverageDistance()) >= 4.95 && finalStep) { + drive.setAllMotors(0); + done = true; + } + } @Override public void end(boolean interrupted) { -/* drive.leftFront.getEncoder().setPosition(0); - drive.leftRear.getEncoder().setPosition(0); - drive.rightFront.getEncoder().setPosition(0); - drive.rightRear.getEncoder().setPosition(0); */ + /* + * drive.leftFront.getEncoder().setPosition(0); + * drive.leftRear.getEncoder().setPosition(0); + * drive.rightFront.getEncoder().setPosition(0); + * drive.rightRear.getEncoder().setPosition(0); + */ arm.armMotor.getEncoder().setPosition(0); - //drive.setAllMotors(0); + // drive.setAllMotors(0); arm.armMotor.set(0); // stop the arm motor + } @Override public boolean isFinished() { -/* return Math.abs(drive.getAverageDistance()) >= 4.95; */ -return false; + /* return Math.abs(drive.getAverageDistance()) >= 4.95; */ + return false; } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 47f7b68..a7daa40 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -4,15 +4,17 @@ package frc.robot.subsystems; +import com.revrobotics.REVPhysicsSim; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Interfaces.CANSparkMax; +import frc.robot.interfaces.CANSparkMax; public class Arm extends SubsystemBase { @@ -23,9 +25,10 @@ public class Arm extends SubsystemBase { public double gearRatio = 87; - public GenericEntry encoderWidget; + public REVPhysicsSim physicsSim; + public Arm() { armMotor = new CANSparkMax(7, MotorType.kBrushless); armMotor.setIdleMode(IdleMode.kBrake); @@ -34,10 +37,15 @@ public Arm() { addChild("Arm Motor", armMotor); encoderWidget = Shuffleboard.getTab("Motors").add("Arm Encoder", armMotor.getEncoder().getPosition()).getEntry(); - Shuffleboard.getTab("Motors").add("Arm", armMotor); - + + // add simulation data + if (RobotBase.isSimulation()) { + physicsSim = new REVPhysicsSim(); + physicsSim.addSparkMax(armMotor, 5000, 20); + } + } @Override diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index bf6b9c7..d8e2e9b 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -15,13 +15,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotContainer; -import frc.robot.Interfaces.*; - +import frc.robot.interfaces.*; public class Claw extends SubsystemBase { public CANSparkMax clawMotor; - // gear ratio is 100:1 public Claw() { @@ -37,7 +35,7 @@ public Claw() { @Override public void periodic() { - SmartDashboard.putNumber("rightTrigger", RobotContainer.getControllerRightTrigger()); + SmartDashboard.putNumber("rightTrigger", RobotContainer.getClawInValue()); } public void closeClaw() { diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java index caaef81..210f136 100644 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java @@ -8,7 +8,8 @@ public class ExampleSubsystem extends SubsystemBase { /** Creates a new ExampleSubsystem. */ - public ExampleSubsystem() {} + public ExampleSubsystem() { + } /** * Example command factory method. @@ -17,7 +18,8 @@ public ExampleSubsystem() {} */ /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). + * An example method querying a boolean state of the subsystem (for example, a + * digital sensor). * * @return value of some boolean subsystem state, such as a digital sensor. */ diff --git a/src/main/java/frc/robot/subsystems/PDP.java b/src/main/java/frc/robot/subsystems/PDP.java index b03c3c5..f356923 100644 --- a/src/main/java/frc/robot/subsystems/PDP.java +++ b/src/main/java/frc/robot/subsystems/PDP.java @@ -1,11 +1,14 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.PDPSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -13,6 +16,7 @@ public class PDP extends SubsystemBase { public PowerDistribution pdp; ShuffleboardTab tab; ComplexWidget widget; + private PDPSim pdpSim; public PDP() { pdp = new PowerDistribution(0, ModuleType.kCTRE); @@ -21,6 +25,11 @@ public PDP() { tab = Shuffleboard.getTab("PDP"); addChild("PDP", pdp); widget = tab.add("PDP", pdp).withWidget(BuiltInWidgets.kPowerDistribution); + if (!RobotBase.isReal()) { + // this is a simulation, so add a simulated PDP + pdpSim = new PDPSim(pdp); + + } } @Override @@ -28,33 +37,43 @@ public void periodic() { // This method will be called once per scheduler run } + /** * @return the voltage of the PDP in Volts */ public double getVoltage() { return pdp.getVoltage(); } + /** * @return the temperature of the PDP in degrees Celsius */ public double getTemperature() { return pdp.getTemperature(); } + /** - * @param param the channel to get the current from, or any string to get the total current - * @return the current drawn from the specified channel, or all channels, in Amps + * @param param the channel to get the current from, or any string to get the + * total current + * @return the current drawn from the specified channel, or all channels, in + * Amps */ public double getCurrent(Object param) { - if (param instanceof Integer) return pdp.getCurrent((int) param); - else if (param instanceof String) return pdp.getTotalCurrent(); - else return 0; + if (param instanceof Integer) + return pdp.getCurrent((int) param); + else if (param instanceof String) + return pdp.getTotalCurrent(); + else + return 0; } + /** * @return the total current drawn from the PDP in Amps */ public double getTotalCurrent() { return pdp.getTotalCurrent(); } + /** * @return the total power drawn from the PDP in Watts */ diff --git a/src/main/java/frc/robot/subsystems/PID.java b/src/main/java/frc/robot/subsystems/PID.java index a189b63..086327a 100644 --- a/src/main/java/frc/robot/subsystems/PID.java +++ b/src/main/java/frc/robot/subsystems/PID.java @@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class PID extends SubsystemBase { - + private final double kP = 0.18; private final double kI = 0.025; private final double kD = 0.3; @@ -25,12 +25,10 @@ public class PID extends SubsystemBase { private float initPitch; - public PID() { toleranceDeg = 0.5; iLimit = 2.0; - initPitch = gyro.getPitch(); } @@ -67,7 +65,6 @@ public double PIDoutput() { I = kI * errorSum; D = kD * errorRate; - double outputSpeed = P + I + D; return outputSpeed; @@ -82,5 +79,6 @@ private float getPitchOffset() { } @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + } } diff --git a/src/main/java/frc/robot/subsystems/Tank.java b/src/main/java/frc/robot/subsystems/Tank.java index 0ffbf22..3fd1239 100644 --- a/src/main/java/frc/robot/subsystems/Tank.java +++ b/src/main/java/frc/robot/subsystems/Tank.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import com.kauailabs.navx.frc.AHRS; +import com.revrobotics.REVPhysicsSim; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -15,8 +16,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.Interfaces.*; import frc.robot.RobotContainer; +import frc.robot.interfaces.*; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.SPI; public class Tank extends SubsystemBase { @@ -51,8 +53,6 @@ public class Tank extends SubsystemBase { private ShuffleboardTab motorsTab; - - private DifferentialDriveOdometry odometry; public static AHRS gyro; @@ -61,6 +61,8 @@ public class Tank extends SubsystemBase { private int initialCurrentLimit = 30; // TODO tune this + private REVPhysicsSim physicsSim; + /** */ public Tank() { @@ -146,8 +148,6 @@ public Tank() { rightFront.setSmartCurrentLimit(40, 50); rightRear.setSmartCurrentLimit(40, 50); - - // implement odometry odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(gyro.getAngle()), Units.inchesToMeters( @@ -160,13 +160,24 @@ public Tank() { field2d = new Field2d(); SmartDashboard.putData(field2d); + // create a physics simulation for the robot + if (RobotBase.isSimulation()) { + physicsSim = new REVPhysicsSim(); + // add all the spark maxes to the simulation + physicsSim.addSparkMax(leftFront, (float) 2.6, 445); + physicsSim.addSparkMax(leftRear, (float) 2.6, 445); + physicsSim.addSparkMax(rightFront, (float) 2.6, 445); + physicsSim.addSparkMax(rightRear, (float) 2.6, 445); + // run the simulation + physicsSim.run(); + } + } @Override public void periodic() { - SmartDashboard.putNumber("leftStickY", RobotContainer.getDriverControllerLeftStickY()); - SmartDashboard.putNumber("rightStickX", RobotContainer.getDriverControllerRightStickX()); - + SmartDashboard.putNumber("leftStickY", RobotContainer.getTurningStickInput()); + SmartDashboard.putNumber("rightStickX", RobotContainer.getForwardStickInput()); // i hate my life @@ -192,9 +203,9 @@ public double getMaxSpeed() { * @param rotation The rotation speed. */ public static void arcadeDrive(double speed, double rotation) { - - drive.arcadeDrive( - -speed * Math.pow(Math.abs(speed), 0.5), rotation * Math.pow(Math.abs(rotation), 0.5)); + + drive.arcadeDrive( + -speed * Math.pow(Math.abs(speed), 0.5), rotation * Math.pow(Math.abs(rotation), 0.5)); } @@ -256,11 +267,11 @@ public void setRampRate(double rampRate) { // for some reason -speed is forward. dont ask me why public void setAllMotors(double speed) { - - leftFront.set(-speed); - leftRear.set(-speed); - rightFront.set(-speed); - rightRear.set(-speed); + + leftFront.set(-speed); + leftRear.set(-speed); + rightFront.set(-speed); + rightRear.set(-speed); } diff --git a/src/main/java/frc/robot/subsystems/Turret.java b/src/main/java/frc/robot/subsystems/Turret.java index 3d22d4a..4d3810c 100644 --- a/src/main/java/frc/robot/subsystems/Turret.java +++ b/src/main/java/frc/robot/subsystems/Turret.java @@ -9,7 +9,7 @@ import com.revrobotics.RelativeEncoder; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Interfaces.CANSparkMax; +import frc.robot.interfaces.CANSparkMax; public class Turret extends SubsystemBase { private CANSparkMax xMotor; diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 4cb5a34..b5f9637 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -21,11 +21,16 @@ public class Vision extends SubsystemBase { private double goalHeightInches = 24.125; private double angleToGoalRadians = limelightMountAngleDegrees + targetOffsetAngle_Vertical; private double angleToGoalDegrees = Units.radiansToDegrees(angleToGoalRadians); - private double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches)/Math.tan(angleToGoalRadians); + private double distanceFromLimelightToGoalInches = (goalHeightInches - limelightLensHeightInches) + / Math.tan(angleToGoalRadians); public GenericEntry widget; + public Vision() { // Stuff goes here - widget = Shuffleboard.getTab("stuff").add("distance", -(goalHeightInches - limelightLensHeightInches)/Math.tan(limelightMountAngleDegrees + ty.getDouble(0))).withWidget(BuiltInWidgets.kGraph).getEntry(); + widget = Shuffleboard.getTab("stuff") + .add("distance", + -(goalHeightInches - limelightLensHeightInches) / Math.tan(limelightMountAngleDegrees + ty.getDouble(0))) + .withWidget(BuiltInWidgets.kGraph).getEntry(); } // display camera on shuffleboard @@ -33,8 +38,9 @@ public Vision() { @Override public void periodic() { // This method will be called once per scheduler run - widget.setDouble(-(goalHeightInches - limelightLensHeightInches)/Math.tan(limelightMountAngleDegrees + ty.getDouble(0))); - + widget.setDouble( + -(goalHeightInches - limelightLensHeightInches) / Math.tan(limelightMountAngleDegrees + ty.getDouble(0))); + } @Override @@ -48,8 +54,7 @@ public void simulationPeriodic() { * @param property The property to fetch. * @return The fetched property. */ - public double[] - getProperties() { // we don't know what type we return until we process it in the if statement + public double[] getProperties() { // we don't know what type we return until we process it in the if statement // var tx = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); var ty = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); @@ -57,15 +62,11 @@ public void simulationPeriodic() { var ta = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ta").getDouble(0); var tl = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tl").getDouble(0); var cl = NetworkTableInstance.getDefault().getTable("limelight").getEntry("cl").getDouble(0); - var tshort = - NetworkTableInstance.getDefault().getTable("limelight").getEntry("tshort").getDouble(0); - var tlong = - NetworkTableInstance.getDefault().getTable("limelight").getEntry("tlong").getDouble(0); - var thor = - NetworkTableInstance.getDefault().getTable("limelight").getEntry("thor").getDouble(0); - var tvert = - NetworkTableInstance.getDefault().getTable("limelight").getEntry("tvert").getDouble(0); + var tshort = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tshort").getDouble(0); + var tlong = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tlong").getDouble(0); + var thor = NetworkTableInstance.getDefault().getTable("limelight").getEntry("thor").getDouble(0); + var tvert = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tvert").getDouble(0); - return new double[] {tx, ty, tv, ta, tl, cl, tshort, tlong, thor, tvert}; + return new double[] { tx, ty, tv, ta, tl, cl, tshort, tlong, thor, tvert }; } }