diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5d67938..04f133d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -162,6 +162,8 @@ public class RobotContainer { driver.a().whileTrue(manipulator.runManipulator(() -> -0.5, false)); driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true)); + + driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true)); driver.start().whileTrue(drivetrain.resetToVision()); @@ -224,18 +226,18 @@ public class RobotContainer { operator.x().onTrue( safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition) - .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .alongWith(manipulator.runManipulator(() -> 0.85, false)) .until(() -> driver.a().getAsBoolean()) ); operator.b().onTrue( safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition) - .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .alongWith(manipulator.runManipulator(() -> 0.85, false)) .until(() -> driver.a().getAsBoolean()) ); operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition) - .alongWith(manipulator.runManipulator(() -> 0.5, false)) + .alongWith(manipulator.runManipulator(() -> 0.85, false)) .until(() -> driver.a().getAsBoolean()) ); @@ -421,30 +423,30 @@ public class RobotContainer { //sensorTab.add("odometry", drivetrain::getPose); apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag()) - .withSize(2,1).withPosition(1,1); + .withSize(1,1).withPosition(1,1); apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX()) - .withSize(2,1).withPosition(3,1); + .withSize(1,1).withPosition(2,1); apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY()) - .withSize(2,1).withPosition(5,1); + .withSize(1,1).withPosition(3,1); apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist()) - .withSize(2,1).withPosition(7,1); + .withSize(1,1).withPosition(4,1); apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS()) - .withSize(2,1).withPosition(9,1); + .withSize(1,1).withPosition(5,1); apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected()) - .withSize(2,1).withPosition(11,1); + .withSize(1,1).withPosition(6,1); apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag()) - .withSize(2,1).withPosition(1,2); + .withSize(1,1).withPosition(1,2); apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX()) - .withSize(2,1).withPosition(3,2); + .withSize(1,1).withPosition(2,2); apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY()) - .withSize(2,1).withPosition(5,2); + .withSize(1,1).withPosition(3,2); apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist()) - .withSize(2,1).withPosition(7,2); + .withSize(1,1).withPosition(4,2); apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS()) - .withSize(2,1).withPosition(7,2); + .withSize(1,1).withPosition(5,2); apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected()) - .withSize(2,1).withPosition(9,2); + .withSize(1,1).withPosition(6,2); apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag()) .withSize(2,1).withPosition(4,4); diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 369fd6e..0ba9841 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -19,10 +19,16 @@ public class AutoConstants { public static final double kPXYController = 3.5; public static final double kPThetaController = 5; + public static final double kAlignPXYController = 2; + public static final double kAlignPThetaController = 5; + // Constraint for the motion profiled robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared); + // TODO This is a constant being managed like a static rewriteable variable public static RobotConfig kRobotConfig; diff --git a/src/main/java/frc/robot/constants/ElevatorConstants.java b/src/main/java/frc/robot/constants/ElevatorConstants.java index 65956aa..8fa220b 100644 --- a/src/main/java/frc/robot/constants/ElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ElevatorConstants.java @@ -42,7 +42,7 @@ public class ElevatorConstants { public static final double kMaxAcceleration = 240; // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2 public static final double kCoralIntakePosition = 0; - public static final double kL1Position = 14; + public static final double kL1Position = 17; public static final double kL2Position = 11; public static final double kL3Position = 27; public static final double kL4Position = 50.5; diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index f0dd5a9..e5c4693 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -115,13 +115,13 @@ public class Drivetrain extends SubsystemBase { VecBuilder.fill(1, 1, Units.degreesToRadians(360)) ); - pidHeading = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints); + pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints); pidHeading.setTolerance(Units.degreesToRadians(3)); pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180)); - pidTranslationX = new PIDController(AutoConstants.kPXYController,0,0); + pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,0,0); pidTranslationX.setTolerance(Units.inchesToMeters(0.5)); - pidTranslationY = new PIDController(AutoConstants.kPXYController,0,0); + pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0); pidTranslationY.setTolerance(Units.inchesToMeters(0.5)); driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading); @@ -192,7 +192,7 @@ public class Drivetrain extends SubsystemBase { }else if(vision.getOrangeClosestTag() >= 17 && vision.getOrangeClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getOrangeTagDetected()){ orangePose2d = vision.getOrangeGlobalPose(gyroBuffer); m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp()); - } + } } Logger.recordOutput("orange pose", new Pose3d(orangePose2d)); @@ -326,6 +326,9 @@ public class Drivetrain extends SubsystemBase { public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier headingSetpoint){ return startRun(() -> { + pidTranslationX.reset(); + pidTranslationY.reset(); + pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ()); }, () -> { ChassisSpeeds controlEffort = driveController.calculate( @@ -459,7 +462,11 @@ public class Drivetrain extends SubsystemBase { public Command resetToVision(){ return runOnce(() -> { - m_estimator.resetPose(orangePose2d); + if(vision.getOrangeTagDetected()){ + m_estimator.resetPose(new Pose2d(orangePose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation())); + }else if(vision.getBlackTagDetected()){ + m_estimator.resetPose(new Pose2d(blackPose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation())); + } }); } }