[UNVERIFIED] Initial Intake subsystem

Author: Bloodhatam

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

@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Intake extends SubsystemBase {
+
+  IntakeIO io;
+  /** Creates a new Intake. */
+  public Intake(IntakeIO io) 
+  {
+    this.io = io;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}

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

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.intake;
+
+public class IntakeConstants {
+
+
+    public static final int SPINNER_CAN_ID = 20;
+    public static final int INTAKE_EXTENDER_CAN_ID = 21;
+
+    public static final double extendedPosition = 1000;
+    public static final double foldedPosition = 0;
+
+    public static final double MotorReduction = 4*4*5;
+    public static final double BeltReduction = 2.0;
+
+    public static final double extenderPID_P = 0.5;
+    public static final double extenderPID_I = 0.0;
+    public static final double extenderPID_D = 0.0;
+    public static final double extenderPID_MINIMUM = -1.0;
+    public static final double extenderPID_MAXIMUM = 1.0;
+
+}

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

@@ -0,0 +1,41 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import com.revrobotics.spark.SparkBase.Faults;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public interface IntakeIO {
+    @AutoLog
+    public static class IntakeIOInputs 
+    {
+        public Rotation2d intakePosition = new Rotation2d();
+        public double intakeSpeed = 0.0;
+
+
+        public double spinnerSpeed =  0.0;
+
+        // intake motors volts and ampers
+        public double spinnerVoltage = 0.0;
+        public double spinnerAmperage = 0.0;
+        public double intakeVoltage = 0.0;
+        public double intakeAmperage = 0.0;
+
+        //faults
+        public Faults spinnerFaults = new Faults(0);
+        public Faults extenderFaults = new Faults(0);
+
+
+    }
+
+    public default void updateInputs(IntakeIOInputs inputs) {}
+
+    public default void setSpinnerSpeed(double speed) {}
+
+    public default void setIntakePosition(double setpoint) {}
+
+    public default void extendIntake() { this.setIntakePosition(IntakeConstants.extendedPosition);}
+
+    public default void foldIntake() { this.setIntakePosition(IntakeConstants.foldedPosition);}
+}

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

@@ -0,0 +1,82 @@
+package frc.robot.subsystems.intake;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.PersistMode;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class IntakeIOSpark implements IntakeIO {
+
+    private SparkMax extenderMotor = new SparkMax(IntakeConstants.INTAKE_EXTENDER_CAN_ID, MotorType.kBrushless);
+    private SparkMax spinnerMotor = new SparkMax(IntakeConstants.SPINNER_CAN_ID, MotorType.kBrushless);
+
+    private AbsoluteEncoder extenderEncoder;
+    SparkClosedLoopController extenderController;
+
+    public IntakeIOSpark() 
+    {
+        SparkMaxConfig extenderConfig = new SparkMaxConfig();
+
+        extenderConfig
+        .openLoopRampRate(0.5)
+        .closedLoopRampRate(0.5)
+        .smartCurrentLimit(30)
+        .idleMode(IdleMode.kBrake);
+
+        extenderConfig.closedLoop
+        .p(IntakeConstants.extenderPID_P)
+        .i(IntakeConstants.extenderPID_I)
+        .d(IntakeConstants.extenderPID_D)
+        .outputRange(IntakeConstants.extenderPID_MINIMUM,
+                     IntakeConstants.extenderPID_MAXIMUM)
+        .feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
+
+        extenderMotor.configure(extenderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        extenderController = extenderMotor.getClosedLoopController();
+        extenderEncoder = extenderMotor.getAbsoluteEncoder();   
+
+
+        SparkMaxConfig spinnerConfig = new SparkMaxConfig();
+        spinnerConfig.inverted(true)
+        .idleMode(IdleMode.kCoast)
+        .smartCurrentLimit(30);
+
+        spinnerMotor.configure(spinnerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    @Override
+    public void updateInputs(IntakeIOInputs inputs) 
+    {
+        inputs.intakeAmperage = extenderMotor.getOutputCurrent();
+        inputs.intakeVoltage = extenderMotor.getAppliedOutput() * extenderMotor.getBusVoltage();
+        inputs.intakePosition = new Rotation2d(extenderEncoder.getPosition());
+        inputs.intakeSpeed = extenderEncoder.getVelocity();
+
+        inputs.spinnerAmperage = spinnerMotor.getOutputCurrent();
+        inputs.spinnerVoltage = spinnerMotor.getAppliedOutput() * spinnerMotor.getBusVoltage();
+        inputs.spinnerSpeed = spinnerMotor.getEncoder().getVelocity();
+        
+        inputs.spinnerFaults = spinnerMotor.getFaults();
+        inputs.extenderFaults = extenderMotor.getFaults();
+    }
+
+    @Override
+    public void setSpinnerSpeed(double speed) 
+    {
+        spinnerMotor.set(speed);
+    }
+    @Override
+    public void setIntakePosition(double setpoint) {
+        extenderController.setSetpoint(setpoint, ControlType.kPosition);
+    }
+    
+}