From ada023d3c9f8972941c4bd042264dc79b9a02a08 Mon Sep 17 00:00:00 2001 From: Jerry Date: Tue, 2 Apr 2024 19:06:50 -0400 Subject: [PATCH] auto aim is better --- src/main/java/frc/robot/Constants.java | 19 ++++++------ src/main/java/frc/robot/RobotContainer.java | 16 +++++----- .../java/frc/robot/commands/auto/AutoAmp.java | 2 +- .../subsystems/SwerveDriveSubsystem.java | 30 +++++++++---------- 4 files changed, 35 insertions(+), 32 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 413ce3e..8a798c1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -58,10 +58,10 @@ public static class Speeds { public static final double SWERVE_DRIVE_SLOW_COEFF = .6; // Default driving speed public static final double SWERVE_DRIVE_FAST_COEFF = .95; - public static final double SWERVE_DRIVE_MAX_SPEED = 4; + // public static final double SWERVE_DRIVE_MAX_SPEED = 4; public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI * 2.5; // TODO: Get this value - // public static final double SWERVE_DRIVE_MAX_SPEED = 1000; + public static final double SWERVE_DRIVE_MAX_SPEED = 4.6; public static final double SHOOTER_FAST_SPIN_SPEED = 1; public static final double SHOOTER_SLOW_SPIN_SPEED = 0.8; @@ -90,7 +90,7 @@ public static class Physical { public static final double SHOOTER_ANGLE_GEAR_RATIO = 112; public static final double SHOOTER_ENCODER_ANGLE_GEAR_RATIO = 2.8; - public static final double ELEVATOR_GEAR_RATIO = 25; + public static final double ELEVATOR_GEAR_RATIO = 15; public static final double ELEVATOR_DISTANCE_PER_ROTATION = .09; public static final SwerveDriveKinematics SWERVE_KINEMATICS = new SwerveDriveKinematics( @@ -110,10 +110,8 @@ public static class FeedForward { public static final double ELEVATOR_KG = 0.0; public static final double ELEVATOR_KS = 0.3; - public static final double MODULE_DRIVE_KS = 0.32; - public static final double MODULE_DRIVE_KV = 2.15; - - // public static final double MODULE_DRIVE_KV = 1; + public static final double MODULE_DRIVE_KS = 0.3; + public static final double MODULE_DRIVE_KV = 2.6; public static final double SHOOTER_ANGLE_KG = 0.001; } @@ -190,8 +188,11 @@ public static class FieldConstants { public static final double SHOOTER_SLOW_THRESHOLD = 2; - public static final double TAG_DISTANCE_THRESHOLD = 3.5; - public static final double SINGLE_TAG_DISTANCE_THRESHOLD = 1.5; + // public static final double TAG_DISTANCE_THRESHOLD = 3.5; + // public static final double SINGLE_TAG_DISTANCE_THRESHOLD = 1.5; + + public static final double FAR_TAG_DISTANCE_THRESHOLD = 3.5; + public static final double CLOSE_TAG_DISTANCE_THRESHOLD = 2.8; public static final int SPEAKER_TAG_BLUE = 7; public static final int SPEAKER_TAG_RED = 4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f09bf52..fddf332 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -183,13 +183,15 @@ private void configureBindings() { // TESTING ONLY!!! _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(180), _swerveSubsystem)); - _driveController.triangle().onTrue(Commands.run(() -> { - _swerveSubsystem.driveChassis(new ChassisSpeeds( - SmartDashboard.getNumber("DRIVE VOLTAGE", 0), - 0, - 0 - )); - }, _swerveSubsystem)); + // _driveController.triangle().onTrue(Commands.run(() -> { + // SmartDashboard.putNumber("KV", SmartDashboard.getNumber("DRIVE VOLTAGE", 0) / _swerveSubsystem.getRobotRelativeSpeeds().vxMetersPerSecond); + + // _swerveSubsystem.driveChassis(new ChassisSpeeds( + // SmartDashboard.getNumber("DRIVE VOLTAGE", 0), + // 0, + // 0 + // )); + // }, _swerveSubsystem)); // TESTING ONLY!!! diff --git a/src/main/java/frc/robot/commands/auto/AutoAmp.java b/src/main/java/frc/robot/commands/auto/AutoAmp.java index aacd155..ecec145 100644 --- a/src/main/java/frc/robot/commands/auto/AutoAmp.java +++ b/src/main/java/frc/robot/commands/auto/AutoAmp.java @@ -31,7 +31,7 @@ public AutoAmp( new SpinShooter(shooter, ShooterState.NONE, true), new PrintCommand("CAN AMP"), new FeedActuate(intake, ActuatorState.STOWED, FeedMode.OUTTAKE).withTimeout(0.5), - new SpinShooter(shooter, ShooterState.SLOW, false).withTimeout(0.1) + new SpinShooter(shooter, ShooterState.SLOW, false).withTimeout(0.2) ); } diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 0437424..43af988 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -78,7 +78,7 @@ public class SwerveDriveSubsystem extends SubsystemBase { private Field2d _field = new Field2d(); - private double _swerveTrim = 4; + private double _swerveTrim = 0; StructArrayPublisher swervePublisher = NetworkTableInstance.getDefault() .getStructArrayTopic("/Advantage Swerve Modules", SwerveModuleState.struct).publish(); @@ -97,7 +97,7 @@ public class SwerveDriveSubsystem extends SubsystemBase { Constants.Physical.SWERVE_KINEMATICS, getHeadingRaw(), new SwerveModulePosition[] {_frontLeft.getPosition(), _frontRight.getPosition(), _backRight.getPosition(), _backLeft.getPosition()}, new Pose2d(), - VecBuilder.fill(0.01, 0.01, 0.01), + VecBuilder.fill(0.03, 0.03, 0.01), VecBuilder.fill(0.9, 0.9, 9999999) ); @@ -127,7 +127,7 @@ public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) { _headingController.setTolerance(2); _headingController.enableContinuousInput(-180, 180); - resetPose(new Pose2d(5, 5, Rotation2d.fromDegrees(180))); + // resetPose(new Pose2d(5, 5, Rotation2d.fromDegrees(180))); // setupOrchestra(); @@ -176,6 +176,10 @@ public void initSendable(SendableBuilder builder) { SmartDashboard.putNumber("SPEEDOMETER X", _gyro.getVelocityX()); // Velocity read by the gyro (I added all three bc the axis might be different for the gyro) SmartDashboard.putNumber("SPEEDOMETER Y", _gyro.getVelocityY()); SmartDashboard.putNumber("SPEEDOMETER Z", _gyro.getVelocityZ()); + SmartDashboard.putNumber("PROBABLY_SPEED", Math.sqrt(Math.pow(_gyro.getVelocityX(), 2) + Math.pow(_gyro.getVelocityY(), 2))); + SmartDashboard.putNumber("PROBABLY_NOT_SPEED", Math.sqrt(Math.pow(_gyro.getVelocityX(), 2) + Math.pow(_gyro.getVelocityZ(), 2))); + + } @Override @@ -228,22 +232,18 @@ private boolean updateBotpose() { SmartDashboard.putNumber("TAG(S) DISTANCE", tagDistance); SmartDashboard.putNumber("TAG COUNT", tagCount); - // distance is too high for any tags, ignore - if (tagDistance > FieldConstants.TAG_DISTANCE_THRESHOLD) return false; - - // 2 tags, good distance - else if (tagCount >= 2) { - xyStds = 0.8; + if (tagDistance <= FieldConstants.CLOSE_TAG_DISTANCE_THRESHOLD) { + if (tagCount >= 2) { xyStds = 0.25; yawStd = 9000; } + else { xyStds = 0.8; yawStd = 10000; } } - // else return; // if only using 2 tags - - // 1 tag, bad single tag distance, ignore - else if (tagDistance > FieldConstants.SINGLE_TAG_DISTANCE_THRESHOLD) return false; + else if (tagDistance <= FieldConstants.FAR_TAG_DISTANCE_THRESHOLD) { + if (tagCount >= 2) { xyStds = 0.9; yawStd = 13000; } + else { xyStds = 1.2; yawStd = 16000; } + } - // 1 tag, good single tag distance, use data but with higher std devs else { - xyStds = 1.1; + return false; } visionPublisher.set(UtilFuncs.ToPose(llBotpose));