diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5bc5953..2e5bbb6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -57,7 +57,6 @@ public class RobotContainer { */ private void configureBindings() { - turretSubsystem.startHoming(); m_driverController.povDown().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_STORED_ANGLE)); m_driverController.povLeft().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE1)); m_driverController.povRight().onTrue(new SetDeflectorCommand(turretSubsystem, Constants.ShooterConstants.DEFLECTOR_SET_ANGLE3)); diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index d30d6fc..5ee7c8c 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -91,7 +91,7 @@ public class AutoShootCommandChassisTurret extends Command{ Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); - deadreckoningAutoCenterController.setSetpoint(delta.getAngle().getRadians()); + deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); } @Override @@ -105,7 +105,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState", "Look for tag"); Pose3d botPose = driveSubsystem.getBotPose(); - driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this + //driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); // May need to flip this } else // We have an april tag, center it to the camera frame { diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 485a5f0..6dbad45 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -135,7 +135,7 @@ public class DriveSubsystem extends TestableSubsystem { // Set vision measurement confidence values - driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 9999999)); + driveSystem.getPoseEstimator().setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.7, 0.7)); this.setDefaultCommand(Commands.run(new Runnable() { diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index a8bae7d..0a2425d 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -26,13 +26,13 @@ public class HopperSubsystem extends TestableSubsystem { kickerMotor.getConfigurator().apply(new MotorOutputConfigs().withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)); } - public void feedKicker() {kickerMotor.set(0.5);} + public void feedKicker() {kickerMotor.set(0.75);} public void reverseKicker() {kickerMotor.set(-0.5);} public void stopKicker() {kickerMotor.stopMotor();} - public void spindexerOn() { spindexerMotor.set(0.5);} + public void spindexerOn() { spindexerMotor.set(0.75);} public void spindexerOff() { spindexerMotor.stopMotor();}