added intake and intake sim

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

@@ -17,6 +17,10 @@
 import frc.robot.subsystems.battery.BatteryIORio;
 import frc.robot.subsystems.battery.BatteryIOSim;
 import frc.robot.subsystems.drive.*;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIO;
+import frc.robot.subsystems.intake.IntakeIOSim;
+import frc.robot.subsystems.intake.IntakeIOSparkMax;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -34,6 +38,7 @@
 public class RobotContainer {
     // Subsystems
     private final Drive drive;
+    private final Intake intake;
 //    private final Flywheel flywheel;
 
     // Controller
@@ -76,6 +81,7 @@ public RobotContainer() throws IOException {
                         new ModuleIOFalcon500(BRTurnMotorId, BRDriveMotorId, false, false, BRTurnEncoderId),
                         new GyroIOReal(gyroID, "rio"),
                         new VisionIOPhoton());
+                intake = new Intake(new IntakeIOSparkMax());
 //                flywheel = new Flywheel(new FlywheelIOSparkMax());
                 break;
 
@@ -94,6 +100,7 @@ public RobotContainer() throws IOException {
                         gyroSim,
                         new VisionIO() {
                         });
+                intake = new Intake(new IntakeIOSim());
 //                flywheel = new Flywheel(new FlywheelIOSim());
                 break;
 
@@ -108,6 +115,8 @@ public RobotContainer() throws IOException {
                 }, new GyroIO() {
                 }, new VisionIO() {
                 });
+                intake = new Intake(new IntakeIO() {
+                });
 //                flywheel = new Flywheel(new FlywheelIO() {
 //                });
                 break;

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

@@ -1,11 +1,71 @@
 package frc.robot.subsystems.intake;
 
-import edu.wpi.first.math.controller.SimpleMotorFeedforward;
-import edu.wpi.first.math.util.Units;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
+import org.littletonrobotics.junction.Logger;
 
