Amp, auto-align, autos

team422 · Oct 14, 2024 · 9699572 · 9699572
1 parent d128ffc
commit 9699572
Show file tree

Hide file tree

Showing 18 changed files with 235 additions and 130 deletions.
diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto
@@ -0,0 +1,25 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 2.018336848061597,
+      "y": 5.5410316277334815
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path
@@ -0,0 +1,106 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.901478536053158,
+        "y": 5.599460783737701
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.901478536053158,
+        "y": 5.599460783737701
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.845446566337974,
+        "y": 6.602494628476801
+      },
+      "prevControl": {
+        "x": 4.845446566337974,
+        "y": 6.602494628476801
+      },
+      "nextControl": {
+        "x": 6.845446566337974,
+        "y": 6.602494628476801
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.897171374413923,
+        "y": 5.044383801697615
+      },
+      "prevControl": {
+        "x": 6.8713089703759485,
+        "y": 5.823439215087208
+      },
+      "nextControl": {
+        "x": 6.923033778451898,
+        "y": 4.2653283883080215
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.336026702895636,
+        "y": 3.7394659842700477
+      },
+      "prevControl": {
+        "x": 5.629530240673767,
+        "y": 4.002397186289034
+      },
+      "nextControl": {
+        "x": 3.0425231651175055,
+        "y": 3.4765347822510613
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.8625257653836784,
+        "y": 5.6384135544071805
+      },
+      "prevControl": {
+        "x": 2.452524465250592,
+        "y": 4.5574741683291204
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.95,
+      "rotationDegrees": 180.0,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -46,7 +46,7 @@ public static final class DriveConstants {
         Math.hypot(kTrackWidthX / 2.0, kTrackWidthY / 2.0);
     public static final double kMaxAngularSpeed = kMaxLinearSpeed / kDriveBaseRadius;
     public static final LoggedTunableNumber kTeleopRotationSpeed =
-        new LoggedTunableNumber("Teleop Rotation Speed", kMaxAngularSpeed);
+        new LoggedTunableNumber("Teleop Rotation Speed", 10.0);
 
     public static final Translation2d[] kModuleTranslations =
         new Translation2d[] {
@@ -65,6 +65,14 @@ public static final class DriveConstants {
     public static final double kDriveGearRatio =
         (50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0); // L3 ratio
     public static final double kTurnGearRatio = 150.0 / 7.0;
+
+    // everything is backwards dont flame me for having negative p
+    public static final LoggedTunableNumber kHeadingP =
+        new LoggedTunableNumber("Drive Heading P", -4.0);
+    public static final LoggedTunableNumber kHeadingI =
+        new LoggedTunableNumber("Drive Heading I", 0.0);
+    public static final LoggedTunableNumber kHeadingD =
+        new LoggedTunableNumber("Drive Heading D", 0.05);
   }
 
   public static final class AprilTagVisionConstants {
@@ -132,27 +140,25 @@ public static final class IntakeConstants {
   }
 
   public static final class IndexerConstants {
-    public static final LoggedTunableNumber kIntakingVelocity =
-        new LoggedTunableNumber("Indexer Intake Velocity", -8.0);
-    public static final LoggedTunableNumber kIndexingVelocity =
-        new LoggedTunableNumber("Indexer Indexing Velocity", -8.0);
-    public static final LoggedTunableNumber kReversingVelocity =
-        new LoggedTunableNumber("Indexer Reverse Velocity", 8.0);
-    public static final LoggedTunableNumber kShootingVelocity =
-        new LoggedTunableNumber("Indexer Shooting Velocity", -10.0);
-    public static final LoggedTunableNumber kEjectingVelocity =
-        new LoggedTunableNumber("Indexer Eject Velocity", 8.0);
+    public static final LoggedTunableNumber kIdleVoltage =
+        new LoggedTunableNumber("Indexer Idle Voltage", 0.0);
+    public static final LoggedTunableNumber kIntakingVoltage =
+        new LoggedTunableNumber("Indexer Intake Voltage", -8.0);
+    public static final LoggedTunableNumber kIndexingVoltage =
+        new LoggedTunableNumber("Indexer Indexing Voltage", -8.0);
+    public static final LoggedTunableNumber kReversingVoltage =
+        new LoggedTunableNumber("Indexer Reverse Voltage", 8.0);
+    public static final LoggedTunableNumber kShootingVoltage =
+        new LoggedTunableNumber("Indexer Shooting Voltage", -12.0);
+    public static final LoggedTunableNumber kEjectingVoltage =
+        new LoggedTunableNumber("Indexer Eject Voltage", 8.0);
 
     public static final LoggedTunableNumber kShootingTimeout =
-        new LoggedTunableNumber("Indexer Shooting Timeout", 1.0);
+        new LoggedTunableNumber("Indexer Shooting Timeout", 1.5);
     public static final LoggedTunableNumber kIndexingTimeout =
         new LoggedTunableNumber("Indexer Indexing Timeout", 0.1);
     public static final LoggedTunableNumber kReverseTimeout =
-        new LoggedTunableNumber("Indexer Reverse Timeout", 0.5);
-
-    public static final LoggedTunableNumber kIndexerP = new LoggedTunableNumber("Indexer P", 0.4);
-    public static final LoggedTunableNumber kIndexerI = new LoggedTunableNumber("Indexer I", 0.0);
-    public static final LoggedTunableNumber kIndexerD = new LoggedTunableNumber("Indexer D", 0.0);
+        new LoggedTunableNumber("Indexer Reverse Timeout", 0.2);
 
     // Simulation constants
     public static final DCMotor kSimGearbox = DCMotor.getNEO(1);
@@ -187,9 +193,9 @@ public static final class ShooterConstants {
         new LoggedTunableNumber("Bottom Flywheel kV", 0.13);
 
     public static final LoggedTunableNumber kAmpTopVelocity =
-        new LoggedTunableNumber("Amp Top Velocity", 5.0);
+        new LoggedTunableNumber("Amp Top Velocity", -3.5);
     public static final LoggedTunableNumber kAmpBottomVelocity =
-        new LoggedTunableNumber("Amp Bottom Velocity", 7.0);
+        new LoggedTunableNumber("Amp Bottom Velocity", 58.0);
 
     public static final LoggedTunableNumber kSubwooferTopVelocity =
         new LoggedTunableNumber("Subwoofer Top Velocity", 60.0);

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -85,11 +85,11 @@ public void robotPeriodic() {
     // finished or interrupted commands, and running subsystem periodic() methods.
     // This must be called from the robot's periodic block in order for anything in
     // the Command-based framework to work.
-    var start = Timer.getFPGATimestamp();
+    double start = Timer.getFPGATimestamp();
 
     CommandScheduler.getInstance().run();
 
-    Logger.recordOutput("PeriodicTime/CommandScheduler", Timer.getFPGATimestamp() - start);
+    Logger.recordOutput("PeriodicTime/CommandScheduler", (double) Timer.getFPGATimestamp() - start);
 
     robotContainer.updateRobotState();
   }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -151,7 +151,7 @@ private void configureSubsystems() {
 
   /** Configure the commands. */
   private void configureCommands() {
-    m_autoChooser = new LoggedDashboardChooser<>("AutoChooser");
+    m_autoChooser = new LoggedDashboardChooser<>("Auto Chooser");
     m_autoFactory = new AutoFactory(m_drive);
 
     // Configure autos here
@@ -343,6 +343,14 @@ private void configureButtonBindings() {
                   m_shooter.updateState(ShooterState.kIdle);
                   m_drive.updateProfile(DriveProfiles.kDefault);
                 }));
+
+    m_driverControls
+        .resetPoseAuto()
+        .onTrue(
+            Commands.runOnce(
+                () -> {
+                  m_drive.setPose(new Pose2d(14.764, 5.594, new Rotation2d(Math.PI)));
+                }));
   }
 
   /**

diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java
@@ -18,7 +18,6 @@
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.Shooter.ShooterPosition;
 import frc.robot.subsystems.shooter.Shooter.ShooterState;
-import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.ShooterMath;
 import frc.robot.util.SubsystemProfiles;
 import java.util.HashMap;
@@ -91,7 +90,7 @@ public static RobotState getInstance() {
   }
 
   public void updateRobotState() {
-    var start = Timer.getFPGATimestamp();
+    double start = Timer.getFPGATimestamp();
 
     m_profiles.getPeriodicFunction().run();
 
@@ -225,8 +224,8 @@ public void updateRobotAction(RobotAction newAction) {
       case kAmpLineup:
         m_intake.updateState(IntakeState.kIdle);
         m_shooter.updateState(ShooterState.kAmp);
-        m_drive.updateProfile(DriveProfiles.kAmpLineup);
-        m_drive.setDesiredHeading(AllianceFlipUtil.apply(Rotation2d.fromDegrees(90)));
+        m_drive.updateProfile(DriveProfiles.kDefault);
+        // m_drive.setDesiredHeading(AllianceFlipUtil.apply(Rotation2d.fromDegrees(90)));
 
         break;
     }

diff --git a/src/main/java/frc/robot/commands/auto/AutoFactory.java b/src/main/java/frc/robot/commands/auto/AutoFactory.java
@@ -15,7 +15,7 @@
 
 public class AutoFactory extends Command {
   public static final PIDConstants kLinearPID = new PIDConstants(2.5, 0.0, 0.0);
-  public static final PIDConstants kAngularPID = new PIDConstants(4, 1.6, 0.0);
+  public static final PIDConstants kAngularPID = new PIDConstants(1.2, 0.0, 0.0);
 
   private final Drive m_drive;
 

diff --git a/src/main/java/frc/robot/oi/DriverControls.java b/src/main/java/frc/robot/oi/DriverControls.java
@@ -28,4 +28,6 @@ public interface DriverControls {
   public Trigger cancelAmpLineup();
 
   public Trigger subwooferShot();
+
+  public Trigger resetPoseAuto();
 }
diff --git a/src/main/java/frc/robot/oi/DriverControlsPS5.java b/src/main/java/frc/robot/oi/DriverControlsPS5.java
@@ -69,4 +69,9 @@ public Trigger cancelAmpLineup() {
   public Trigger subwooferShot() {
     return m_controller.cross();
   }
+
+  @Override
+  public Trigger resetPoseAuto() {
+    return m_controller.square();
+  }
 }
diff --git a/src/main/java/frc/robot/oi/DriverControlsXbox.java b/src/main/java/frc/robot/oi/DriverControlsXbox.java
@@ -69,4 +69,9 @@ public Trigger cancelAmpLineup() {
   public Trigger subwooferShot() {
     return m_controller.a();
   }
+
+  @Override
+  public Trigger resetPoseAuto() {
+    return m_controller.x();
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java b/src/main/java/frc/robot/subsystems/aprilTagVision/AprilTagVision.java
@@ -60,7 +60,7 @@ public AprilTagVision(AprilTagVisionIO... ios) {
 
   @Override
   public void periodic() {
-    var start = Timer.getFPGATimestamp();
+    double start = Timer.getFPGATimestamp();
 
     for (int i = 0; i < m_ios.length; i++) {
       m_ios[i].updateInputs(m_inputs[i]);

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -39,6 +39,7 @@
 import frc.robot.Constants.DriveConstants;
 import frc.robot.subsystems.aprilTagVision.AprilTagVision.VisionObservation;
 import frc.robot.util.LocalADStarAK;
+import frc.robot.util.LoggedTunableNumber;
 import frc.robot.util.SubsystemProfiles;
 import java.util.HashMap;
 import java.util.concurrent.locks.Lock;
@@ -64,7 +65,11 @@ public enum DriveProfiles {
   private ChassisSpeeds m_desiredChassisSpeeds = new ChassisSpeeds();
 
   private Rotation2d m_desiredHeading = new Rotation2d();
-  private PIDController m_headingController = new PIDController(0.1, 0, 0.09);
+  private PIDController m_headingController =
+      new PIDController(
+          DriveConstants.kHeadingP.get(),
+          DriveConstants.kHeadingI.get(),
+          DriveConstants.kHeadingD.get());
 
   private Rotation2d m_rawGyroRotation = new Rotation2d();
   private SwerveModulePosition[] m_lastModulePositions = // For delta tracking
@@ -149,7 +154,7 @@ public Drive(
   }
 
   public void periodic() {
-    var start = Timer.getFPGATimestamp();
+    double start = Timer.getFPGATimestamp();
 
     m_odometryLock.lock(); // Prevents odometry updates while reading data
     m_gyroIO.updateInputs(m_gyroInputs);
@@ -158,6 +163,17 @@ public void periodic() {
     }
     m_odometryLock.unlock();
 
+    LoggedTunableNumber.ifChanged(
+        hashCode(),
+        () -> {
+          m_headingController.setP(DriveConstants.kHeadingP.get());
+          m_headingController.setI(DriveConstants.kHeadingI.get());
+          m_headingController.setD(DriveConstants.kHeadingD.get());
+        },
+        DriveConstants.kHeadingP,
+        DriveConstants.kHeadingI,
+        DriveConstants.kHeadingD);
+
     m_profiles.getPeriodicFunction().run();
 
     Logger.processInputs("Drive/Gyro", m_gyroInputs);
@@ -218,6 +234,7 @@ public void defaultPeriodic() {
     runVelocity(m_desiredChassisSpeeds);
 
     Logger.recordOutput("Drive/DesiredHeading", m_desiredHeading.getDegrees());
+    Logger.recordOutput("Drive/CurrentHeading", getPose().getRotation().getDegrees());
     Logger.recordOutput("Drive/DesiredSpeeds", m_desiredChassisSpeeds);
   }
 
@@ -247,6 +264,9 @@ public ChassisSpeeds calculateAutoAlignSpeeds() {
       m_desiredChassisSpeeds.omegaRadiansPerSecond =
           m_headingController.calculate(
               getPose().getRotation().getRadians(), m_desiredHeading.getRadians());
+      Logger.recordOutput("AutoAlign/Curr", getPose().getRotation().getRadians());
+      Logger.recordOutput("AutoAlign/Des", m_desiredHeading.getRadians());
+      Logger.recordOutput("AutoAlign/Out", m_desiredChassisSpeeds.omegaRadiansPerSecond);
     }
 
     return m_desiredChassisSpeeds;