initial auto and vision tuning

src/main/deploy/pathplanner/autos/1M Auto.auto

@@ -6,9 +6,34 @@
     "data": {
       "commands": [
         {
-          "type": "path",
+          "type": "named",
           "data": {
-            "pathName": "1M Path"
+            "name": "AimAndShoot"
+          }
+        },
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "speakerToCenterNote"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Run Intake"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "AimAndShoot"
           }
         }
       ]

src/main/deploy/pathplanner/paths/1M Path.path → src/main/deploy/pathplanner/paths/speakerToCenterNote.path

@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 2.34,
+        "x": 2.4599653194866877,
         "y": 5.55
       },
       "prevControl": {
-        "x": 1.3399999999999999,
+        "x": 1.4599653194866877,
         "y": 5.55
       },
       "nextControl": null,
@@ -32,21 +32,21 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 1.0,
+    "maxAcceleration": 1.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.44451110511873265,
+    "rotation": -179.61351583273776,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": 0.2420983632108403,
+    "rotation": 179.7976785591257,
     "velocity": 0
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/java/frc/robot/Constants.java

@@ -86,24 +86,24 @@ private DriveConstants() {
 
     public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d(
         new Translation3d(
-            Units.inchesToMeters(7.0351), // 11.0351
-            Units.inchesToMeters(-6.523204), // 10.023204
-            Units.inchesToMeters(6.1374)), // 4.1374
+            Units.inchesToMeters(11.0351), // 11.0351
+            Units.inchesToMeters(10.023204), // 10.023204
+            Units.inchesToMeters(4.1374)), // 4.1374
         new Rotation3d(
             Units.degreesToRadians(0.0),
-            Units.degreesToRadians(-30.0 + 1), // -120.0 + 91.0
-            Units.degreesToRadians(-13.7)) // 165.3224 + 180
+            Units.degreesToRadians(-30.0), // -120.0 + 91.0
+            Units.degreesToRadians(-14.7)) // 165.3224 + 180
     );
 
     public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d(
         new Translation3d(
-            Units.inchesToMeters(5.0351), //11.0351
-            Units.inchesToMeters(-22.023204), //-10.023204
+            Units.inchesToMeters(11.0351), //11.0351
+            Units.inchesToMeters(-10.023204), //-10.023204
             Units.inchesToMeters(4.1374)), // 7.1374
         new Rotation3d(
-            Units.degreesToRadians(2.0),
-            Units.degreesToRadians(-30.0 + 3), // -30.0 - 1
-            Units.degreesToRadians(5.25)) // 165.3224)
+            Units.degreesToRadians(0.0),
+            Units.degreesToRadians(-30.0), // -30.0 - 1
+            Units.degreesToRadians(14.7)) // 165.3224)
     );
 
     public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints(
@@ -123,7 +123,7 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(0.472168), // offset 0.457764
+            Units.rotationsToDegrees(-0.017822) + 180, // offset 0.457764
             true, // inversion
             ModuleConstants.GearRatios.L3_KRAKEN
     );
@@ -134,7 +134,7 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(0.046143),
+            Units.rotationsToDegrees(-0.453857) + 180,
             true,
             ModuleConstants.GearRatios.L3_KRAKEN
     );
@@ -145,7 +145,7 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(-0.073730),
+            Units.rotationsToDegrees(0.428467) + 180,
             true,
             ModuleConstants.GearRatios.L3_KRAKEN
     );
@@ -156,7 +156,7 @@ private DriveConstants() {
             DRIVE_FF_GAINS,
             DRIVE_FB_GAINS,
             TURN_FB_GAINS,
-            Units.rotationsToDegrees(0.404297),
+            Units.rotationsToDegrees(-0.093750) + 180,
             true,
             ModuleConstants.GearRatios.L3_KRAKEN
     );

src/main/java/frc/robot/Robot.java

@@ -80,7 +80,7 @@ public void robotPeriodic() {
     // the Command-based framework to work.
     CommandScheduler.getInstance().run();
     TalonFXFactory.handleFaults();
-//    DataLogUtil.updateTables();
+    DataLogUtil.updateTables();
   }
 
   /** This function is called once when the robot is disabled. */

src/main/java/frc/robot/RobotContainer.java

@@ -36,6 +36,7 @@
 import frc.robot.commands.DriveCommands;
 import frc.robot.commands.FeedForwardCharacterization;
 import frc.robot.commands.IntakeControlCommand;
+import frc.robot.commands.ShooterAutoCommand;
 import frc.robot.subsystems.arm.*;
 import frc.robot.subsystems.climber.ClimberIO;
 import frc.robot.subsystems.climber.ClimberIOKraken;
