tune lighter flywheel

src/main/java/frc2713/lib/util/LoggedTunableGains.java

@@ -3,6 +3,8 @@
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import frc2713.lib.io.AdvantageScopePathBuilder;
 import java.util.function.BiConsumer;
 import java.util.function.Consumer;
@@ -63,6 +65,20 @@ public LoggedTunableGains(String name, Slot0Configs gains, MotionMagicConfigs mo
     this.motionMagicRef = motionMagic;
   }
 
+  public LoggedTunableGains(String name, double p, double i, double d) {
+    this(name, new Slot0Configs().withKP(p).withKI(i).withKD(d), new MotionMagicConfigs());
+  }
+
+  public LoggedTunableGains(
+      String name, double p, double i, double d, TrapezoidProfile.Constraints constraints) {
+    this(
+        name,
+        new Slot0Configs().withKP(p).withKI(i).withKD(d),
+        new MotionMagicConfigs()
+            .withMotionMagicCruiseVelocity(constraints.maxVelocity)
+            .withMotionMagicAcceleration(constraints.maxAcceleration));
+  }
+
   /**
    * Runs the provided action when any of the tunables have changed. The Slot0Configs and
    * MotionMagicConfigs instance passed to the constructor will be updated before the action runs.
@@ -123,6 +139,16 @@ public PIDController createPIDController() {
     return new PIDController(this.P.getAsDouble(), this.I.getAsDouble(), this.D.getAsDouble());
   }
 
+  public ProfiledPIDController createProfiledPIDController() {
+    return new ProfiledPIDController(
+        this.P.getAsDouble(),
+        this.I.getAsDouble(),
+        this.D.getAsDouble(),
+        new TrapezoidProfile.Constraints(
+            this.motionMagicCruiseVelocity.getAsDouble(),
+            this.motionMagicAcceleration.getAsDouble()));
+  }
+
   public PIDController createAngularPIDController() {
     var pid = new PIDController(this.P.getAsDouble(), this.I.getAsDouble(), this.D.getAsDouble());
     pid.enableContinuousInput(-Math.PI, Math.PI);

src/main/java/frc2713/robot/GameCommandGroups.java

@@ -97,8 +97,8 @@ public static Command otfShot(
                   Optional.of(DegreesPerSecondPerSecond.of(360.0))),
               flywheels.otfCommand(),
               hood.otfCommand(),
-              turret.otfCommand(),
-              intakeRoller.intake(),
+              // turret.otfCommand(),
+              // intakeRoller.intake(),
               flywheels.simulateLaunchFuelCommand(flywheels::atTarget),
               feeder.feedWhenReady(flywheels::atTarget),
               dyeRotor.dynamicFeedWhenReady(flywheels::atTarget),

src/main/java/frc2713/robot/Robot.java

@@ -113,12 +113,6 @@ public void disabledInit() {}
   /** This function is called periodically when disabled. */
   @Override
   public void disabledPeriodic() {
-    // Reset pose to vision pose if available whilst disabled
-    var visionPose = RobotContainer.vision.getPose();
-    if (visionPose.isPresent()) {
-      RobotContainer.drive.setPose(visionPose.get());
-    }
-
     LaunchingSolutionManager.currentGoal =
         AllianceFlipUtil.apply(FieldConstants.Hub.topCenterPoint);
 

src/main/java/frc2713/robot/RobotContainer.java

@@ -19,6 +19,7 @@
 import frc2713.lib.subsystem.TalonFXSubsystemConfig;
 import frc2713.lib.util.AllianceFlipUtil;
 import frc2713.robot.commands.DriveCommands;
+import frc2713.robot.commands.autos.Demo;
 import frc2713.robot.commands.autos.DriveTest;
 import frc2713.robot.commands.autos.Midwars;
 import frc2713.robot.commands.autos.MidwarsFlipped;
@@ -383,6 +384,8 @@ private void configureAutonomousRoutines(
                 GameCommandGroups.Launching.autoOtfShot(
                     flywheels, hood, turret, feeder, dyeRotor, intakeExtension, intakeRoller)));
 
