Flywheel tuning

src/main/java/org/team2342/frc/Constants.java

@@ -39,8 +39,8 @@ public static enum Mode {
   }
 
   public static final class VisionConstants {
-    public static final String SWERVE_CAMERA_NAME = "swerve_arducam";
-    public static final String SHOOTER_CAMERA_NAME = "shooter_arducam";
+    public static final String LEFT_CAMERA_NAME = "left_arducam";
+    public static final String RIGHT_CAMERA_NAME = "right_arducam";
 
     public static final Transform3d SWERVE_CAMERA_TRANSFORM =
         new Transform3d(
@@ -51,6 +51,22 @@ public static final class VisionConstants {
             new Rotation3d(
                 0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-(90 + 61.475))));
 
+    public static final Transform3d RIGHT_CAMERA =
+        new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(-13.428),
+                Units.inchesToMeters(3.917),
+                Units.inchesToMeters(7.618)),
+            new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(190)));
+
+    public static final Transform3d LEFT_CAMERA =
+        new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(-13.428),
+                Units.inchesToMeters(-3.931),
+                Units.inchesToMeters(7.618)),
+            new Rotation3d(0, Units.degreesToRadians(-30), Units.degreesToRadians(-190)));
+
     public static final Transform3d SHOOTER_CAMERA_TRANSFORM =
         new Transform3d(
             new Translation3d(
@@ -62,13 +78,11 @@ public static final class VisionConstants {
                 Units.degreesToRadians((90 - 63.435 + 180)),
                 Units.degreesToRadians(-119.745 + 90)));
 
-    public static final CameraParameters SWERVE_CAMERA_PARAMETERS =
-        CameraParameters.loadFromName(SWERVE_CAMERA_NAME, 800, 600)
-            .withTransform(SWERVE_CAMERA_TRANSFORM);
+    public static final CameraParameters LEFT_CAMERA_PARAMETERS =
+        CameraParameters.loadFromName(LEFT_CAMERA_NAME, 800, 600).withTransform(LEFT_CAMERA);
 
-    public static final CameraParameters SHOOTER_CAMERA_PARAMETERS =
-        CameraParameters.loadFromName(SHOOTER_CAMERA_NAME, 800, 600)
-            .withTransform(SHOOTER_CAMERA_TRANSFORM);
+    public static final CameraParameters RIGHT_CAMERA_PARAMETERS =
+        CameraParameters.loadFromName(LEFT_CAMERA_NAME, 800, 600).withTransform(RIGHT_CAMERA);
 
     // Basic filtering thresholds
     public static final double MAX_AMBIGUITY = 0.1;
@@ -151,7 +165,7 @@ public static final class IndexerConstants {
             .withStatorCurrentLimit(70.0)
             .withIdleMode(MotorConfig.IdleMode.COAST);
 
-    public static final DCMotor INDEXER_SIM_MOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotor INDEXER_SIM_MOTOR = DCMotor.getKrakenX44(1);
     public static final DCMotorSim INDEXER_SIM =
         new DCMotorSim(
             LinearSystemId.createDCMotorSystem(INDEXER_SIM_MOTOR, 0.003, 1), INDEXER_SIM_MOTOR);
@@ -180,7 +194,7 @@ public static final class IntakeConstants {
             .withStatorCurrentLimit(70.0)
             .withIdleMode(IdleMode.COAST);
 
-    public static final DCMotor INTAKE_WHEELS_SIM_MOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotor INTAKE_WHEELS_SIM_MOTOR = DCMotor.getKrakenX44(1);
     public static final DCMotorSim INTAKE_WHEEL_SIM =
         new DCMotorSim(
             LinearSystemId.createDCMotorSystem(INTAKE_WHEELS_SIM_MOTOR, 0.003, 1),
@@ -201,7 +215,7 @@ public static final class IntakeConstants {
             .withSupplyCurrentLimit(40.0)
             .withFeedbackConfig(
                 FeedbackConfig.fused(
-                    CANConstants.INTAKE_PIVOT_ENCODER_ID, PIVOT_GEAR_RATIO, 0.681, true))
+                    CANConstants.INTAKE_PIVOT_ENCODER_ID, PIVOT_GEAR_RATIO, 0.01, true))
             .withProfileConstraintsRad(
                 new TrapezoidProfile.Constraints(
                     Units.degreesToRadians(1800), Units.degreesToRadians(540)));
@@ -212,7 +226,8 @@ public static final class ShooterConstants {
     public static final double FLYWHEEL_RADIUS_METERS = Units.inchesToMeters(2.0);
 
     public static final double IDLE_SPEED = 15.0;
-    public static final double FLYWHEEL_AT_GOAL_TOLERANCE = 3.0;
+    public static final double FLYWHEEL_AT_GOAL_TOLERANCE =
+        2.0 / ShooterConstants.FLYWHEEL_RADIUS_METERS;
 
     public static final PIDFFConfigs FLYWHEEL_PID_CONFIGS =
         new PIDFFConfigs()
@@ -265,8 +280,8 @@ public static final class TurretConstants {
     public static final double AT_POSITION_THRESHOLD = 0.01;
 
     public static final double STARTING_ANGLE = Units.degreesToRadians(90);
-    public static final double MIN_TURRET_ANGLE = Units.degreesToRadians(0);
-    public static final double MAX_TURRET_ANGLE = Units.degreesToRadians(359);
+    public static final double MIN_TURRET_ANGLE = Units.degreesToRadians(5);
+    public static final double MAX_TURRET_ANGLE = Units.degreesToRadians(355);
 
     public static final double GEAR_RATIO = (48.0 / 12.0) * (100.0 / 10.0);
 

src/main/java/org/team2342/frc/Robot.java

@@ -13,6 +13,7 @@
 import edu.wpi.first.math.MathShared;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.IterativeRobotBase;
@@ -30,6 +31,7 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import org.team2342.frc.Constants.VisionConstants;
 import org.team2342.frc.util.FiringSolver;
 import org.team2342.frc.util.HubShiftUtil;
 import org.team2342.frc.util.PhoenixUtils;
@@ -161,18 +163,22 @@ public void robotPeriodic() {
     ExecutionLogger.log("Commands");
 
     Logger.recordOutput("ShiftUtil/Official", HubShiftUtil.getOfficialShiftInfo());
+    Logger.recordOutput("ShiftUtil/Shifted", HubShiftUtil.getShiftedShiftInfo());
 
     Logger.recordOutput(
-        "ShiftUtil/Dashboard/CurrentShift", HubShiftUtil.getShiftedShiftInfo().currentShift());
+        "ShiftUtil/Dashboard/CurrentShift", HubShiftUtil.getOfficialShiftInfo().currentShift());
     Logger.recordOutput(
-        "ShiftUtil/Dashboard/RemainingTime", HubShiftUtil.getShiftedShiftInfo().remainingTime());
-    Logger.recordOutput("ShiftUtil/Dashboard/Active", HubShiftUtil.getShiftedShiftInfo().active());
+        "ShiftUtil/Dashboard/RemainingTime", HubShiftUtil.getOfficialShiftInfo().remainingTime());
+    Logger.recordOutput("ShiftUtil/Dashboard/Active", HubShiftUtil.getOfficialShiftInfo().active());
 
-    Logger.recordOutput("ShiftUtil/Shifted", HubShiftUtil.getShiftedShiftInfo());
     Logger.recordOutput("TurretManual", Units.radiansToDegrees(robotContainer.getTurretManual()));
     Logger.recordOutput("FlywheelManual", robotContainer.getFlywheelManual());
     Logger.recordOutput("Vision/HasTags", robotContainer.getVision().hasTags());
 
+    Logger.recordOutput("zero", Pose3d.kZero);
+    Logger.recordOutput("camera", Pose3d.kZero.plus(VisionConstants.LEFT_CAMERA));
+    Logger.recordOutput("camera2", Pose3d.kZero.plus(VisionConstants.RIGHT_CAMERA));
+
     robotContainer.updateAlerts();
     FiringSolver.getInstance().clearCachedSolution();
 

src/main/java/org/team2342/frc/RobotContainer.java

@@ -49,7 +49,6 @@
 import org.team2342.frc.subsystems.indexer.Indexer;
 import org.team2342.frc.subsystems.intake.Pivot;
 import org.team2342.frc.subsystems.intake.Wheels;
-import org.team2342.frc.subsystems.leds.LEDSubsystem;
 import org.team2342.frc.subsystems.shooter.Flywheel;
 import org.team2342.frc.subsystems.shooter.Kicker;
 import org.team2342.frc.subsystems.shooter.Turret;
@@ -60,8 +59,6 @@
 import org.team2342.frc.util.FieldConstants;
 import org.team2342.frc.util.FiringSolver;
 import org.team2342.frc.util.HubShiftUtil;
-import org.team2342.lib.leds.LedIO;
-import org.team2342.lib.leds.LedIOCANdle;
 import org.team2342.lib.motors.dumb.DumbMotorIO;
 import org.team2342.lib.motors.dumb.DumbMotorIOSim;
 import org.team2342.lib.motors.dumb.DumbMotorIOSparkFlex;
@@ -84,7 +81,6 @@ public class RobotContainer {
   @Getter private final Wheels wheels;
   @Getter private final Flywheel flywheel;
   @Getter private final Turret turret;
-  @Getter private final LEDSubsystem leds;
 
   @Getter private final Conductor conductor;
 
@@ -106,6 +102,7 @@ public class RobotContainer {
   private final Trigger allianceZoneTrigger;
   private final Trigger shiftAboutToEnd;
   private final Trigger activeOrPassing;
+  private final Trigger readyToFire;
 
   @Getter private double turretManual, flywheelManual;
 
@@ -124,7 +121,11 @@ public RobotContainer() {
                 drive::addVisionMeasurement,
                 drive::getTimestampedHeading,
                 new VisionIOPhoton(
-                    VisionConstants.SWERVE_CAMERA_PARAMETERS,
+                    VisionConstants.LEFT_CAMERA_PARAMETERS,
+                    PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+                    PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR),
+                new VisionIOPhoton(
+                    VisionConstants.RIGHT_CAMERA_PARAMETERS,
                     PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
                     PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
         turret =
@@ -173,13 +174,6 @@ public RobotContainer() {
                 drive::getChassisSpeeds,
                 () -> turretManual,
                 () -> flywheelManual);
-        leds =
-            new LEDSubsystem(
-                new LedIOCANdle(CANConstants.CANDLE_ID, 32),
-                "CANdle",
-                vision::hasTags,
-                conductor::getCurrentState);
-
         LoggedPowerDistribution.getInstance(CANConstants.PDH_ID, ModuleType.kRev);
         break;
 
@@ -196,12 +190,12 @@ public RobotContainer() {
                 drive::addVisionMeasurement,
                 drive::getTimestampedHeading,
                 new VisionIOSim(
-                    VisionConstants.SWERVE_CAMERA_PARAMETERS,
+                    VisionConstants.LEFT_CAMERA_PARAMETERS,
                     PoseStrategy.CONSTRAINED_SOLVEPNP,
                     PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
                     drive::getRawOdometryPose),
                 new VisionIOSim(
-                    VisionConstants.SHOOTER_CAMERA_PARAMETERS,
+                    VisionConstants.RIGHT_CAMERA_PARAMETERS,
                     PoseStrategy.CONSTRAINED_SOLVEPNP,
                     PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
                     drive::getRawOdometryPose));
@@ -238,9 +232,6 @@ public RobotContainer() {
                 drive::getChassisSpeeds,
                 () -> turretManual,
                 () -> flywheelManual);
-        leds =
-            new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState);
-
         break;
 
       default:
@@ -273,9 +264,6 @@ public RobotContainer() {
                 drive::getChassisSpeeds,
                 () -> turretManual,
                 () -> flywheelManual);
-        leds =
-            new LEDSubsystem(new LedIO() {}, "CANdle", vision::hasTags, conductor::getCurrentState);
-
         break;
     }
 
@@ -315,10 +303,11 @@ public RobotContainer() {
     activeOrPassing =
         new Trigger(
             () ->
-                HubShiftUtil.getShiftedShiftInfo().active()
+                HubShiftUtil.getOfficialShiftInfo().active()
                     || FiringSolver.getInstance()
                         .calculate(drive.getChassisSpeeds(), drive.getPose())
                         .passing());
+    readyToFire = new Trigger(() -> turret.aroundGoal() && flywheel.atGoal());
 
     configureBindings();
   }
@@ -329,15 +318,14 @@ private void configureNamedCommands() {
         "autoShoot",
         conductor
             .runState(ConductorState.WARM_UP)
-            .withTimeout(2.0)
+            .withTimeout(1.0)
             .andThen(
                 conductor
                     .runState(ConductorState.TRACKED_FIRING)
-                    .alongWith(pivot.holdAngle(0))
-                    .alongWith(wheels.in())
-                    .alongWith(indexer.in())
-                    .alongWith(disruptor.in())
-                    .alongWith(kicker.in())));
+                    .alongWith(pivot.holdAngle(IntakeConstants.MIN_ANGLE))
+                    .alongWith(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))
+                    .withTimeout(3.0))
+            .finallyDo(() -> Commands.parallel(indexer.stop(), kicker.stop(), disruptor.stop())));
 
     NamedCommands.registerCommand(
         "autoIntake",
@@ -392,7 +380,7 @@ private void configureBindings() {
     driverController
         .leftTrigger()
         .whileTrue(wheels.in().alongWith(pivot.holdAngle(IntakeConstants.MIN_ANGLE)))
-        .onFalse(wheels.stop().alongWith(pivot.stop()));
+        .onFalse(wheels.stop().alongWith(pivot.holdAngle(0.875)));
 
     // Retract Intake
     driverController
@@ -414,6 +402,7 @@ private void configureBindings() {
     driverController
         .rightTrigger()
         .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING))
+        .and(readyToFire)
         .and(activeOrPassing)
         .whileTrue(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))
         .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), disruptor.stop()));
