Feature/intake

src/main/deploy/elastic-layout.json

@@ -1826,6 +1826,92 @@
           }
         ]
       }
+    },
+    {
+      "name": "Debug Hopper",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Hopper Speed",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/PARTs/Hopper/Hopper Speed",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Debug Intake",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Intake State",
+            "x": 576.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/PARTs/Intake/Intake State",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Pivot Position",
+            "x": 576.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/PARTs/Intake/Pivot Position",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Pivot Speed",
+            "x": 448.0,
+            "y": 128.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/PARTs/Intake/Pivot Speed",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Intake Speed",
+            "x": 448.0,
+            "y": 0.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/PARTs/Intake/Intake Speed",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": false
+            }
+          }
+        ]
+      }
     }
   ]
 }

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

@@ -42,6 +42,9 @@
 import frc.robot.subsystems.Hopper.Hopper;
 import frc.robot.subsystems.Hopper.HopperPhys;
 import frc.robot.subsystems.Hopper.HopperSim;
+import frc.robot.subsystems.Intake.Intake;
+import frc.robot.subsystems.Intake.IntakePhys;
+import frc.robot.subsystems.Intake.IntakeSim;
 import frc.robot.subsystems.Kicker.Kicker;
 import frc.robot.subsystems.Kicker.KickerPhys;
 import frc.robot.subsystems.Kicker.KickerSim;
@@ -95,10 +98,12 @@ public class RobotContainer {
     private final Kicker kicker = Robot.isReal() ? new KickerPhys() : new KickerSim();
 
     private final Hopper hopper = Robot.isReal() ? new HopperPhys() : new HopperSim();
+
+    private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim();
     // private final ShooterSysid shooter = new ShooterSysid(); //for sysid
 
     private final ArrayList<IPARTsSubsystem> subsystems = new ArrayList<IPARTsSubsystem>(
-            Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper));
+            Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake));
 
     //endregion End Subsystems
 

src/main/java/frc/robot/constants/IntakeConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class IntakeConstants {
+    public static final int INTAKE_MOTOR_ID = 36;
+    public static final int PIVOT_MOTOR_ID = 38;
+}

src/main/java/frc/robot/states/IntakeState.java

@@ -0,0 +1,18 @@
+package frc.robot.states;
+
+public enum IntakeState {
+    IDLE(0),
+    DISABLED(0),
+    INTAKING(0),
+    OUTTAKING(0);
+    private double speed;
+
+    private IntakeState(double speed){
+        this.speed = speed;
+    }
+
+    public double getSpeed(){
+        return speed;
+    }
+}
+

src/main/java/frc/robot/states/PivotState.java

@@ -0,0 +1,16 @@
+package frc.robot.states;
+
+public enum PivotState {
+    HOME(0),
+    INTAKE(0);
+
+    private double position;
+
+    private PivotState(double position){
+        this.position = position;
+    }
+
+    public double getPosition(){
+        return position;
+    }
+}

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

@@ -0,0 +1,64 @@
+package frc.robot.subsystems.Intake;
+
+import org.parts3492.partslib.PARTsUnit;
+import org.parts3492.partslib.PARTsUnit.PARTsUnitType;
+import org.parts3492.partslib.command.PARTsSubsystem;
+
+import frc.robot.constants.RobotConstants;
+import frc.robot.states.IntakeState;
+
+public abstract class Intake extends PARTsSubsystem {
+
+    IntakeState state = IntakeState.IDLE;
+
+    public Intake() {
+        super("Intake");
+
+        if (RobotConstants.DEBUGGING) {
+            partsNT.putDouble("Intake Speed", 0);
+            partsNT.putDouble("Pivot Speed", 0);
+        }
+    }
+
+    @Override
+    public void outputTelemetry() {
+        partsNT.putDouble("Pivot Position", getPivotPosition().to(PARTsUnitType.Angle));
+        partsNT.putDouble("Current Intake Speed", getIntakeSpeed());
+        partsNT.putString("Intake State", state.toString());
+    }
+
+    @Override
+    public void stop() {
+        state = IntakeState.DISABLED;
+    }
+
+    @Override
+    public void reset() {
+        state = IntakeState.IDLE;
+    }
+
+    @Override
+    public void periodic() {
+        if (RobotConstants.DEBUGGING) {
+            setIntakeSpeed(partsNT.getDouble("Intake Speed"));
+            setPivotSpeed(partsNT.getDouble("Pivot Speed"));
+        }
+    }
+
+    @Override
+    public void log() {
+        partsLogger.logDouble("Pivot Position", getPivotPosition().to(PARTsUnitType.Angle));
+        partsLogger.logDouble("Intake Speed", getIntakeSpeed());
+        partsLogger.logString("Intake State", state.toString());
+    }
+
+    public abstract void setIntakeSpeed(double speed);
+
+    public abstract void setPivotSpeed(double speed);
+
+    public abstract void setPivotPosition(PARTsUnit position);
+
+    public abstract double getIntakeSpeed();
+
+    public abstract PARTsUnit getPivotPosition();
+}

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

