auto aim is better

Team334 · Apr 2, 2024 · ada023d · ada023d
1 parent 4d891f3
commit ada023d
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 32 deletions.
diff --git 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
@@ -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
@@ -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
@@ -78,7 +78,7 @@ public class SwerveDriveSubsystem extends SubsystemBase {
 
   private Field2d _field = new Field2d();
 
-  private double _swerveTrim = 4;
+  private double _swerveTrim = 0;
 
   StructArrayPublisher<SwerveModuleState> 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));