@@ -222,9 +223,13 @@ private void configureButtonBindings() {
     ampLineupTrigger.whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP));
     ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5),
         () -> m_shooter.setKickerPower(0.0),
-        m_shooter));
+        m_shooter)
+        .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)));
 
     controller.pov(0).onTrue(Commands.runOnce(SignalLogger::stop));
+    controller.pov(180).whileTrue(m_driveSubsystem.pathfollowFactory(
+        () -> AllianceFlipUtil.apply(FieldConstants.AMP_LINEUP)
+    ));
 
     m_driveSubsystem.setDefaultCommand(
         DriveCommands.joystickDrive(
@@ -248,7 +253,16 @@ private void configureButtonBindings() {
    * Use this method to configure any named commands needed for PathPlanner autos
    */
   private void configureNamedCommands() {
-    NamedCommands.registerCommand("Run Intake", Commands.run(() -> m_shooter.setIntakePower(0.5)));
+    NamedCommands.registerCommand("Run Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1)
+        .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE)));
+
+    NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter)
+        .raceWith(DriveCommands.alignmentDrive(
+            m_driveSubsystem,
+            () -> -controller.getLeftY(),
+            () -> -controller.getLeftX(),
+            () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER)
+        )));
   }
 
   private void configureDashboard() {

src/main/java/frc/robot/commands/ShooterAutoCommand.java

@@ -0,0 +1,41 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.arm.ArmSubsystem;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
+
+
+public class ShooterAutoCommand extends Command {
+  private final ArmSubsystem armSubsystem;
+  private final ShooterSubsystem shooterSubsystem;
+
+  public ShooterAutoCommand(ArmSubsystem armSubsystem, ShooterSubsystem shooterSubsystem) {
+    this.armSubsystem = armSubsystem;
+    this.shooterSubsystem = shooterSubsystem;
+    // each subsystem used by the command must be passed into the
+    // addRequirements() method (which takes a vararg of Subsystem)
+    addRequirements(this.armSubsystem, this.shooterSubsystem);
+  }
+
+  @Override
+  public void initialize() {
+
+  }
+
+  @Override
+  public void execute() {
+    shooterSubsystem.runShooterVelocity(shooterSubsystem.atSpeed()).execute();
+    armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM);
+  }
+
+  @Override
+  public boolean isFinished() {
+    return !shooterSubsystem.hasPiece();
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    shooterSubsystem.setShooterPowerFactory(0.0, 0.0, 0.0).execute();
+    armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW);
+  }
+}

src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java

@@ -47,6 +47,7 @@
 
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Supplier;
 
 import lib.logger.DataLogUtil;
 import lib.logger.DataLogUtil.DataLogTable;
@@ -156,7 +157,9 @@ public DriveSubsystem(
         kinematics,
         new Rotation2d(),
         getModulePositions(),
-        new Pose2d()
+        new Pose2d(),
+        VecBuilder.fill(0.15, 0.15, 0.1),
+        VecBuilder.fill(1.0, 1.0, 1.25)
     );
 
     // Configure AutoBuilder for PathPlanner
@@ -375,8 +378,8 @@ public double getMaxAngularSpeedRadPerSec() {
     return DriveConstants.MAX_ANGULAR_SPEED;
   }
 