@@ -0,0 +1,86 @@
+package frc.robot.subsystems.Intake;
+
+import static edu.wpi.first.units.Units.Rotations;
+
+import org.parts3492.partslib.PARTsUnit;
+import org.parts3492.partslib.PARTsUnit.PARTsUnitType;
+import org.parts3492.partslib.network.PARTsNT;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import edu.wpi.first.wpilibj.motorcontrol.Talon;
+
+import frc.robot.constants.IntakeConstants;
+
+public class IntakePhys extends Intake {
+    TalonFX intakeMotor;
+    TalonFX pivotMotor;
+
+    public IntakePhys() {
+        super();
+        TalonFXConfiguration intakeConfig = new TalonFXConfiguration();
+        intakeMotor = new TalonFX(IntakeConstants.INTAKE_MOTOR_ID);
+        intakeConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+        intakeMotor.getConfigurator().apply(intakeConfig);
+        pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID);
+    }
+
+    @Override
+    public void setIntakeSpeed(double speed) {
+        intakeMotor.set(speed);
+    }
+
+    @Override
+    public void setPivotPosition(PARTsUnit position) {
+        pivotMotor.setPosition(position.to(PARTsUnitType.Rotations));
+    }
+
+    @Override
+    public double getIntakeSpeed() {
+        return intakeMotor.get();
+    }
+
+    @Override
+    public PARTsUnit getPivotPosition() {
+        return new PARTsUnit(pivotMotor.getPosition().getValueAsDouble(), PARTsUnitType.Rotations);
+    }
+
+    @Override
+    public void outputTelemetry() {
+        super.outputTelemetry();
+        partsNT.putDouble("Pivot/Output", pivotMotor.getMotorOutputStatus().getValueAsDouble());
+        partsNT.putDouble("Pivot/Current", pivotMotor.getStatorCurrent().getValueAsDouble());
+
+        partsNT.putDouble("Intake/Output", intakeMotor.getMotorOutputStatus().getValueAsDouble());
+        partsNT.putDouble("Intake/Current", intakeMotor.getStatorCurrent().getValueAsDouble());
+    }
+
+    @Override
+    public void stop() {
+        super.stop();
+        intakeMotor.set(0);
+        pivotMotor.set(0);
+    }
+
+    @Override
+    public void reset() {
+        super.reset();
+    }
+
+    @Override
+    public void log() {
+        super.log();
+        partsLogger.logDouble("Pivot/Output", pivotMotor.getMotorOutputStatus().getValueAsDouble());
+        partsLogger.logDouble("Pivot/Current", pivotMotor.getStatorCurrent().getValueAsDouble());
+
+        partsLogger.logDouble("Intake/Output", intakeMotor.getMotorOutputStatus().getValueAsDouble());
+        partsLogger.logDouble("Intake/Current", intakeMotor.getStatorCurrent().getValueAsDouble());
+    }
+
+    @Override
+    public void setPivotSpeed(double speed) {
+        pivotMotor.set(speed);
+    }
+}

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

@@ -0,0 +1,30 @@
+package frc.robot.subsystems.Intake;
+
+import org.parts3492.partslib.PARTsUnit;
+import org.parts3492.partslib.PARTsUnit.PARTsUnitType;
+
+public class IntakeSim extends Intake {
+
+    @Override
+    public void setIntakeSpeed(double speed) {
+    }
+
+    @Override
+    public void setPivotSpeed(double speed) {
+    }
+
+    @Override
+    public void setPivotPosition(PARTsUnit position) {
+    }
+
+    @Override
+    public double getIntakeSpeed() {
+        return 0;
+    }
+
+    @Override
+    public PARTsUnit getPivotPosition() {
+        return new PARTsUnit(0, PARTsUnitType.Angle);
+    }
+
+}

src/main/java/frc/robot/subsystems/Kicker/Kicker.java

@@ -47,22 +47,21 @@ public void log() {
     public void periodic() {
         if (RobotConstants.DEBUGGING) {
             setSpeed(partsNT.getDouble("Kicker Speed"));
-        }
-        else {
+        } else {
             switch (kickerState) {
-            case ROLLING:
-                setSpeed(kickerState.getSpeed());
-                break;
-            case DISABLED:
-                setSpeed(kickerState.getSpeed());
-                break;
-            case IDLE:
-                setSpeed(kickerState.getSpeed());
-                break;
-            default:
-                setSpeed(0);
-                break;
-        }
+                case ROLLING:
+                    setSpeed(kickerState.getSpeed());
+                    break;
+                case DISABLED:
+                    setSpeed(kickerState.getSpeed());
+                    break;
+                case IDLE:
+                    setSpeed(kickerState.getSpeed());
+                    break;
+                default:
+                    setSpeed(0);
+                    break;
+            }
         }
     }