Merge branch 'intake' into pathplanner

# Conflicts:
#	.gitignore
#	build.gradle
#	src/main/java/frc/robot/BuildConstants.java
#	src/main/java/frc/robot/RobotContainer.java

src/main/java/frc/robot/RobotContainer.java

@@ -19,6 +19,9 @@
 import frc.robot.subsystems.drive.module.ModuleIOSim;
 import frc.robot.subsystems.drive.module.ModuleIOSparkMax;
 import frc.robot.subsystems.vision.*;
+import frc.robot.subsystems.intake.IntakeIO;
+import frc.robot.subsystems.intake.IntakeIOProto;
+import frc.robot.subsystems.intake.IntakeSubsystem;
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
 import org.littletonrobotics.junction.Logger;
@@ -29,16 +32,20 @@ public class RobotContainer {
   private final CommandXboxController driveController = new CommandXboxController(0);
 
   DriveSubsystem driveSubsystem;
+  private final IntakeSubsystem intakeSubsystem;
+
   public RobotContainer() {
     switch (Constants.getMode()) {
-      case REAL ->
+      case REAL -> {
         driveSubsystem = new DriveSubsystem(
             new GyroIOPigeon2(),
             new ModuleIOSparkMax(DriveConstants.MODULE_CONSTANTS[0]),
             new ModuleIOSparkMax(DriveConstants.MODULE_CONSTANTS[1]),
             new ModuleIOSparkMax(DriveConstants.MODULE_CONSTANTS[2]),
             new ModuleIOSparkMax(DriveConstants.MODULE_CONSTANTS[3])
         );
+        intakeSubsystem = new IntakeSubsystem(new IntakeIOProto());
+      }
 
       case SIM -> {
         driveSimulation = new SwerveDriveSimulation(DriveConstants.MAPLE_SIM_CONFIG, new Pose2d(3, 3, new Rotation2d()));
@@ -64,8 +71,10 @@ public RobotContainer() {
         VisionEnvironmentSimulator.getInstance().addRobotPoseSupplier(RobotState.getInstance()::getEstimatedPose);
 
         SimulatedArena.getInstance().resetFieldForAuto();
+
+        intakeSubsystem = new IntakeSubsystem(new IntakeIO() {});
       }
-      case REPLAY ->
+      case REPLAY -> {
         driveSubsystem = new DriveSubsystem(
             new GyroIO() {
             },
@@ -74,6 +83,11 @@ public RobotContainer() {
             new ModuleIO() {},
             new ModuleIO() {}
         );
+        intakeSubsystem = new IntakeSubsystem(new IntakeIO() {});
+      }
+      default -> {
+        intakeSubsystem = new IntakeSubsystem(new IntakeIO() {});
+      }
     }
     configureBindings();
   }
@@ -92,6 +106,17 @@ private void configureBindings() {
               .setSimulationWorldPose(
                   RobotState.getInstance().getEstimatedPose())));
     }
+
+    driveController.a().whileTrue(
+        Commands.run(() -> intakeSubsystem.setIntakePower(6.0), intakeSubsystem)
+    ).whileFalse(
+        Commands.run(() -> intakeSubsystem.setIntakePower(0.0))
+    );
+    driveController.b().whileTrue(
+        Commands.run(() -> intakeSubsystem.setIntakePower(-6.0), intakeSubsystem)
+    ).whileFalse(
+        Commands.run(() -> intakeSubsystem.setIntakePower(0.0))
+    );
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/subsystems/climber/ClimberConstants.java

@@ -0,0 +1,9 @@
+package frc.robot.subsystems.climber;
+
+public class ClimberConstants {
+    public static final int CLIMBER_ID = 0;
+    public static final double CLIMBER_KP = 0.0;
+    public static final double CLIMBER_KI = 0.0;
+    public static final double CLIMBER_KD = 0.0;
+    public static final double CLIMBER_OFFSET = 0.0;
+}

src/main/java/frc/robot/subsystems/climber/ClimberIO.java

@@ -0,0 +1,18 @@
+package frc.robot.subsystems.climber;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ClimberIO {
+    @AutoLog
+    class ClimberIOInputs{
+        public double position = 0.0;
+        public double currentDraw = 0.0;
+        public double setpoint = 0.0;
+        public double appliedOutput = 0.0;
+    }
+    default void setMotorVoltage(double voltage) {}
+    default void setPosititon(double degrees) {}
+    default void updateInputs(ClimberIOInputsAutoLogged inputs) {}
+    default void stop() {}
+    default void resetPosition() {}
+}

src/main/java/frc/robot/subsystems/climber/ClimberIOKraken.java

@@ -0,0 +1,85 @@
+package frc.robot.subsystems.climber;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.PositionVoltage;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Current;
+
+public class ClimberIOKraken implements ClimberIO{
+    private final TalonFX climber;
+    private final PositionVoltage posRequest;
+    private final StatusSignal<Angle> positionSignal;
+    private final StatusSignal<Current> currentDrawSignal;
+    private final StatusSignal<Double> setpointSignal;
+    private final StatusSignal<Double> appliedOutputSignal;
+    private final NeutralOut stopRequest;
+    public ClimberIOKraken() {
+        climber = new TalonFX(ClimberConstants.CLIMBER_ID);
+        TalonFXConfiguration climberConfig = new TalonFXConfiguration();
+        climberConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+        climberConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+        climber.getConfigurator().apply(climberConfig);
+
+        var climberConfigs = new Slot0Configs();
+        climberConfigs.kP = 2.4;
+        climberConfigs.kI = 0;
+        climberConfigs.kD = 0.1;
+        climber.getConfigurator().apply(climberConfigs);
+        final PositionVoltage climberRequest = new PositionVoltage(0).withSlot(0);
+        climber.setControl(climberRequest.withPosition(10));
+
+        positionSignal = climber.getPosition();
+        currentDrawSignal = climber.getSupplyCurrent();
+        setpointSignal = climber.getClosedLoopReference();
+        appliedOutputSignal = climber.getDutyCycle();
+
+        posRequest = new PositionVoltage(0).withSlot(0)
+                .withEnableFOC(climber.getIsProLicensed().getValue());
+        stopRequest = new NeutralOut();
+
+        BaseStatusSignal.setUpdateFrequencyForAll(50.0,
+                positionSignal,
+                currentDrawSignal,
+                setpointSignal,
+                appliedOutputSignal);
+
+        climber.optimizeBusUtilization();
+    }
+    @Override
+    public void updateInputs(ClimberIOInputsAutoLogged inputs) {
+        BaseStatusSignal.refreshAll(
+                positionSignal,
+                currentDrawSignal,
+                setpointSignal,
+                appliedOutputSignal
+        );
+        inputs.position = positionSignal.refresh().getValueAsDouble();
+        inputs.currentDraw = currentDrawSignal.refresh().getValueAsDouble();
+        inputs.setpoint = setpointSignal.refresh().getValueAsDouble();
+        inputs.appliedOutput = appliedOutputSignal.refresh().getValueAsDouble();
+
+    }
+    @Override
+    public void setMotorVoltage(double voltage) {
+        climber.setVoltage(voltage);
+    }
+    @Override
+    public void setPosititon(double degrees) {
+        climber.setControl(posRequest.withPosition(degrees / 360));
+    }
+    @Override
+    public void resetPosition() {
+        climber.setPosition(0);
+    }
+    @Override
+    public void stop() {
+        climber.setControl(stopRequest);
+    }
+}

src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java

@@ -0,0 +1,44 @@
+package frc.robot.subsystems.climber;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class ClimberSubsystem extends SubsystemBase {
+    private final ClimberIO io;
+    private final ClimberIOInputsAutoLogged inputs;
+    private boolean climberLock = false;
+    public ClimberSubsystem(ClimberIO io) {
+        this.io = io;
+        inputs = new ClimberIOInputsAutoLogged();
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Climber", inputs);
+    }
+    public void setClimberPower(double power) {
+        io.setMotorVoltage(power);
+    }
+    public boolean getClimberLock() {
+        return climberLock;
+    }
+    public Command resetClimberLock() {
+        return runOnce(() -> climberLock = false).ignoringDisable(true);
+    }
+    public Command setClimberPowerFactory(double power) {
+        climberLock = true;
+        return run(() -> setClimberPower(power));
+    }
+    public Command setClimberPosition(double degrees) {
+        climberLock = true;
+        return runEnd(() -> {
+            io.setPosititon(degrees);
+        },
+                () -> setClimberPower(0.0));
+    }
+    public Command resetClimber() {
+        return runOnce(io::resetPosition);
+    }
+}

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -84,7 +84,7 @@ public class DriveConstants {
                 .driveId(4)
                 .steerId(5)
                 .encoderId(6)
-                .encoderOffset(Rotation2d.fromDegrees(-101.60).unaryMinus())
+                .encoderOffset(Rotation2d.fromDegrees(-101.60).unaryMinus().plus(Rotation2d.k180deg))
                 .steerInverted(true)
                 .turnInverted(false)
                 .build(),
@@ -102,7 +102,7 @@ public class DriveConstants {
                 .driveId(10)
                 .steerId(11)
                 .encoderId(12)
-                .encoderOffset(Rotation2d.fromDegrees(60.20).unaryMinus())
+                .encoderOffset(Rotation2d.fromDegrees(60.20).unaryMinus().plus(Rotation2d.k180deg))
                 .steerInverted(true)
                 .turnInverted(false)
                 .build()

src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java

@@ -26,5 +26,4 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) {
 
   }
 
-
 }

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -0,0 +1,18 @@
+package frc.robot.subsystems.intake;
+
+public class IntakeConstants {
+    public static final int INTAKE_ID = 0;
+    public static final int PIVOT_ID = 0;
+    public static final double INTAKE_KP = 0.0;
+    public static final double INTAKE_KI = 0.0;
+    public static final double INTAKE_KD = 0.0;
+    public static final double PIVOT_KP = 0.0;
+    public static final double PIVOT_KI = 0.0;
+    public static final double PIVOT_KD = 0.0;
+    public static final double INTAKE_OFFSET = 0.0;
+    public static final double PIVOT_OFFSET = 0.0;
+    public static final double LOOP_PERIOD_SECS = 10.0;
+    public static final double INTAKE_GEAR_RATIO = 10.0;
+    public static final double PIVOT_GEAR_RATIO = 10.0;
+
+}

src/main/java/frc/robot/subsystems/intake/IntakeIO.java

@@ -0,0 +1,24 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+    @AutoLog
+    class IntakeIOInputs {
+        //position, voltage, current draw, and any other important logging data
+        public Rotation2d pivotPosititon = new Rotation2d();
+        public double pivotVoltage = 0.0;
+        public double intakeVoltage = 0.0;
+        public double pivotCurrentDraw = 0.0;
+        public double intakeCurrentDraw = 0.0;
+        public double pivotAppliedVolts = 0.0;
+        public double intakeAppliedVolts = 0.0;
+        public double pivotTemperature = 0.0;
+        public double intakeTemperature = 0.0;
+    }
+    default void setMotorVoltageIntake(double voltage) {}
+    default void setMotorVoltagePivot(double voltage) {}
+    default void updateInputs(IntakeIOInputsAutoLogged inputs) {}
+    default void stop() {}
+}

