diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 99c6348..09cfa4f 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -42,7 +42,8 @@ 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 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; @@ -84,7 +85,8 @@ public static final class Community { 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 + 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); @@ -138,13 +140,15 @@ public static final class Grids { 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); + 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 + 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] @@ -187,7 +191,8 @@ public static final class LoadingZone { 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 + midX, rightY), // Start at lower left next to border with opponent + // community new Translation2d(midX, midY), new Translation2d(outerX, midY), new Translation2d(outerX, leftY), @@ -205,14 +210,17 @@ public static final class LoadingZone { 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 singleSubstationCenterX = singleSubstationLeftX + + (singleSubstationWidth / 2.0); public static final double singleSubstationRightX = singleSubstationLeftX + singleSubstationWidth; - public static final Translation2d singleSubstationTranslation = new Translation2d(singleSubstationCenterX, + 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 singleSubstationCenterZ = singleSubstationLowZ + + (singleSubstationHeight / 2.0); public static final double singleSubstationHighZ = singleSubstationLowZ + singleSubstationHeight; } @@ -242,28 +250,32 @@ public static final class StagingLocations { Units.inchesToMeters(610.125), Units.inchesToMeters(43.5), Units.inchesToMeters(19.25), - new Rotation3d(0.0, 0.0, Math.PI))), + 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 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 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 Rotation3d(0.0, 0.0, + Math.PI))), new AprilTag( 5, new Pose3d( @@ -302,28 +314,32 @@ public static final class StagingLocations { Units.inchesToMeters(610.77), Grids.nodeY[1], Units.inchesToMeters(18.22), - new Rotation3d(0.0, 0.0, Math.PI))), + 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 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 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 Rotation3d(0.0, 0.0, + Math.PI))), new AprilTag( 5, new Pose3d( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3bb2f33..157966b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,6 +50,7 @@ public class RobotContainer { private final FineDriveCommand m_FineDriveCommand = new FineDriveCommand(m_Tank); private final LineupCommand m_LineupCommand = new LineupCommand(m_LineupPID); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the button bindings @@ -207,4 +208,9 @@ public void initTeleop() { m_Turret.setDefaultCommand(m_MoveArmXCommand); m_Arm.setDefaultCommand(m_MoveArmYCommand); } + + public void initTest() { + // Set the default tank command to DriveCommand + + } } diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java index e5093bd..15e59ee 100644 --- a/src/main/java/frc/robot/commands/DriveCommand.java +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -20,12 +20,14 @@ public DriveCommand(Tank subsystem) { } @Override - public void initialize() {} + public void initialize() { + // nothing to do here + } @Override public void execute() { // double check getMaxSpeed(), might be wrong - m_drivebase.arcadeDrive( + Tank.arcadeDrive( RobotContainer.getDriverControllerLeftStickYAdjusted() * Constants.CanConstants.maxSpeed, RobotContainer.getDriverControllerRightStickXAdjusted() * Constants.CanConstants.maxSpeed); @@ -34,7 +36,7 @@ public void execute() { @Override public void end(boolean interrupted) { - m_drivebase.arcadeDrive(0, 0); + Tank.arcadeDrive(0, 0); } @Override diff --git a/src/main/java/frc/robot/commands/FineDriveCommand.java b/src/main/java/frc/robot/commands/FineDriveCommand.java index 29e9eb4..1369f1a 100644 --- a/src/main/java/frc/robot/commands/FineDriveCommand.java +++ b/src/main/java/frc/robot/commands/FineDriveCommand.java @@ -18,18 +18,20 @@ public FineDriveCommand(Tank subsystem) { } @Override - public void initialize() {} + public void initialize() { + // nothing to do here + } @Override public void execute() { // double check getMaxSpeed(), might be wrong - m_drivebase.arcadeDrive( + Tank.arcadeDrive( RebindHat.ControllerToYAxis() * 0.43, RebindHat.ControllerToXAxis() * 0.43); } @Override public void end(boolean interrupted) { - m_drivebase.arcadeDrive(0, 0); + Tank.arcadeDrive(0, 0); } @Override diff --git a/src/main/java/frc/robot/commands/tests/TestDrivebaseCommand.java b/src/main/java/frc/robot/commands/tests/TestDrivebaseCommand.java index 785746d..2721625 100644 --- a/src/main/java/frc/robot/commands/tests/TestDrivebaseCommand.java +++ b/src/main/java/frc/robot/commands/tests/TestDrivebaseCommand.java @@ -14,27 +14,27 @@ public TestDrivebaseCommand(Tank subsystem) { @Override public void execute() { - m_drivebase.arcadeDrive(1, 0); + Tank.arcadeDrive(1, 0); Timer.delay(1); - m_drivebase.arcadeDrive(0, 0); + Tank.arcadeDrive(0, 0); Timer.delay(1); - m_drivebase.arcadeDrive(-1, 0); + Tank.arcadeDrive(-1, 0); Timer.delay(1); - m_drivebase.arcadeDrive(0, 0); + Tank.arcadeDrive(0, 0); Timer.delay(1); - m_drivebase.arcadeDrive(0, 1); + Tank.arcadeDrive(0, 1); Timer.delay(1); - m_drivebase.arcadeDrive(0, 0); + Tank.arcadeDrive(0, 0); Timer.delay(1); - m_drivebase.arcadeDrive(0, -1); + Tank.arcadeDrive(0, -1); Timer.delay(1); - m_drivebase.arcadeDrive(0, 0); + Tank.arcadeDrive(0, 0); Timer.delay(1); } @Override public void end(boolean interrupted) { - m_drivebase.arcadeDrive(0, 0); + Tank.arcadeDrive(0, 0); } @Override diff --git a/src/main/java/frc/robot/subsystems/Tank.java b/src/main/java/frc/robot/subsystems/Tank.java index ac258f7..d3cec7c 100644 --- a/src/main/java/frc/robot/subsystems/Tank.java +++ b/src/main/java/frc/robot/subsystems/Tank.java @@ -188,9 +188,11 @@ public Tank() { // implement odometry odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(gyro.getAngle()), Units.inchesToMeters( - (leftFront.getEncoder().getPosition() / 10) * (leftFront.getEncoder().getPosition() / 10) * Math.PI), + (leftFront.getEncoder().getPosition() / 10) * (leftFront.getEncoder().getPosition() / 10) + * Math.PI), Units.inchesToMeters( - (rightFront.getEncoder().getPosition() / 10) * (rightFront.getEncoder().getPosition() / 10) * Math.PI)); + (rightFront.getEncoder().getPosition() / 10) * (rightFront.getEncoder().getPosition() / 10) + * Math.PI)); field2d = new Field2d(); SmartDashboard.putData(field2d); @@ -231,9 +233,11 @@ public void periodic() { odometry.update(Rotation2d.fromDegrees(gyro.getAngle()), Units.inchesToMeters( - (leftFront.getEncoder().getPosition() / 10) * (leftFront.getEncoder().getPosition() / 10) * Math.PI), + (leftFront.getEncoder().getPosition() / 10) * (leftFront.getEncoder().getPosition() / 10) + * Math.PI), Units.inchesToMeters( - (rightFront.getEncoder().getPosition() / 10) * (rightFront.getEncoder().getPosition() / 10) * Math.PI)); + (rightFront.getEncoder().getPosition() / 10) * (rightFront.getEncoder().getPosition() / 10) + * Math.PI)); field2d.setRobotPose(odometry.getPoseMeters()); } @@ -306,14 +310,11 @@ public void simulationPeriodic() { /** Change the ramp rate of the motor controllers. */ public void setRampRate(double rampRate) { - try { - leftFront.setOpenLoopRampRate(rampRate); - leftRear.setOpenLoopRampRate(rampRate); - rightFront.setOpenLoopRampRate(rampRate); - rightRear.setOpenLoopRampRate(rampRate); - } catch (Exception e) { - throw e; - } + leftFront.setOpenLoopRampRate(rampRate); + leftRear.setOpenLoopRampRate(rampRate); + rightFront.setOpenLoopRampRate(rampRate); + rightRear.setOpenLoopRampRate(rampRate); + } // for some reason -speed is forward. dont ask me why