auto aim works

Team334 · Mar 23, 2024 · 5ff522c · 5ff522c
1 parent 42f4834
commit 5ff522c
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -56,7 +56,7 @@ public static class CAN {
 
   public static class Speeds {
     public static final double SWERVE_DRIVE_SLOW_COEFF = .6; // Default driving speed
-    public static final double SWERVE_DRIVE_FAST_COEFF = 1;
+    public static final double SWERVE_DRIVE_FAST_COEFF = .95;
 
     public static final double SWERVE_DRIVE_MAX_SPEED = 4; // meters per second
     public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI * 1.8; // TODO: Get this value
@@ -65,7 +65,7 @@ public static class Speeds {
     public static final double SHOOTER_SLOW_SPIN_SPEED = 1;
     public static final double SHOOTER_AMP_SPEED = 0.2;
     public static final double SHOOTER_INTAKE_SPEED = -.15;
-    public static final double SHOOTER_IDLE_SPEED = 0.1;
+    public static final double SHOOTER_IDLE_SPEED = 0.45;
 
     public static final double SHOOTER_ANGLE_MAX_SPEED = 0.3;
     public static final double ELEVATOR_MAX_SPEED = 1;
@@ -133,8 +133,8 @@ public static class PID {
     public static final double ELEVATOR_KP = 5.2;
     public static final double ELEVATOR_KD = 0.2;
 
-    public static final double SWERVE_HEADING_KP = 0.025;
-    public static final double SWERVE_HEADING_KD = 0.0025;
+    public static final double SWERVE_HEADING_KP = 0.15;
+    public static final double SWERVE_HEADING_KD = 0.01;
   }
 
 

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -178,9 +178,7 @@ private void configureBindings() {
         _swerveSubsystem,
         _shooterSubsystem,
         _elevatorSubsystem,
-        _ledSubsystem,
-        () -> MathUtil.applyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.05),
-        () -> MathUtil.applyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.05)
+        _ledSubsystem
       )
     );
   }

diff --git a/src/main/java/frc/robot/commands/auto/AutoAim.java b/src/main/java/frc/robot/commands/auto/AutoAim.java
@@ -76,7 +76,7 @@ public AutoAim(
     DoubleSupplier xSpeed,
     DoubleSupplier ySpeed
   ) {
-    this(swerve, shooter, elevator, leds, xSpeed, ySpeed, swerve::speakerHeading, shooter::speakerAngle, elevator::speakerHeight, false);
+    this(swerve, shooter, elevator, leds, xSpeed, ySpeed, swerve::speakerHeading, shooter::speakerAngle, elevator::speakerHeight, true);
   }
 
   /** 

diff --git a/src/main/java/frc/robot/commands/swerve/SetHeading.java b/src/main/java/frc/robot/commands/swerve/SetHeading.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.PID;
 import frc.robot.Constants.Speeds;
@@ -47,7 +48,7 @@ public SetHeading(
     _xSpeed = xSpeed;
     _ySpeed = ySpeed;
 
-    _headingController.setTolerance(2);
+    _headingController.setTolerance(3);
     _headingController.enableContinuousInput(-180, 180);
 
     addRequirements(_swerve);
@@ -70,6 +71,8 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+    SmartDashboard.putData(_headingController);
+
     double rotationVelocity = MathUtil.clamp(
       _headingController.calculate(_swerve.getHeading().getDegrees(), _heading.getAsDouble()),
       -Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED,

diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -54,7 +54,7 @@ public class ShooterSubsystem extends SubsystemBase {
   private final Debouncer _beamDebouncer = new Debouncer(0.3, DebounceType.kRising);
   private final Encoder _incremental = new Encoder(1, 2, false, Encoder.EncodingType.k2X);
 
-  private double _shooterTrim = 0;
+  private double _shooterTrim = 1.55;
 
   private boolean _holdNote = false;
 
@@ -193,7 +193,7 @@ public double getVelocity() {
    */
   public void driveAngle(double speed) {
     double ff = UtilFuncs.FromVolts(_angleFeed.calculate(Math.toRadians(getAngle()), 0));
-    System.out.println(ff + speed);
+    // System.out.println(ff + speed);
     _angleMotor.set(ff + speed);
     // _angleMotor.set(0);
   }
@@ -205,6 +205,9 @@ public void stopAngle() {
 
   /** Sets the state of the shooter. */
   public void setShooterState(ShooterState state) {
+    SmartDashboard.putString("SHOOTER STATE", state.toString());
+    spinShooter(0.3);
+
     switch (state) {
       case SHOOT:
         if (UtilFuncs.ShotVector().getNorm() > FieldConstants.SHOOTER_SLOW_THRESHOLD) { 
@@ -220,9 +223,12 @@ public void setShooterState(ShooterState state) {
 
       case INTAKE:
         spinShooter(Speeds.SHOOTER_INTAKE_SPEED);
+        break;
 
       case IDLE:
-        spinShooter(0);
+        // spinShooter(0);
+        spinShooter(Speeds.SHOOTER_IDLE_SPEED);
+        break;
 
       case NONE:
         stopShooter();