src/main/java/frc/robot/subsystems/intake/IntakeIOProto.java

@@ -0,0 +1,29 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+public class IntakeIOProto implements IntakeIO {
+  private final TalonFX intake;
+
+  public IntakeIOProto() {
+    intake = new TalonFX(17);
+
+    var config = new TalonFXConfiguration();
+    config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    intake.getConfigurator().apply(config);
+  }
+
+  @Override
+  public void updateInputs(IntakeIOInputsAutoLogged inputs) {
+    inputs.intakeAppliedVolts = intake.getMotorVoltage().getValueAsDouble();
+    inputs.intakeCurrentDraw = intake.getSupplyCurrent().getValueAsDouble();
+  }
+
+  @Override
+  public void setMotorVoltageIntake(double voltage) {
+    intake.setVoltage(voltage);
+  }
+}

src/main/java/frc/robot/subsystems/intake/IntakeIOSimulation.java

@@ -0,0 +1,48 @@
+package frc.robot.subsystems.intake;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.sim.TalonFXSimState;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+public class IntakeIOSimulation implements IntakeIO {
+    private final TalonFX intake = new TalonFX(IntakeConstants.INTAKE_ID);
+    private final TalonFX pivot = new TalonFX(IntakeConstants.PIVOT_ID);
+    private final TalonFXSimState intakeSim = intake.getSimState();
+    private final TalonFXSimState pivotSim = pivot.getSimState();
+    private final DCMotorSim intakeMotorSim = new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(
+                    DCMotor.getKrakenX60Foc(1), 0.001, IntakeConstants.INTAKE_GEAR_RATIO
+            ),
+            DCMotor.getKrakenX60Foc(1)
+    );
+    private final DCMotorSim pivotMotorSim = new DCMotorSim(
+            LinearSystemId.createDCMotorSystem(
+                    DCMotor.getKrakenX60Foc(1), 0.001, IntakeConstants.PIVOT_GEAR_RATIO
+            ),
+            DCMotor.getKrakenX60Foc(1)
+    );
+
+    @Override
+    public void updateInputs(IntakeIOInputsAutoLogged inputs) {
+        // update simulation
+        intakeMotorSim.update(IntakeConstants.LOOP_PERIOD_SECS);
+        pivotMotorSim.update(IntakeConstants.LOOP_PERIOD_SECS);
+
+        // update inputs
+        intakeSim.setSupplyVoltage(intake.getMotorVoltage().getValueAsDouble());
+        pivotSim.setSupplyVoltage(pivot.getMotorVoltage().getValueAsDouble());
+        intakeMotorSim.setInputVoltage(intakeSim.getMotorVoltage());
+        pivotMotorSim.setInputVoltage(pivotSim.getMotorVoltage());
+
+        //inputs.intakeVoltage = in
+        inputs.pivotPosititon = Rotation2d.fromRadians(pivot.getPosition().refresh().getValueAsDouble());
+        inputs.intakeVoltage = intake.getMotorVoltage().refresh().getValueAsDouble();
+        inputs.pivotVoltage = pivot.getMotorVoltage().refresh().getValueAsDouble();
+        inputs.intakeCurrentDraw = intake.getSupplyCurrent().refresh().getValueAsDouble();
+        inputs.pivotCurrentDraw = pivot.getSupplyCurrent().refresh().getValueAsDouble();
+        inputs.intakeTemperature = intake.getDeviceTemp().refresh().getValueAsDouble();
+        inputs.pivotTemperature = pivot.getDeviceTemp().refresh().getValueAsDouble();
+    }
+}