Climber subsystem

unconfirmed subsytem - incomplete,no motors mapped
2026-06-09 08:48:05 -06:00 · 2026-01-29 18:27:18 -08:00
parent 5d38173168
commit 6c082c7103
5 changed files with 280 additions and 6 deletions
@@ -0,0 +1,79 @@
+package frc4388.robot.subsystems.climber;
+
+import static edu.wpi.first.units.Units.Rotation;
+
+import java.util.function.Supplier;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.utility.status.FaultReporter;
+
+public class Climber extends SubsystemBase {
+    ClimberIO io;
+    ClimberStateAutoLogged state = new ClimberStateAutoLogged();
+
+    Supplier<Pose2d> m_swervePoseSupplier;
+
+    public Climber(
+        ClimberIO io,
+        Supplier<Pose2d> swervePoseSupplier
+    ) {
+        this.io = io;
+        this.m_swervePoseSupplier = swervePoseSupplier;
+    }
+
+    // public enum ClimberMode {
+    //     Up,
+    //     Down,
+    // }
+
+    // public void setMode(IntakeMode mode) {
+    //     switch (mode) {
+    //         case Up:
+    //             io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER);
+    //             io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP);
+    //             break;
+    //         case Down:
+    //             io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER);
+    //             io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY);
+    //             break;
+    //     }
+    // }
+
+
+    // public enum FieldZone {
+    //     // The robot should aim at the hub
+    //     InShootZone,
+    //     // The robot should aim towards the wall
+    //     AimAtWall,
+        
+    // }
+
+    // // Calculate what should be done based off of the position of the robot
+    // // TODO: Implement field zones
+    // public FieldZone getTarget(Pose2d position) {
+    //     return FieldZone.InShootZone;
+    // }
+
+    @Override
+    public void periodic() {
+        
+        
+
+        // FaultReporter.register(this); // TODO Implement fault reporter
+
+
+        Logger.processInputs("Climber", state);
+
+        Pose2d pose = m_swervePoseSupplier.get();
+        Angle robotRot = pose.getRotation().getMeasure();
+
+        io.updateInputs(state);
+
+    }
+}
+
+