-  public Command pathfollowFactory(Pose2d pose) {
-    return AutoBuilder.pathfindToPose(pose, DriveConstants.DEFAULT_CONSTRAINTS);
+  public Command pathfollowFactory(Supplier<Pose2d> pose) {
+    return AutoBuilder.pathfindToPose(pose.get(), DriveConstants.DEFAULT_CONSTRAINTS);
   }
 
   /** Returns an array of module translations. */

src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java

@@ -99,8 +99,8 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) {
 
     // run configs on drive motor
     var driveConfig = new TalonFXConfiguration();
-    driveConfig.CurrentLimits.StatorCurrentLimit = 40.0;
-    driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+    driveConfig.CurrentLimits.SupplyCurrentLimit = 40.0;
+    driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
     driveConfig.MotorOutput.Inverted =
             moduleConstants.DRIVE_MOTOR_INVERTED() ? InvertedValue.Clockwise_Positive
                     : InvertedValue.CounterClockwise_Positive;

src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java

@@ -26,6 +26,9 @@ public class ShooterSubsystem extends SubsystemBase {
 
   private final DataLogTable m_logTable = DataLogUtil.getTable("Shooter");
 
+  private double m_leftSpeedSetpoint = 0.0;
+  private double m_rightSpeedSetpoint = 0.0;
+
   private final GosDoubleProperty m_leftPower = new
       GosDoubleProperty(false, "Shooter/Left RPM", 3600);
   private final GosDoubleProperty m_rightPower = new
@@ -76,8 +79,11 @@ public Command runShooterVelocity(boolean runKicker) {
           Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d();
           double groundDistance = m_poseSupplier.get().getTranslation().getDistance(speakerPoseGround);
 
-          m_io.setLeftVelocityRpm(AimbotUtils.getLeftSpeed(groundDistance));
-          m_io.setRightVelocityRpm(AimbotUtils.getRightSpeed(groundDistance));
+          m_leftSpeedSetpoint = AimbotUtils.getLeftSpeed(groundDistance);
+          m_rightSpeedSetpoint = AimbotUtils.getRightSpeed(groundDistance);
+
+          m_io.setLeftVelocityRpm(m_leftSpeedSetpoint);
+          m_io.setRightVelocityRpm(m_rightSpeedSetpoint);
 
           if (runKicker) {
             m_io.setKickerVoltage(12.0);
@@ -140,6 +146,9 @@ public void setupLogging() {
     m_logTable.addDouble("LeftVelocity", () -> m_inputs.tlVelocityRPM, true);
     m_logTable.addDouble("RightVelocity", () -> m_inputs.trVelocityRPM, true);
 
+    m_logTable.addDouble("LeftSetpoint", () -> m_leftSpeedSetpoint, true);
+    m_logTable.addDouble("RightSetpoint", () -> m_rightSpeedSetpoint, true);
+
     m_logTable.addDouble("LeftAppliedVolts", () -> m_inputs.tlAppliedVolts, true);
     m_logTable.addDouble("RightAppliedVolts", () -> m_inputs.trAppliedVolts, true);
 
@@ -151,4 +160,9 @@ public void setupLogging() {
 
     m_logTable.addDouble("IntakeCurrent", () -> m_inputs.intakeCurrentDraw, true);
   }
+
+  public boolean atSpeed() {
+    return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 75
+        && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 75;
+  }
 }

src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

@@ -31,8 +31,11 @@ public class VisionSubsystem {
   private final String m_name;
 
   private Pose3d m_lastEstimatedPose = new Pose3d();
-  private final double xyStdDevCoefficient = 0.01;
-  private final double thetaStdDevCoefficient = 0.01;
+  private final double xyStdDevCoefficient = 0.03;
+  private final double thetaStdDevCoefficient = 0.05;
+
+  private final double xyStdDevMultiTagCoefficient = 0.02;
+  private final double thetaStdDevMultiTagCoefficient = 0.04;
 
   private final PhotonVisionIOInputs inputs = new PhotonVisionIOInputs();
 
@@ -80,15 +83,15 @@ public Optional<PoseEstimator.TimestampedVisionUpdate> getPose(Pose2d prevEstima
       double distance = estPose.estimatedPose.toPose2d().getTranslation().getDistance(
           robotToCam.getTranslation().toTranslation2d());
 
-      double xyStdDev = 0.0;
-      double thetaStdDev = 0.0;
+      double xyStdDev;
+      double thetaStdDev;
 
       if (estPose.strategy != PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) {
         xyStdDev = xyStdDevCoefficient * Math.pow(distance, 2.0);
         thetaStdDev = thetaStdDevCoefficient * Math.pow(distance, 2.0);
       } else {
-        xyStdDev = (xyStdDevCoefficient / 2.0) * Math.pow(distance, 2.0);
-        thetaStdDev = (thetaStdDevCoefficient / 2.0) * Math.pow(distance, 2.0);
+        xyStdDev = xyStdDevMultiTagCoefficient * Math.pow(distance, 2.0);
+        thetaStdDev = thetaStdDevMultiTagCoefficient * Math.pow(distance, 2.0);
       }
 
       return Optional.of(new PoseEstimator.TimestampedVisionUpdate(estPose.timestampSeconds,

src/main/java/lib/utils/FieldConstants.java

@@ -29,6 +29,13 @@ private FieldConstants() {}
   public static final Translation2d AMP_CENTER =
       new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996));
 
+  public static final Pose2d AMP_LINEUP =
+    new Pose2d(
+        AMP_CENTER.getX() - Units.inchesToMeters(10),
+        AMP_CENTER.getY(),
+        Rotation2d.fromDegrees(90)
+    );
+
   /** Staging locations for each note */
   public static final class StagingLocations {
     public static final double CENTERLINE_X = FIELD_LENGTH / 2.0;