@@ -428,6 +417,7 @@ private void configureBindings() {
     driverController
         .rightBumper()
         .whileTrue(conductor.runState(ConductorState.TRACKED_FIRING))
+        .and(readyToFire)
         .whileTrue(Commands.parallel(indexer.pulseIn(), kicker.in(), disruptor.in()))
         .onFalse(Commands.parallel(indexer.stop(), kicker.stop(), disruptor.stop()));
 

src/main/java/org/team2342/frc/subsystems/Conductor.java

@@ -107,7 +107,7 @@ private void setupStateCommands() {
                         .calculate(velocitySupplier.get(), poseSupplier.get())
                         .turretAngle())
             .alongWith(
-                flywheel.warmUpCommand(
+                flywheel.shoot(
                     () ->
                         FiringSolver.getInstance()
                             .calculate(velocitySupplier.get(), poseSupplier.get())

src/main/java/org/team2342/frc/subsystems/intake/Pivot.java

@@ -71,6 +71,8 @@ public Command stop() {
   }
 
   public Command agitate() {
-    return Commands.repeatingSequence(goToAngle(1.4), goToAngle(1.5));
+    return Commands.repeatingSequence(
+            holdAngle(1.4).withTimeout(0.5), holdAngle(0.5).withTimeout(0.5))
+        .withName("Pivot Agitate");
   }
 }

src/main/java/org/team2342/frc/subsystems/shooter/Turret.java

@@ -113,6 +113,10 @@ public boolean atGoal() {
     return Math.abs(inputs.positionRad - goal) <= TurretConstants.AT_POSITION_THRESHOLD;
   }
 
+  public boolean aroundGoal() {
+    return Math.abs(inputs.positionRad - goal) <= 0.05;
+  }
+
   public void zeroTurret() {
     turretMotor.setPosition(TurretConstants.STARTING_ANGLE);
     goal = TurretConstants.STARTING_ANGLE;