-public class Intake extends SubsystemBase{
+import static edu.wpi.first.math.MathUtil.clamp;
+import static java.lang.Math.PI;
 
+public class Intake extends SubsystemBase {
+    IntakeIO intakeIO;
+    IntakeIOInputsAutoLogged inputs;
 
+    ProfiledPIDController deployPID = new ProfiledPIDController(
+            4.0,
+            0.0,
+            0.05,
+            new TrapezoidProfile.Constraints(
+                    12.0,
+                    18.0
+            )
+    );
+
+    ProfiledPIDController modePID = new ProfiledPIDController(
+            3.05,
+            0.0,
+            0.035,
+            new TrapezoidProfile.Constraints(
+                    16.0,
+                    32.0
+            )
+    );
+    double modeVoltage = 0.0;
+    boolean modeZeroed = false;
+    public Intake(IntakeIO intakeIO) {
+        this.intakeIO = intakeIO;
+        inputs = new IntakeIOInputsAutoLogged();
+
+        deployPID.setTolerance(
+                0.0,
+                0.0
+        );
+        deployPID.enableContinuousInput(0.0, PI * 2);
+
+        modePID.setTolerance(
+                0.0,
+                0.0
+        );
+    }
+
+    @Override
+    public void periodic() {
+        super.periodic();
+        this.intakeIO.updateInputs(inputs);
+        Logger.getInstance().processInputs("Intake", inputs);
+
+        var deployVoltage = deployPID.calculate(
+                inputs.deployEncoderPosition,
+                0.0
+        );
+
+        var modeV = (modeZeroed) ? modePID.calculate(
+                inputs.modeEncoderPosition,
+                0.0
+        ) : modeVoltage;
+
+        intakeIO.setDeployVoltage(clamp(deployVoltage, -6.0, 6.0));
+        intakeIO.setModeVoltage(clamp(modeV, -3.0, 3.0));
+    }
 }

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

@@ -1,4 +1,35 @@
 package frc.robot.subsystems.intake;
 
+import org.littletonrobotics.junction.AutoLog;
+
 public interface IntakeIO {
+    default void updateInputs(IntakeIOInputs inputs) {
+    }
+
+    default void setModeEncoderPosition(double position) {
+    }
+
+    default void setModeVoltage(double voltage) {
+    }
+
+    default void setIntakePercentage(double percentage) {
+    }
+
+    default void setIntakeVoltage(double voltage) {
+    }
+
+    default void setDeployVoltage(double voltage) {
+    }
+
+    @AutoLog
+    class IntakeIOInputs {
+        double modeMotorOutputCurrent;
+        double modeEncoderPosition;
+
+        double intakeMotorOutputCurrent;
+        double intakeMotorSpeed;
+
+        double deployMotorOutputCurrent;
+        double deployEncoderPosition;
+    }
 }

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

@@ -1,4 +1,60 @@
 package frc.robot.subsystems.intake;
 
-public class IntakeIOSim {
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+import frc.robot.Constants;
+
+import static java.lang.Math.PI;
+
+public class IntakeIOSim implements IntakeIO {
+    SingleJointedArmSim deploySim = new SingleJointedArmSim(DCMotor.getNEO(1),
+            1 / 20.0,
+            5.0,
+            .5,
+            -PI / 4,
+            PI / 2,
+            true);
+
+    SingleJointedArmSim modeSim = new SingleJointedArmSim(DCMotor.getNeo550(1),
+            1 / 50.0,
+            5.0,
+            .5,
+            -PI / 2,
+            3 * PI / 4,
+            true);
+
+    @Override
+    public void updateInputs(IntakeIOInputs inputs) {
+        deploySim.update(Constants.loopPeriodSecs);
+        modeSim.update(Constants.loopPeriodSecs);
+        inputs.modeMotorOutputCurrent = modeSim.getCurrentDrawAmps();
+        inputs.deployMotorOutputCurrent = deploySim.getCurrentDrawAmps();
+        inputs.modeEncoderPosition = modeSim.getAngleRads();
+        inputs.deployEncoderPosition = deploySim.getAngleRads();
+    }
+
+    @Override
+    public void setModeEncoderPosition(double position) {
+        IntakeIO.super.setModeEncoderPosition(position);
+    }
+
+    @Override
+    public void setIntakeVoltage(double voltage) {
+        IntakeIO.super.setIntakeVoltage(voltage);
+    }
+
+    @Override
+    public void setModeVoltage(double voltage) {
+        modeSim.setInputVoltage(voltage);
+    }
+
+    @Override
+    public void setDeployVoltage(double voltage) {
+        deploySim.setInputVoltage(voltage);
+    }
+
+    @Override
+    public void setIntakePercentage(double percentage) {
+        IntakeIO.super.setIntakePercentage(percentage);
+    }
 }

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

@@ -1,4 +1,91 @@
 package frc.robot.subsystems.intake;
 
-public class IntakeIOSparkMax {
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkMaxAbsoluteEncoder;
+
+import static com.revrobotics.CANSparkMax.IdleMode.kBrake;
+import static com.revrobotics.CANSparkMaxLowLevel.MotorType.kBrushless;
+import static com.revrobotics.SparkMaxAbsoluteEncoder.Type.kDutyCycle;
+
+public class IntakeIOSparkMax implements IntakeIO {
+    private final CANSparkMax intakeMotor;
+
+    private final CANSparkMax modeMotor;
+    private final RelativeEncoder modeEncoder;
+
+    private final CANSparkMax deployMotor;
+    private final SparkMaxAbsoluteEncoder deployEncoder;
+
+    public IntakeIOSparkMax() {
+        // —— Intake ——
+        intakeMotor = new CANSparkMax(
+                48,
+                kBrushless
+        );
+        intakeMotor.setSmartCurrentLimit(0); // add current limit to limit the torque
+        intakeMotor.setIdleMode(kBrake);
+
+        // —— Mode ——
+        modeMotor = new CANSparkMax(
+                50,
+                kBrushless
+        );
+        modeMotor.setSmartCurrentLimit(20); // add current limit to limit the torque
+        modeMotor.setIdleMode(kBrake);
+
+        modeEncoder = modeMotor.getEncoder();
+        modeEncoder.setPositionConversionFactor(1.0 / 20.0);
+
+        // —— Deploy ——
+        deployMotor = new CANSparkMax(
+                49,
+                kBrushless
+        );
+        deployMotor.setSmartCurrentLimit(0); // add current limit to limit the torque
+        deployMotor.setIdleMode(kBrake);
+
+        deployEncoder = deployMotor.getAbsoluteEncoder(
+                kDutyCycle
+        );
+        deployEncoder.setInverted(true);
+        deployEncoder.setZeroOffset(0.1);
+    }
+
+    @Override
+    public void updateInputs(IntakeIOInputs inputs) {
+        inputs.deployEncoderPosition = deployEncoder.getPosition();
+        inputs.deployMotorOutputCurrent = deployMotor.getOutputCurrent();
+
+        inputs.intakeMotorSpeed = intakeMotor.getEncoder().getVelocity();
+        inputs.intakeMotorOutputCurrent = intakeMotor.getOutputCurrent();
+
+        inputs.modeEncoderPosition = modeEncoder.getPosition();
+        inputs.modeMotorOutputCurrent = modeMotor.getOutputCurrent();
+    }
+
+    @Override
+    public void setDeployVoltage(double voltage) {
+        deployMotor.setVoltage(voltage);
+    }
+
+    @Override
+    public void setIntakePercentage(double percentage) {
+        intakeMotor.set(percentage);
+    }
+
+    @Override
+    public void setIntakeVoltage(double voltage) {
+        intakeMotor.setVoltage(voltage);
+    }
+
+    @Override
+    public void setModeEncoderPosition(double position) {
+        modeEncoder.setPosition(position);
+    }
+
+    @Override
+    public void setModeVoltage(double voltage) {
+        modeMotor.setVoltage(voltage);
+    }
 }