merge main into turret

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

@@ -15,14 +15,17 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import org.team2342.lib.motors.MotorConfig;
 import org.team2342.lib.motors.MotorConfig.IdleMode;
 import org.team2342.lib.motors.smart.SmartMotorConfig;
 import org.team2342.lib.motors.smart.SmartMotorConfig.ControlType;
+import org.team2342.lib.motors.smart.SmartMotorConfig.FeedbackConfig;
+import org.team2342.lib.pidff.PIDFFConfigs;
 import org.team2342.lib.util.CameraParameters;
 
 public final class Constants {
   public static final Mode CURRENT_MODE = Mode.REAL;
-  public static final boolean TUNING = true;
+  public static final boolean TUNING = false;
 
   public static enum Mode {
     /** Running on a real robot. */
@@ -36,18 +39,36 @@ public static enum Mode {
   }
 
   public static final class VisionConstants {
-    public static final String CAMERA_NAME = "left_arducam";
+    public static final String SWERVE_CAMERA_NAME = "swerve_arducam";
+    public static final String SHOOTER_CAMERA_NAME = "shooter_arducam";
 
-    public static final Transform3d CAMERA_TRANSFORM =
+    public static final Transform3d SWERVE_CAMERA_TRANSFORM =
         new Transform3d(
             new Translation3d(
-                Units.inchesToMeters(7.883 + 0.5),
-                Units.inchesToMeters(-10.895 - 0.5),
+                Units.inchesToMeters(-7.883),
+                Units.inchesToMeters(10.895),
                 Units.inchesToMeters(8)),
-            new Rotation3d(0, Units.degreesToRadians(-22.0), Units.degreesToRadians(90 - 61.475)));
+            new Rotation3d(
+                0, Units.degreesToRadians(-22.0), Units.degreesToRadians(-(90 + 61.475))));
 
-    public static final CameraParameters LEFT_PARAMETERS =
-        CameraParameters.loadFromName(CAMERA_NAME, 800, 600).withTransform(CAMERA_TRANSFORM);
+    public static final Transform3d SHOOTER_CAMERA_TRANSFORM =
+        new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(-13.255),
+                Units.inchesToMeters(11.9),
+                Units.inchesToMeters(14.755)),
+            new Rotation3d(
+                0,
+                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 SHOOTER_CAMERA_PARAMETERS =
+        CameraParameters.loadFromName(SHOOTER_CAMERA_NAME, 800, 600)
+            .withTransform(SHOOTER_CAMERA_TRANSFORM);
 
     // Basic filtering thresholds
     public static final double MAX_AMBIGUITY = 0.1;
@@ -78,7 +99,7 @@ public static final class DriveConstants {
     public static final double CONTROLLER_DEADBAND = 0.1;
     public static final double ROTATION_LOCK_TIME = 0.25;
 
-    public static final double MAX_LINEAR_SPEED = Units.feetToMeters(15.5);
+    public static final double MAX_LINEAR_SPEED = Units.feetToMeters(15.0);
     public static final double MAX_LINEAR_ACCELERATION = 20.0;
     public static final double DRIVE_GEARING = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0);
     public static final double TURN_GEARING = 150.0 / 7.0;
@@ -87,12 +108,13 @@ public static final class DriveConstants {
     public static final double WHEEL_RADIUS = Units.inchesToMeters(2.0);
     public static final double WHEEL_COF = 1.2;
 
-    public static final double TRACK_WIDTH_X = Units.inchesToMeters(28.0 - (2.625 * 2));
-    public static final double TRACK_WIDTH_Y = Units.inchesToMeters(28.0 - (2.625 * 2));
+    public static final double TRACK_WIDTH_X = Units.inchesToMeters(27.0 - (2.625 * 2));
+    public static final double TRACK_WIDTH_Y = Units.inchesToMeters(27.0 - (2.625 * 2));
     public static final double DRIVE_BASE_RADIUS =
         Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
     public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
 
+    // TODO: New Mass & MOI
     public static final double ROBOT_MASS_KG = Units.lbsToKilograms(112);
     public static final double ROBOT_MOI = 5.278;
 
@@ -101,9 +123,8 @@ public static final class DriveConstants {
     public static final double DRIVE_SUPPLY_LIMIT = 40.0;
     public static final double MAX_MODULE_VELOCITY_RAD = Units.degreesToRadians(1080.0);
 
-    // TODO: Find new offsets for season modules
     public static final double[] ENCODER_OFFSETS = {
-      0.229 + 0.5, 0.2834 + 0.5, 0.2009 + 0.5, 0.1563 + 0.5
+      0.12109375 + 0.5, -0.142333984375 + 0.5, -0.3896484375 + 0.5, -0.150146484375 + 0.5,
     };
 
     // Pitch, Roll, Yaw
@@ -113,6 +134,116 @@ public static final class DriveConstants {
     public static final double ODOMETRY_FREQUENCY = IS_CANFD ? 250.0 : 100.0;
   }
 
+  public static final class ConductorConstants {
+    public static final double TRENCH_BUFFER = 0.1;
+  }
+
+  public static final class IndexerConstants {
+    public static final double RUN_VOLTAGE = 8.0;
+    public static final double FEEDER_VOLTAGE = 10.0;
+
+    public static final double RUN_CURRENT = 30.0;
+    public static final double FEEDER_CURRENT = 30.0;
+
+    public static final MotorConfig INDEXER_BELT_CONFIG =
+        new MotorConfig()
+            .withMotorInverted(false)
+            .withSupplyCurrentLimit(30.0)
+            .withStatorCurrentLimit(70.0)
+            .withIdleMode(MotorConfig.IdleMode.BRAKE);
+    public static final MotorConfig INDEXER_FEEDER_CONFIG =
+        new MotorConfig()
+            .withMotorInverted(false)
+            .withSupplyCurrentLimit(30.0)
+            .withStatorCurrentLimit(70.0)
+            .withIdleMode(MotorConfig.IdleMode.BRAKE);
+
+    public static final DCMotor INDEXER_BELT_SIM_MOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotorSim INDEXER_BELT_SIM =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(INDEXER_BELT_SIM_MOTOR, 0.003, 1),
+            INDEXER_BELT_SIM_MOTOR);
+
+    public static final DCMotor INDEXER_FEEDER_SIM_MOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotorSim INDEXER_FEEDER_SIM =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(INDEXER_FEEDER_SIM_MOTOR, 0.0025, 1),
+            INDEXER_FEEDER_SIM_MOTOR);
+  }
+
+  public static final class IntakeConstants {
+    public static final double RUN_VOLTAGE = 7.0;
+    public static final double RUN_CURRENT = 10.0;
+
+    public static final MotorConfig INTAKE_WHEELS_MOTOR_CONFIG =
+        new MotorConfig()
+            .withMotorInverted(true)
+            .withSupplyCurrentLimit(30.0)
+            .withStatorCurrentLimit(70.0)
+            .withIdleMode(IdleMode.COAST);
+
+    public static final DCMotor INTAKE_WHEELS_SIM_MOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotorSim INTAKE_WHEEL_SIM =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(INTAKE_WHEELS_SIM_MOTOR, 0.003, 1),
+            INTAKE_WHEELS_SIM_MOTOR);
+  }
+
+  public static final class ShooterConstants {
+    public static final double FLYWHEEL_GEAR_RATIO = 23.0 / 24.0;
+    public static final double FLYWHEEL_RADIUS_METERS = Units.inchesToMeters(2.0);
+
+    public static final double IDLE_SPEED = 15.0;
+
+    public static final PIDFFConfigs FLYWHEEL_PID_CONFIGS =
+        new PIDFFConfigs()
+            .withKP(0.5)
+            .withKI(0.0)
+            .withKS(Units.radiansToRotations(0.1824))
+            .withKV(Units.radiansToRotations(0.020077))
+            .withKA(Units.radiansToRotations(0.0037398));
+    public static final SmartMotorConfig FLYWHEEL_CONFIG =
+        new SmartMotorConfig()
+            .withControlType(ControlType.VELOCITY)
+            .withGearRatio(FLYWHEEL_GEAR_RATIO)
+            .withMotorInverted(false)
+            .withSupplyCurrentLimit(40)
+            .withStatorCurrentLimit(70)
+            .withProfileConstraintsRad(new TrapezoidProfile.Constraints(1000, 1000));
+    public static final DCMotor FLYWHEEL_SIM_MOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotorSim FLYWHEEL_SIM =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(FLYWHEEL_SIM_MOTOR, 0.03, FLYWHEEL_GEAR_RATIO),
+            FLYWHEEL_SIM_MOTOR);
+
+    public static final double KRAKEN_TO_ENCODER = (64.0 / 14.0) * (46.0 / 20.0);
+    public static final double ENCODER_TO_HOOD = 344.0 / 22.0;
+    public static final double MAX_ANGLE = 0.273;
+    public static final double TARGET_TOLERANCE = 0.01;
+
+    public static final PIDFFConfigs HOOD_MOTOR_PID_CONFIGS =
+        new PIDFFConfigs().withKP(400).withKI(100).withKD(30);
+    public static final SmartMotorConfig HOOD_MOTOR_CONFIG =
+        new SmartMotorConfig()
+            .withGearRatio(ENCODER_TO_HOOD)
+            .withControlType(ControlType.PROFILED_POSITION)
+            .withIdleMode(IdleMode.BRAKE)
+            .withSupplyCurrentLimit(30.0)
+            .withFeedbackConfig(
+                FeedbackConfig.fused(
+                    CANConstants.HOOD_ENCODER_ID, KRAKEN_TO_ENCODER, 0.662 / 2, false))
+            .withProfileConstraintsRad(
+                new TrapezoidProfile.Constraints(
+                    Units.degreesToRadians(1800), Units.degreesToRadians(1800)));
+
+    public static final DCMotor HOOD_SIM_MOTOR = DCMotor.getKrakenX60(1);
+    public static final DCMotorSim HOOD_SIM =
+        new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(
+                HOOD_SIM_MOTOR, 0.01, (KRAKEN_TO_ENCODER * ENCODER_TO_HOOD)),
+            HOOD_SIM_MOTOR);
+  }
+
   public static final class TurretConstants {
     public static final double AT_POSITION_THRESHOLD = 0.01;
     public static final Rotation2d MIN_TURRET_ANGLE =
@@ -142,6 +273,18 @@ public static final class CANConstants {
     public static final int[] BL_IDS = {3, 7, 11};
     public static final int[] BR_IDS = {4, 8, 12};
 
+    public static final int FLYWHEEL_MOTOR_ID = 14;
+    public static final int HOOD_MOTOR_ID = 15;
+    public static final int HOOD_ENCODER_ID = 16;
+
+    public static final int INTAKE_WHEEL_MOTOR_ID = 17;
+    public static final int INTAKE_PIVOT_MOTOR_ID = 18;
+    public static final int INTAKE_PIVOT_ENCODER_ID = 19;
+
+    public static final int INDEXER_WHEEL_ID = 20;
+    public static final int INDEXER_BELT_ID = 21;
+    public static final int INDEXER_FEEDER_ID = 22;
+
     public static final int PDH_ID = 62;
   }
 }

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

@@ -29,6 +29,7 @@
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
+import org.team2342.frc.util.FiringSolver;
 import org.team2342.frc.util.PhoenixUtils;
 import org.team2342.lib.logging.ExecutionLogger;
 
@@ -158,6 +159,7 @@ public void robotPeriodic() {
     ExecutionLogger.log("Commands");
 
     robotContainer.updateAlerts();
+    FiringSolver.getInstance().clearCachedSolution();
 
     ExecutionLogger.log("RobotPeriodic");
   }
@@ -173,6 +175,10 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
+    robotContainer.getDrive().calculateVisionHeadingOffset();
+
+    CommandScheduler.getInstance().schedule(robotContainer.getWheels().out().withTimeout(1));
+
     autonomousCommand = robotContainer.getAutonomousCommand();
 
     if (autonomousCommand != null) {