Fixed some issues, fleshed out ShooterHood

comp/src/main/java/org/team100/frc2026/ShooterHood.java

@@ -1,5 +1,135 @@
 package org.team100.frc2026;
 
-public class ShooterHood {
-
+import java.util.Optional;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+import org.team100.lib.config.Identity;
+import org.team100.lib.config.PIDConstants;
+import org.team100.lib.controller.r1.PIDFeedback;
+import org.team100.lib.logging.LoggerFactory;
+import org.team100.lib.mechanism.RotaryMechanism;
+import org.team100.lib.motor.MotorPhase;
+import org.team100.lib.motor.NeutralMode100;
+import org.team100.lib.motor.ctre.KrakenX44Motor;
+import org.team100.lib.motor.ctre.KrakenX60Motor;
+import org.team100.lib.motor.sim.SimulatedBareMotor;
+import org.team100.lib.profile.r1.IncrementalProfile;
+import org.team100.lib.profile.r1.TrapezoidIncrementalProfile;
+import org.team100.lib.reference.r1.IncrementalProfileReferenceR1;
+import org.team100.lib.reference.r1.ProfileReferenceR1;
+import org.team100.lib.sensor.position.absolute.sim.SimulatedRotaryPositionSensor;
+import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder;
+import org.team100.lib.sensor.position.incremental.ctre.Talon6Encoder;
+import org.team100.lib.servo.AngularPositionServo;
+import org.team100.lib.servo.OnboardAngularPositionServo;
+import org.team100.lib.servo.OutboardAngularPositionServo;
+import org.team100.lib.state.ModelSE2;
+import org.team100.lib.subsystems.turret.Turret.Solution;
+import org.team100.lib.util.CanId;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+
+public class ShooterHood extends SubsystemBase {
+    private final AngularPositionServo m_servo;
+
+    public ShooterHood(LoggerFactory parent, CanId canID) {
+        LoggerFactory log = parent.type(this);
+
+        switch (Identity.instance) {
+
+            case TEST_BOARD_B0 -> {
+                float gearRatio = 10;
+                PIDConstants PID = PIDConstants.makePositionPID(log, 1);
+                double supplyLimit = 50;
+                double statorLimit = 20;
+                KrakenX44Motor m_motor = new KrakenX44Motor(
+                        log, // LoggerFactor y parent,
+                        canID, // CanId canId,
+                        NeutralMode100.COAST, // NeutralMode neutral,
+                        MotorPhase.REVERSE, // MotorPhase motorPhase,
+                        supplyLimit, // og 50 //double supplyLimit,
+                        statorLimit, // og 2 //double statorLimit,
+                        KrakenX60Motor.highFrictionFF(log), // Feedforward100 ff
+                        KrakenX60Motor.highFriction(log),
+                        PID // PIDConstants pid,
+                );
+                Talon6Encoder encoder = m_motor.encoder();
+
+                TrapezoidIncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05);
+                ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, 0.05, 0.05);
+                double initialPosition = 0;
+                RotaryMechanism climberMech = new RotaryMechanism(
+                        log, m_motor, encoder, initialPosition, gearRatio,
+                        Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
+                m_servo = new OutboardAngularPositionServo(log, climberMech, ref);
+
+            }
+
+            default -> {
+                SimulatedBareMotor m_motor = new SimulatedBareMotor(log, 600);
+
+                IncrementalProfile profile = new TrapezoidIncrementalProfile(log, 1, 2, 0.05);
+                ProfileReferenceR1 ref = new IncrementalProfileReferenceR1(log, () -> profile, 0.05, 0.05);
+                PIDFeedback feedback = new PIDFeedback(log, 5, 0, 0, false, 0.05, 0.1);
+
+                IncrementalBareEncoder encoder = m_motor.encoder();
+                SimulatedRotaryPositionSensor sensor = new SimulatedRotaryPositionSensor(log, encoder, 1);
+
+                RotaryMechanism climberMech = new RotaryMechanism(
+                        log, m_motor, sensor, 1,
+                        Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY);
+
+                m_servo = new OnboardAngularPositionServo(log, climberMech, ref, feedback);
+            }
+        }
+    }
+
+
+        private Optional<Solution> getAbsoluteBearingInstantaneous() {
+        ModelSE2 state = m_state.get();
+        Translation2d target = m_target.get();
+        return Optional.of(
+                new Solution(
+                        target.minus(state.translation()).getAngle(),
+                        Rotation2d.kZero));
+    }
+
+    @Override
+    public void periodic() {
+        m_servo.periodic();
+    }
+
+    public Command gotoAngle(DoubleSupplier trigger) {
+        return new FunctionalCommand(
+                () -> reset(), // onInit
+                () -> setAngle(trigger.getAsDouble()), // onExecute
+                interrupted -> { // onEnd
+                },
+                () -> m_servo.atGoal(), // isFinished
+                this).withName("Shooter Hood Angler");
+    }
+
+    public Command stop() {
+        return run(this::stopServo).withName("Shooter Hood Stop");
+    }
+
+    public void stopServo() {
+        m_servo.stop();
+    }
+
+    private void reset() {
+        m_servo.reset();
+    }
+
+    /** Use a profile to set the position. */
+    private void setAngle(double value) {
+        m_servo.setPositionProfiled(value, 0);
+    }
+
 }