+    autoChooser.addOption("DemoMode", Demo.demo());
+
     autoChooser.addOption(
         "Trench to Neutral, Launch At Bump",
         RightSideAutoBump.routine(

src/main/java/frc2713/robot/commands/DriveCommands.java

@@ -28,6 +28,7 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc2713.lib.util.LoggedTunableGains;
 import frc2713.robot.subsystems.drive.Drive;
 import java.text.DecimalFormat;
 import java.text.NumberFormat;
@@ -49,6 +50,13 @@ public class DriveCommands {
   private static final double WHEEL_RADIUS_MAX_VELOCITY = 1.25; // Rad/Sec
   private static final double WHEEL_RADIUS_RAMP_RATE = 0.25; // Rad/Sec^2
   public static final DoubleSupplier INCH_SPEED = () -> 0.1;
+  public static final LoggedTunableGains DRIVE_HEADING_CONTROLLER_GAINS =
+      new LoggedTunableGains(
+          "Drive/HeadingController",
+          ANGLE_KP,
+          0.0,
+          ANGLE_KD,
+          new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
 
   private DriveCommands() {}
 
@@ -140,11 +148,7 @@ public static Command joystickDriveAtAngle(
 
     // Create PID controller
     ProfiledPIDController angleController =
-        new ProfiledPIDController(
-            ANGLE_KP,
-            0.0,
-            ANGLE_KD,
-            new TrapezoidProfile.Constraints(ANGLE_MAX_VELOCITY, ANGLE_MAX_ACCELERATION));
+        DRIVE_HEADING_CONTROLLER_GAINS.createProfiledPIDController();
     angleController.enableContinuousInput(-Math.PI, Math.PI);
 
     // Construct command

src/main/java/frc2713/robot/commands/autos/Demo.java

@@ -0,0 +1,27 @@
+package frc2713.robot.commands.autos;
+
+import static edu.wpi.first.units.Units.Degree;
+import static edu.wpi.first.units.Units.Degrees;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc2713.robot.RobotContainer;
+
+public class Demo {
+  public static Command demo() {
+    return Commands.repeatingSequence(
+        Commands.parallel(
+                RobotContainer.hood.setAngleCommand(() -> Degrees.of(0.5))
+                //   ,  RobotContainer.flywheels.setVelocity(() -> RPM.of(0))
+                )
+            .withTimeout(0.5),
+        RobotContainer.turret.setAngle(() -> Degree.of(-15)).withTimeout(1),
+        Commands.parallel(
+                RobotContainer.hood.setAngleCommand(() -> Degrees.of(10)),
+                RobotContainer.turret.setAngle(() -> Degree.of(15))
+                //   ,  RobotContainer.flywheels.setVelocity(() -> RPM.of(400))
+                )
+            .withTimeout(1),
+        RobotContainer.turret.setAngle(() -> Degree.of(0)).withTimeout(1));
+  }
+}

src/main/java/frc2713/robot/oi/DevControls.java

@@ -1,7 +1,10 @@
 package frc2713.robot.oi;
 
-import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.DegreesPerSecondPerSecond;
+import static edu.wpi.first.units.Units.FeetPerSecond;
+import static edu.wpi.first.units.Units.FeetPerSecondPerSecond;
 import static edu.wpi.first.units.Units.RPM;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -10,7 +13,6 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import frc2713.robot.GameCommandGroups;
-import frc2713.robot.RobotContainer;
 import frc2713.robot.commands.DriveCommands;
 import frc2713.robot.subsystems.drive.Drive;
 import frc2713.robot.subsystems.intake.IntakeExtension;
@@ -21,6 +23,7 @@
 import frc2713.robot.subsystems.launcher.Turret;
 import frc2713.robot.subsystems.serializer.DyeRotor;
 import frc2713.robot.subsystems.serializer.Feeder;
+import java.util.Optional;
 
 @SuppressWarnings("unused")
 public class DevControls {
@@ -88,16 +91,16 @@ public void configureButtonBindings() {
     controller.povLeft().onTrue(flywheels.stop());
 
     // Test setting drive limits
-    // controller
-    //     .x()
-    //     .onTrue(
-    //         DriveCommands.setDriveLimits(
-    //             drive,
-    //             Optional.of(FeetPerSecond.of(2.0)),
-    //             Optional.of(FeetPerSecondPerSecond.of(12.0)),
-    //             Optional.of(DegreesPerSecond.of(90.0)),
-    //             Optional.of(DegreesPerSecondPerSecond.of(360.0))))
-    //     .onFalse(DriveCommands.clearDriveLimits(drive));
+    controller
+        .a()
+        .onTrue(
+            DriveCommands.setDriveLimits(
+                drive,
+                Optional.of(FeetPerSecond.of(1.0)),
+                Optional.of(FeetPerSecondPerSecond.of(12.0)),
+                Optional.of(DegreesPerSecond.of(90.0)),
+                Optional.of(DegreesPerSecondPerSecond.of(360.0))));
+    controller.b().onTrue(DriveCommands.clearDriveLimits(drive));
 
     // Intake Controls
 
@@ -133,9 +136,9 @@ public void configureButtonBindings() {
 
     // Turret Controls
 
-    controller.a().whileTrue(turret.setAngleStopAtBounds(LauncherConstants.Turret.PIDTestAngleOne));
+    // controller.a().whileTrue(turret.setAngleStopAtBounds(LauncherConstants.Turret.PIDTestAngleOne));
 
-    controller.b().whileTrue(turret.setAngleStopAtBounds(LauncherConstants.Turret.PIDTestAngleTwo));
+    // controller.b().whileTrue(turret.setAngleStopAtBounds(LauncherConstants.Turret.PIDTestAngleTwo));
 
     // controller
     //     .a()
@@ -186,13 +189,15 @@ public void configureButtonBindings() {
     //     .onTrue(flywheels.velocitySetpointCommand(LauncherConstants.Flywheels.PIDTest))
     //     .onFalse(flywheels.velocitySetpointCommand(() -> RPM.of(0)));
 
-    controller
-        .x()
-        .onTrue(Commands.runOnce(() -> RobotContainer.drive.changeDriveCurrentLimits(Amps.of(70))));
+    // controller
+    //     .x()
+    //     .onTrue(Commands.runOnce(() ->
+    // RobotContainer.drive.changeDriveCurrentLimits(Amps.of(70))));
 
-    controller
-        .y()
-        .onTrue(Commands.runOnce(() -> RobotContainer.drive.changeDriveCurrentLimits(Amps.of(80))));
+    // controller
+    //     .y()
+    //     .onTrue(Commands.runOnce(() ->
+    // RobotContainer.drive.changeDriveCurrentLimits(Amps.of(80))));
 
     // controller
     //     .x()

src/main/java/frc2713/robot/subsystems/launcher/LauncherConstants.java

@@ -137,14 +137,14 @@ public final class Flywheels {
     public static TalonFXSubsystemConfig leaderConfig = new TalonFXSubsystemConfig();
     public static TalonFXSubsystemConfig followerConfig = new TalonFXSubsystemConfig();
     public static MomentOfInertia flywhMomentOfInertia = MoiUnits.PoundSquareInches.of(10.410164);
-    public static double gearRatio = 24.0 / 18.0; // 1.33:1 reduction from motor to flywheel
+    public static double gearRatio = 21.0 / 21.0; // 1.33:1 reduction from motor to flywheel
 
     static {
       leaderConfig.name = "Flywheels";
       leaderConfig.talonCANID = new CANDeviceId(50, "canivore");
-      leaderConfig.fxConfig.Slot0.kP = Util.modeDependentValue(.7, 3.5);
+      leaderConfig.fxConfig.Slot0.kP = Util.modeDependentValue(.5, 3.5);
       leaderConfig.fxConfig.Slot0.kI = 0.0;
-      leaderConfig.fxConfig.Slot0.kD = 0.004;
+      leaderConfig.fxConfig.Slot0.kD = 0.00;
       leaderConfig.fxConfig.Slot0.kS = Util.modeDependentValue(0.2, 2.0);
       leaderConfig.fxConfig.Slot0.kV = 0.12 * gearRatio;
       leaderConfig.fxConfig.CurrentLimits.StatorCurrentLimit = 120.0;
@@ -208,9 +208,9 @@ public final class Flywheels {
 
       rpmVelocityMap.put(1.03, 2500.);
       rpmVelocityMap.put(2.1, 2500.);
-      rpmVelocityMap.put(3.36, 2713.);
-      rpmVelocityMap.put(5.0, 3250.);
-      rpmVelocityMap.put(6.03, 4200.);
+      rpmVelocityMap.put(3.36, 3500.);
+      rpmVelocityMap.put(5.0, 4000.);
+      rpmVelocityMap.put(6.03, 5000.);
 
       rpmVelocityAZMap.put(1.03, 2500.);
       rpmVelocityAZMap.put(2.1, 2500.);

src/main/java/frc2713/robot/subsystems/launcher/Turret.java

@@ -52,7 +52,7 @@ public Turret(
       final CanCoderInputsAutoLogged cancoderInputs,
       final CanCoderIO cancoderIO) {
     super(config, new MotorInputsAutoLogged(), turretMotorIO, cancoderInputs, cancoderIO);
-    setDefaultCommand(otfCommand());
+    // setDefaultCommand(otfCommand());
   }
 
   @AutoLogOutput

src/main/java/frc2713/robot/subsystems/vision/Vision.java

@@ -1,9 +1,7 @@
 package frc2713.robot.subsystems.vision;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc2713.robot.FieldConstants;
 import frc2713.robot.RobotContainer;
 import frc2713.robot.subsystems.vision.VisionConstants.PoseEstimatorErrorStDevs;
 import java.util.Optional;
@@ -23,17 +21,7 @@ public Vision(VisionIO io) {
   public void periodic() {
     io.updateInputs(inputs);
 
-    if (inputs.applying && DriverStation.isEnabled()) {
-      if (!FieldConstants.FIELD_PLUS_HALF_METER.contains(
-          RobotContainer.drive.getPose().getTranslation())) {
-        inputs.reasoning = "ROBOT OUTSIDE FIELD!! HARD RESET";
-        RobotContainer.drive.setPose(inputs.pose);
-        Logger.recordOutput("Odometry/inFieldPlus", false);
-
-      } else {
-        Logger.recordOutput("Odometry/inFieldPlus", true);
-      }
-
+    if (inputs.applying) {
       RobotContainer.drive.addVisionMeasurement(
           inputs.pose,
           inputs.timestamp,

src/main/java/frc2713/robot/subsystems/vision/VisionConstants.java

@@ -19,7 +19,7 @@
 import frc2713.lib.util.LoggedTunableMeasure;
 
 public class VisionConstants {
-  public static Distance MAX_POSE_JUMP = Meters.of(2);
+  public static Distance MAX_POSE_JUMP = Meters.of(1);
   public static LinearVelocity MAX_LINEAR_SPEED =
       MetersPerSecond.of(0.02); // TODO: reasoning based on linear speed
   public static AngularVelocity MAX_ANGULAR_SPEED =
@@ -43,7 +43,7 @@ public Matrix<N3, N1> toMatrix() {
   // on other sensors (e.g., gyro/odometry) for heading, since vision rotation
   // data is considered too unreliable/noisy for this robot.
   public static PoseEstimatorErrorStDevs POSE_ESTIMATOR_STATE_STDEVS =
-      new PoseEstimatorErrorStDevs(Meters.of(0.03), Degrees.of(999));
+      new PoseEstimatorErrorStDevs(Meters.of(0.01), Degrees.of(5));
 
   public static PoseEstimatorErrorStDevs POSE_ESTIMATOR_STATE_LOW_TAGS_FAST_STDEVS =
       new PoseEstimatorErrorStDevs(Meters.of(0.8), Degrees.of(99999));

src/main/java/frc2713/robot/subsystems/vision/VisionIO.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.units.measure.Time;
@@ -15,6 +16,7 @@ public static class VisionInputs {
     public Pose2d pose = new Pose2d();
     // pose3d can be left unchanged for visualization
     public Pose3d pose3d = new Pose3d();
+    public Rotation2d rawYaw = new Rotation2d();
     public double timestamp;
     public Time latency;
 
@@ -26,6 +28,8 @@ public static class VisionInputs {
     public Distance translationStdDev =
         VisionConstants.POSE_ESTIMATOR_STATE_STDEVS.translationalStDev();
     public Angle rotationStdDev = VisionConstants.POSE_ESTIMATOR_STATE_STDEVS.rotationalStDev();
+    public double subgraphError;
+    public double avgTagSize;
   }
 
   public default void updateInputs(VisionInputs inputs) {}