Factory edits

src/main/java/com/team900/frc2026/factories/HoodFactory.java

@@ -1,31 +1,74 @@
 package com.team900.frc2026.factories;
 
 import com.team900.frc2026.RobotContainer;
+import com.team900.frc2026.RobotState;
 import com.team900.frc2026.subsystems.hood.HoodConstants;
 import com.team900.frc2026.subsystems.hood.HoodSubsystem;
+import com.team900.lib.util.ShooterSetpoint;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
 import java.util.function.Supplier;
 
 public class HoodFactory {
 
     // Sets the hood to a fixed position in radians
-    public static Command setPosition(RobotContainer container, double radians) {
+    public static Command setPositionMotionMagicCommand(double radians, RobotContainer container) {
+        HoodSubsystem hood = container.getHoodSubsystem();
+        return hood.motionMagicSetpointCommand(() -> radians).withName("Hood Set Position");
+    }
+
+    public static Command aimHoodToPose(
+            Supplier<ShooterSetpoint> setPointSupplier, RobotContainer container) {
         HoodSubsystem hood = container.getHoodSubsystem();
-        return hood.motionMagicSetpointCommand(() -> radians)
-                .withName("Hood Set Position");
+        return Commands.run(
+                        () ->
+                                hood.setPositionRadians(
+                                        setPointSupplier.get().getHoodRadians(),
+                                        setPointSupplier.get().getHoodFF()),
+                        hood)
+                .withName("Aim Hood to Pose (rad)");
     }
 
     // Sets the hood to a fixed position and finishes when it arrives within the tolerance
     public static Command setPositionBlocking(
-            RobotContainer container, double radians, double tolerance) {
-        HoodSubsystem hood = container.getHoodSubsystem();
-        return hood.motionMagicSetpointCommandBlocking(() -> radians, tolerance)
+            double radians, double tolerance, RobotContainer container) {
+        return container
+                .getHoodSubsystem()
+                .motionMagicSetpointCommandBlocking(() -> radians, tolerance)
                 .withName("Hood Set Position Blocking");
     }
 
     // Stows the hood
     public static Command stow(RobotContainer container) {
-        return setPosition(container, HoodConstants.kHoodStowTrenchPositionRadians)
+        return setPositionMotionMagicCommand(
+                        HoodConstants.kHoodStowTrenchPositionRadians, container)
                 .withName("Hood Stow");
     }
+
+    public static Command zero(RobotContainer container) {
+        return new InstantCommand(container.getHoodSubsystem()::disableSoftLimits)
+                .andThen(
+                        container
+                                .getHoodSubsystem()
+                                .dutyCycleCommand(() -> -0.05)
+                                .until(RobotState.getInstance()::getHoodHasZeroed)
+                                .andThen(container.getHoodSubsystem()::enableSoftLimits));
+    }
+
+    public static Command pass(RobotContainer container, double tolerance) {
+        return container
+                .getHoodSubsystem()
+                .motionMagicSetpointCommandBlocking(() -> 0.6, tolerance)
+                .withName("Hood pass Position Blocking");
+    }
+
+    public static Command shoot(RobotContainer container, double tolerance) {
+        return container
+                .getHoodSubsystem()
+                .motionMagicSetpointCommandBlocking(() -> 0.3, tolerance)
+                .withName("Hood Shoot Position Blocking");
+    }
 }

src/main/java/com/team900/frc2026/factories/ShooterFactory.java

@@ -6,9 +6,8 @@
 
 import com.team900.frc2026.RobotContainer;
 import com.team900.frc2026.subsystems.shooter.ShooterConstants;
-import com.team900.lib.util.ShooterSetpoint;
+
 import edu.wpi.first.wpilibj2.command.Command;
-import java.util.function.Supplier;
 
 public class ShooterFactory {
 
@@ -17,11 +16,7 @@ public class ShooterFactory {
     /* Commands for shooting */
 
     public static Command idle() {
-        return container.getShooterSubsystem().setTorqueCurrentFOC(() -> ShooterConstants.kIdleRPM);
-    }
 
-    public static Command setShooterRPS(Supplier<ShooterSetpoint> setpointSupplier) {
-        var shooter = container.getShooterSubsystem();
-        return shooter.velocitySetpointCommand(setpointSupplier.get()::getShooterRPS);
+        return container.getShooterSubsystem().setTorqueCurrentFOC(ShooterConstants.IDLE_RPS);
     }
 }

src/main/java/com/team900/frc2026/factories/ShootingFactory.java

@@ -4,14 +4,48 @@
 
 package com.team900.frc2026.factories;
 
+import com.team900.frc2026.RobotContainer;
+import com.team900.frc2026.RobotState;
+import com.team900.lib.util.ShooterSetpoint;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import java.util.function.Supplier;
 
 public class ShootingFactory {
 
     /* Commands for shooting */
 
-    public static Command SpinBoth() {
-
-        return null;
+    public static Command shoot(
+            Supplier<ShooterSetpoint> setPointSupplier, RobotContainer container) {
+        return (new ParallelCommandGroup(
+                                ShooterFactory.setShooterRPS(setPointSupplier, container),
+                                SuperstructureFactory.aim(setPointSupplier, container))
+                        .until(
+                                () ->
+                                        MathUtil.isNear(
+                                                        setPointSupplier.get().getShooterRPS(),
+                                                        container
+                                                                .getShooterSubsystem()
+                                                                .getCurrentVelocity(),
+                                                        1)
+                                                && MathUtil.isNear(
+                                                        setPointSupplier.get().getHoodRadians(),
+                                                        container
+                                                                .getHoodSubsystem()
+                                                                .getCurrentPosition(),
+                                                        1)
+                                                && MathUtil.isNear(
+                                                        0,
+                                                        RobotState.getInstance()
+                                                                .getLatestRotationRobotToHub()
+                                                                .getDegrees(),
+                                                        3)))
+                .andThen(
+                        new ParallelCommandGroup(
+                                        IntakeFactory.runIntake(container),
+                                        HandoffFactory.runHandoff(container),
+                                        SpindexerFactory.runSpindexer(container))
+                                .onlyWhile(container.getDriveCommand()::isNearTarget));
     }
-}
+}

src/main/java/com/team900/frc2026/factories/SuperstructureFactory.java

@@ -1,18 +1,45 @@
 package com.team900.frc2026.factories;
 
+import java.util.function.Supplier;
+
 import com.team900.frc2026.RobotContainer;
+import com.team900.lib.util.ShooterSetpoint;
+
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+
+import java.util.function.Supplier;
+import java.util.function.DoubleSupplier;
 
 public class SuperstructureFactory {
 
-    public static Command aimAndShootAll(RobotContainer container) {
-        return null;
-        /*Command to shoot all balls in robot (should auto aim)
+    public static Command aim(
+            Supplier<ShooterSetpoint> setPointSupplier, 
+            RobotContainer container, 
+            DoubleSupplier turretRadiansFromCenter,
+            DoubleSupplier turretFF) {
+        // return null;
+        return new ParallelCommandGroup(
+                new InstantCommand(() -> container.getDriveCommand().setKAiming(true)),
+                HoodFactory.aimHoodToPose(setPointSupplier, container),
+                TurretFactory.aimTurretToPoseDegrees(container, turretRadiansFromCenter,turretFF));
+        /*Command to aim at target
          * Aim turret
-         * Aim hood
-         * Maintain shooter and handoff speed
-         * Move spindexer to load to turret as needed
-         */
+         * Aim hood */
+    }
+
+    public static Command stow(RobotContainer container) {
+        return new ParallelCommandGroup(HoodFactory.stow(container));
+    }
+
+    public static Command trench(RobotContainer container) {
+        return new ParallelCommandGroup(
+                HoodFactory.stow(container), IntakeFactory.deploySlapdown(container));
+    }
+
+    public static Command intakeDeploy(RobotContainer container) {
 
+        return IntakeFactory.deploySlapdown(container);
     }
 }

src/main/java/com/team900/frc2026/subsystems/hood/HoodConstants.java

@@ -25,7 +25,6 @@ public class HoodConstants {
     public static final double kHoodMinPositionRadians = Math.PI / 12;
     public static final double kHoodMaxPositionRadians = Math.PI / 4;
     public static final double kHoodZeroedAngleDegrees = 15;
-
     public static final double kHoodEpsilon = Units.degreesToRadians(1.0);
     public static final double kHoodShootingEpsilon = Units.degreesToRadians(5.0);
         //TODO: find this experimetnatlly
@@ -36,23 +35,23 @@ public class HoodConstants {
     public static CanCoderConfig kHoodCanCoderConfig = new CanCoderConfig();
 
     static {
-        // subsystem configs
+        // subsystem configs, TODO: add the freaking talon debicve id
         kHoodConfig.name = "Hood";
 
         kHoodConfig.cancoderToUnitsRatio = 170. / 10.;
         kHoodConfig.isFusedCancoder = true;
         kHoodConfig.kMaxPositionUnits = Math.PI / 4;
         kHoodConfig.kMinPositionUnits = Math.PI / 12;
         kHoodConfig.momentOfInertia = 0.0255356814;
-        kHoodConfig.talonCANID = new CANDeviceId(30, Constants.kCanBusCanivoreMech);
+        kHoodConfig.talonCANID = new CANDeviceId(0, Constants.kCanBusCanivoreMech);
         kHoodConfig.unitToRotorRatio = 15.625 * 170. / 10.;
 
         // configs for sim
         kHoodConfig.ratioForSim = kHoodGearRatio;
         kHoodConfig.cancoderUnitsForSim = 1;
 
-        // cancoder config TODO: add the freaking cancoder debicve id
-        kHoodCanCoderConfig.CANID = new CANDeviceId(0, Constants.kCanBusCanivoreMech);
+        // cancoder config
+        kHoodCanCoderConfig.CANID = new CANDeviceId(30, Constants.kCanBusCanivoreMech);
         kHoodCanCoderConfig.config.MagnetSensor.AbsoluteSensorDiscontinuityPoint = 1;
         kHoodCanCoderConfig.config.MagnetSensor.MagnetOffset = 0;
         kHoodCanCoderConfig.config.MagnetSensor.SensorDirection =

src/main/java/com/team900/frc2026/subsystems/intake/IntakeRollerConstants.java

@@ -1,6 +1,9 @@
 package com.team900.frc2026.subsystems.intake;
 
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.team900.frc2026.Constants;
 import com.team900.lib.drivers.CANDeviceId;
 import com.team900.lib.subsystems.ServoMotorSubsystemConfig;
@@ -10,15 +13,28 @@ public class IntakeRollerConstants {
     public static ServoMotorSubsystemConfig kIntakeRollerConfig = new ServoMotorSubsystemConfig();
 
     static {
-        kIntakeRollerConfig.fxConfig = new TalonFXConfiguration();
         kIntakeRollerConfig.name = "Intake";
-        kIntakeRollerConfig.talonCANID = new CANDeviceId(60, Constants.kCanBusCanivoreMech);
-        // Top Roller - 16t:24t 24t:15t 30t:18t overall: 0.5625:1
-        // Middle Roller - 16t:24t 24t:15t 30t:18t 18t:18t overall: 0.5625:1
-        // Bottom Roller - 16t:24t 24t:15t 30t:18t 36t:36t 15t:15t overall: 0.5625:1
-        kIntakeRollerConfig.unitToRotorRatio = 16.0 / 24.0 * 24.0 / 15.0 * 30.0 / 18.0;
-    }
+        kIntakeRollerConfig.talonCANID = new CANDeviceId(60, new CANBus("mech"));
+        // Top Roller - 16t:24t 24t:15t 30t:15t overall: 0.46875:1
+        // Middle Roller - 16t:24t 24t:15t 30t:15t 24t:24t overall: 0.46875:1
+        // Bottom Roller - 16t:24t 24t:15t 30t:15t 36t:36t 15t:15t overall: 0.46875:1
+        kIntakeRollerConfig.unitToRotorRatio = 16.0 / 24.0 * 24.0 / 15.0 * 30.0 / 15.0;
 
+
+        kIntakeRollerConfig.fxConfig = new TalonFXConfiguration();
+        kIntakeRollerConfig.fxConfig.OpenLoopRamps = Constants.makeDefaultOpenLoopRampConfig();
+        kIntakeRollerConfig.fxConfig.CurrentLimits.StatorCurrentLimit = 80;
+                kIntakeRollerConfig.fxConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+                        kIntakeRollerConfig.fxConfig.CurrentLimits.SupplyCurrentLimit = 70;
+                                kIntakeRollerConfig.fxConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+                                        kIntakeRollerConfig.fxConfig.CurrentLimits.SupplyCurrentLowerLimit = 40;
+                                        kIntakeRollerConfig.fxConfig.CurrentLimits.SupplyCurrentLowerTime = 1;
+        kIntakeRollerConfig.fxConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+        kIntakeRollerConfig.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+
+    }
+    public static final double kIntakeGearRatio = 0.46875;
     public static final double kIntakeDutyCycle = 0.5;
     public static final double kIntakeDutyCycleExhaust = -0.5;
 }

src/main/java/com/team900/frc2026/subsystems/shooter/ShooterConstants.java

@@ -1,27 +1,75 @@
 package com.team900.frc2026.subsystems.shooter;
 
+import java.util.function.DoubleSupplier;
+
+import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.team900.frc2026.Constants;
 import com.team900.lib.drivers.CANDeviceId;
 import com.team900.lib.subsystems.ServoMotorSubsystemWithFollowersConfig;
 import com.team900.lib.subsystems.ServoMotorSubsystemWithFollowersConfig.FollowerConfig;
 
 public class ShooterConstants {
 
-    public static ServoMotorSubsystemWithFollowersConfig kShooterConfig =
-            new ServoMotorSubsystemWithFollowersConfig();
+
+    public static ServoMotorSubsystemWithFollowersConfig kShooterConfig = new ServoMotorSubsystemWithFollowersConfig();
+    public static ServoMotorSubsystemWithFollowersConfig.FollowerConfig kShooterLeftConfig = new FollowerConfig();
 
     static {
-        kShooterConfig.name = "Shooter";
-        kShooterConfig.talonCANID = new CANDeviceId(55, Constants.kCanBusCanivoreMech);
-        kShooterConfig.unitToRotorRatio = 1.0;
+        kShooterConfig.name = "Shooter Right";
+        kShooterConfig.talonCANID = new CANDeviceId(55, new CANBus("mech"));
+        kShooterConfig.unitToRotorRatio = 1;
+
+
         kShooterConfig.fxConfig = new TalonFXConfiguration();
         kShooterConfig.fxConfig.OpenLoopRamps = Constants.makeDefaultOpenLoopRampConfig();
-        kShooterConfig.followers = new FollowerConfig[] {new FollowerConfig()};
-        kShooterConfig.followers[0].config.talonCANID =
-                new CANDeviceId(56, Constants.kCanBusCanivoreMech);
-        kShooterConfig.followers[0].config.fxConfig = new TalonFXConfiguration();
-    }
+        kShooterConfig.fxConfig.CurrentLimits.StatorCurrentLimit = 150;
+                kShooterConfig.fxConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+                        kShooterConfig.fxConfig.CurrentLimits.SupplyCurrentLimit = 80;
+                                kShooterConfig.fxConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+                                        kShooterConfig.fxConfig.CurrentLimits.SupplyCurrentLowerLimit = 40;
+                                        kShooterConfig.fxConfig.CurrentLimits.SupplyCurrentLowerTime = 1;
+        kShooterConfig.fxConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+        kShooterConfig.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+        kShooterConfig.fxConfig.TorqueCurrent.PeakForwardTorqueCurrent = 150;
+                kShooterConfig.fxConfig.TorqueCurrent.PeakReverseTorqueCurrent = 150;
+
+
+
 
-    public static final double kIdleRPM = 2000.0;
-}
+
+        kShooterLeftConfig.config.name = "Shooter Left";
+        kShooterLeftConfig.inverted = false;
+        kShooterLeftConfig.config.momentOfInertia = 1;
+        kShooterLeftConfig.config.talonCANID = new CANDeviceId(56, new CANBus("mech"));
+        kShooterLeftConfig.config.unitToRotorRatio = 1;
+
+kShooterLeftConfig.config.fxConfig = new TalonFXConfiguration();
+        kShooterLeftConfig.config.fxConfig.OpenLoopRamps = Constants.makeDefaultOpenLoopRampConfig();
+        kShooterLeftConfig.config.fxConfig.CurrentLimits.StatorCurrentLimit = 150;
+                kShooterLeftConfig.config.fxConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+                        kShooterLeftConfig.config.fxConfig.CurrentLimits.SupplyCurrentLimit = 80;
+                                kShooterLeftConfig.config.fxConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+                                        kShooterLeftConfig.config.fxConfig.CurrentLimits.SupplyCurrentLowerLimit = 40;
+                                        kShooterLeftConfig.config.fxConfig.CurrentLimits.SupplyCurrentLowerTime = 1;
+        kShooterLeftConfig.config.fxConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+        kShooterLeftConfig.config.fxConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+        kShooterLeftConfig.config.fxConfig.TorqueCurrent.PeakForwardTorqueCurrent = 150;
+                kShooterLeftConfig.config.fxConfig.TorqueCurrent.PeakReverseTorqueCurrent = 150;
+
+
+
+                kShooterConfig.followers[0] = kShooterLeftConfig;
+
+
+
+    }
+    public static final double kShooterGearRatio = 1;
+    public static final double kShooterDutyCycle = 0.5;
+    public static final double kShooterDutyCycleExhaust = -0.5;
+    public static final DoubleSupplier IDLE_RPS = null; // TODO: find this experimentally
+}

src/main/java/com/team900/frc2026/subsystems/turret/TurretConstants.java

@@ -44,7 +44,7 @@ public class TurretConstants {
     public static final double toleranceRad = 0.1;
     /*
     *
-    *         config.Slot0.kS = 0.18;
+    *      config.Slot0.kS = 0.18;
            config.Slot0.kP = 6.0;
            config.Slot0.kD = 0.1;
            config.Slot0.kV = 0.120;