comp/src/main/java/org/team100/frc2026/ShooterTable.java

@@ -0,0 +1,32 @@
+package org.team100.frc2026;
+
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+
+/** Interpolates gun elevation in radians, given range in meters. */
+public class ShooterTable {
+
+    public static final ShooterTable instance = new ShooterTable();
+
+    private final InterpolatingDoubleTreeMap m_table;
+
+    public ShooterTable() {
+        m_table = new InterpolatingDoubleTreeMap();
+        loadTable();
+    }
+
+    public double getAngleRad(double rangeM) {
+        return m_table.get(rangeM);
+    }
+
+    public void loadTable() {
+        m_table.put(1.49, 0.9);
+        m_table.put(2.07, 0.78);
+        m_table.put(2.5, 0.66);
+        m_table.put(3.02, 0.59);
+        m_table.put(3.59, 0.53);
+        m_table.put(4.1, 0.475);
+        m_table.put(4.5, 0.44);
+
+    }
+
+}

comp/src/main/java/org/team100/frc2026/robot/Binder.java

@@ -120,6 +120,8 @@ public void bind() {
                 m_machinery.m_intake.stop());
         m_machinery.m_extender.setDefaultCommand(
                 m_machinery.m_extender.stop());
+        m_machinery.m_shooterHood.setDefaultCommand(
+                m_machinery.m_shooterHood.stop());
 
         ///////////////////////////
         //
@@ -138,6 +140,7 @@ public void bind() {
         ///
         /// SUBSYSTEMS
         ///
+
         whileTrue(driver::b, m_machinery.m_shooter.shoot());
 
         whileTrue(driver::x, m_machinery.m_intake.intake());

comp/src/main/java/org/team100/frc2026/robot/Machinery.java

@@ -10,6 +10,7 @@
 import org.team100.frc2026.IntakeExtend;
 import org.team100.frc2026.Serializer;
 import org.team100.frc2026.Shooter;
+import org.team100.frc2026.ShooterHood;
 import org.team100.lib.coherence.Takt;
 import org.team100.lib.indicator.Beeper;
 import org.team100.lib.localization.AprilTagFieldLayoutWithCorrectOrientation;
@@ -68,14 +69,15 @@ public class Machinery {
     public final TrajectoryVisualization m_trajectoryViz;
     public final SwerveKinodynamics m_swerveKinodynamics;
     final AprilTagRobotLocalizer m_localizer;
-    public final SwerveDriveSubsystem m_drive;
+    public final static SwerveDriveSubsystem m_drive;
     final Beeper m_beeper;
     final Shooter m_shooter;
     final Intake m_intake;
     final IntakeExtend m_extender;
     final Serializer m_serializer;
     final ClimberExtension m_ClimberExtension;
     final Climber m_Climber;
+    final ShooterHood m_shooterHood;
 
     public Machinery() {
 
@@ -94,6 +96,7 @@ public Machinery() {
         m_serializer = new Serializer(driveLog);
         m_ClimberExtension = new ClimberExtension(driveLog);
         m_Climber = new Climber(driveLog);
+        m_shooterHood = new ShooterHood(driveLog, null);
 
         ////////////////////////////////////////////////////////////
         //