Intake Advantagekit Done

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

@@ -10,12 +10,17 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.Constants.SimulationConstants;
 import frc.robot.commands.drive.DriveCommand;
+import frc.robot.commands.drive.Eject;
+import frc.robot.commands.drive.Intake;
 import frc.robot.extras.simulation.field.SimulatedField;
+import frc.robot.extras.simulation.mechanismSim.IntakeSimulation.IntakeSide;
 import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveDriveSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation.WHEEL_GRIP;
 import frc.robot.extras.util.JoystickUtil;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.intake.PhysicalIntake;
 import frc.robot.subsystems.swerve.SwerveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.DriveConstants;
 import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
@@ -43,6 +48,7 @@ public class RobotContainer {
   // private final Pivot pivot = new Pivot(new PivotIOTalonFX());
   // private final Flywheel flywheel = new Flywheel(new FlywheelIOTalonFX());
   private final CommandXboxController driverController = new CommandXboxController(0);
+  private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(new PhysicalIntake());
 
   // Simulation, we store them here in the robot container
   // private final SimulatedField simulatedArena;
@@ -179,6 +185,10 @@ private void configureButtonBindings() {
     Trigger driverDownDirectionPad = new Trigger(driverController.pov(180));
     Trigger driverLeftDirectionPad = new Trigger(driverController.pov(270));
 
+    driverController.a().whileTrue(new Intake(intakeSubsystem));
+    driverController.b().whileTrue(new Eject(intakeSubsystem));  
+
+
     // // autodrive
     // Trigger driverAButton = new Trigger(driverController::getAButton);
     // lol whatever
@@ -211,17 +221,7 @@ private void configureButtonBindings() {
 
     // // driving
 
-    Command driveCommand =
-        new DriveCommand(
-            swerveDrive,
-            visionSubsystem,
-            driverLeftStick[1],
-            driverLeftStick[0],
-            () -> JoystickUtil.modifyAxis(driverRightStickX, 3),
-            () -> !driverRightBumper.getAsBoolean(),
-            () -> driverLeftBumper.getAsBoolean());
-    swerveDrive.setDefaultCommand(driveCommand);
-
+
     // // shooterSubsystem.setDefaultCommand(new FlywheelSpinUpAuto(shooterSubsystem,
     // visionSubsystem));
 

src/main/java/frc/robot/commands/drive/Eject.java

@@ -0,0 +1,27 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+
+
+public class Eject extends Command{
+  private final IntakeSubsystem intakeSubsystem;
+
+  public Eject(IntakeSubsystem intakeSubsystem){
+    this.intakeSubsystem = intakeSubsystem;
+    addRequirements(this.intakeSubsystem);
+  }
+
+  public void execute(){
+    intakeSubsystem.setIntakeSpeed(IntakeConstants.ejectSpeed);
+  }
+
+  public void end(){
+    intakeSubsystem.setIntakeSpeed(0);
+  }
+
+  public boolean isFinished(){
+    return false;
+  }
+}

src/main/java/frc/robot/commands/drive/Intake.java

@@ -0,0 +1,26 @@
+package frc.robot.commands.drive;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+
+public class Intake extends Command{
+  private final IntakeSubsystem intakeSubsystem;
+
+  public Intake(IntakeSubsystem intakeSubsystem){
+    this.intakeSubsystem = intakeSubsystem;
+    addRequirements(this.intakeSubsystem);
+  }
+
+  public void execute(){
+    intakeSubsystem.setIntakeSpeed(IntakeConstants.intakeSpeed);
+  }
+
+  public void end(){
+    intakeSubsystem.setIntakeSpeed(0);
+  }
+
+  public boolean isFinished(){
+    return false;
+  }
+}

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -0,0 +1,43 @@
+// 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.elevator;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Elevator extends SubsystemBase {
+  /** Creates a new elevator. */
+  private ElevatorInterface elevatorInterface;
+
+  private ElevatorInputsAutoLogged inputs = new ElevatorInputsAutoLogged();
+
+  public Elevator(ElevatorInterface elevatorInterface) {
+    this.elevatorInterface = elevatorInterface;
+  }
+
+  public double getElevatorPosition() {
+    return elevatorInterface.getElevatorPosition();
+  }
+
+  public double getVolts() {
+    return elevatorInterface.getVolts();
+  }
+
+  public void setElevatorPosition(double position) {
+    elevatorInterface.setElevatorPosition(position);
+    Logger.recordOutput("Elevator/Position", getElevatorPosition());
+  }
+
+  public void setVolts(double volts) {
+    elevatorInterface.setVolts(volts);
+    Logger.recordOutput("Elevator/Volts", getVolts());
+  }
+
+  @Override
+  public void periodic() {
+    elevatorInterface.updateInputs(inputs);
+    Logger.processInputs("Elevator/", inputs);
+  }
+}

src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java

@@ -0,0 +1,32 @@
+// 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.elevator;
+
+/** Add your docs here. */
+public class ElevatorConstants {
+    public static final int ELEVATOR_LEADER_MOTOR_ID = 0 - 9;
+    public static final int ELEVATOR_FOLLOWER_MOTOR_ID = 0 - 9;
+
+    public static final double ELEVATOR_P = 0 - 9;
+    public static final double ELEVATOR_I = 0 - 9;
+    public static final double ELEVATOR_D = 0 - 9;
+    public static final double ELEVATOR_S = 0 - 9;
+    public static final double ELEVATOR_V = 0 - 9;
+    public static final double ELEVATOR_A = 0 - 9;
+    public static final double ELEVATOR_G = 0 - 9;
+
+
+    public static final double DRUM_RADIUS = 0 - 9;
+    public static final double ELEVATOR_GEAR_RATIO = 2;
+    public static final double ELEVATOR_CARRIAGE_MASS = 0 - 9;
+    public static final double ENCODER_CONVERSION_FACTOR = 71.81;
+    public static final double MIN_HEIGHT = 0;
+    public static final double MAX_HEIGHT = 0 - 9;
+
+    public static final double STATOR_CURRENT_LIMIT = 0 - 9;
+    public static final double SUPPLY_CURRENT_LIMIT = 0 - 9;
+    public static final boolean STATOR_CURRENT_LIMIT_ENABLE = true;
+    public static final boolean SUPPLY_CURRENT_LIMIT_ENABLE = true;
+  }

src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java

@@ -0,0 +1,33 @@
+// 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.elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ElevatorInterface {
+  /** Creates a new ElevatorInterface. */
+  @AutoLog
+  public static class ElevatorInputs { // For values
+    public double leaderMotorPosition = 0.0;
+    public double followerMotorPosition = 0.0;
+    public double leaderMotorVoltage = 0.0;
+    public double followerMotorVoltage = 0.0;
+
+  }
+
+  public default void updateInputs(ElevatorInputs inputs) {}
+
+  public default double getElevatorPosition() {
+    return 0.0;
+  }
+
+  public default void setElevatorPosition(double position) {}
+
+  public default void setVolts(double volts) {}
+
+  public default double getVolts() {
+    return 0.0;
+  }
+}

src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java

@@ -0,0 +1,105 @@
+// 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.elevator;
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.Constants.HardwareConstants;
+import frc.robot.subsystems.elevator.ElevatorInterface.ElevatorInputs;
+
+public class PhysicalElevator implements ElevatorInterface {
+
+  /** Creates a new ElevatorHardware. */
+  private final TalonFX leaderMotor = new TalonFX(ElevatorConstants.ELEVATOR_LEADER_MOTOR_ID);
+  private final TalonFX followerMotor = new TalonFX(ElevatorConstants.ELEVATOR_FOLLOWER_MOTOR_ID);
+  private final Follower followerMotorControl = new Follower(ElevatorConstants.ELEVATOR_LEADER_MOTOR_ID, true);
+
+  StatusSignal<Angle> leaderPosition;
+  StatusSignal<Angle> followerPosition;
+  StatusSignal<Voltage> leaderAppliedVoltage;
+  StatusSignal<Voltage> followerAppliedVoltage;
+
+  public PhysicalElevator() {
+    leaderPosition = leaderMotor.getRotorPosition();
+    leaderAppliedVoltage = leaderMotor.getMotorVoltage();
+    followerPosition = followerMotor.getRotorPosition();
+    followerAppliedVoltage = followerMotor.getMotorVoltage();
+
+    followerMotor.setControl(followerMotorControl);
+
+    TalonFXConfiguration elevatorConfig = new TalonFXConfiguration();
+
+    elevatorConfig.Slot0.kP = ElevatorConstants.ELEVATOR_P;
+    elevatorConfig.Slot0.kI = ElevatorConstants.ELEVATOR_I;
+    elevatorConfig.Slot0.kD = ElevatorConstants.ELEVATOR_D;
+    elevatorConfig.Slot0.kS = ElevatorConstants.ELEVATOR_S;
+    elevatorConfig.Slot0.kV = ElevatorConstants.ELEVATOR_V;
+    elevatorConfig.Slot0.kA = ElevatorConstants.ELEVATOR_A;
+    elevatorConfig.Slot0.kG = ElevatorConstants.ELEVATOR_G;
+
+
+    elevatorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+
+    elevatorConfig.CurrentLimits.StatorCurrentLimit = ElevatorConstants.STATOR_CURRENT_LIMIT;
+    elevatorConfig.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.SUPPLY_CURRENT_LIMIT;
+    elevatorConfig.CurrentLimits.StatorCurrentLimitEnable = ElevatorConstants.STATOR_CURRENT_LIMIT_ENABLE;
+    elevatorConfig.CurrentLimits.SupplyCurrentLimitEnable = ElevatorConstants.STATOR_CURRENT_LIMIT_ENABLE;
+
+    elevatorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+    leaderMotor.getConfigurator().apply(elevatorConfig);
+    followerMotor.getConfigurator().apply(elevatorConfig);
+
+    BaseStatusSignal.setUpdateFrequencyForAll(HardwareConstants.SIGNAL_FREQUENCY, leaderPosition, leaderAppliedVoltage, followerPosition, followerAppliedVoltage);
+  }
+  public void updateInputs(ElevatorInputs inputs) {
+    inputs.leaderMotorPosition = leaderPosition.getValueAsDouble();
+    inputs.leaderMotorVoltage = leaderAppliedVoltage.getValueAsDouble();
+    inputs.followerMotorPosition = followerPosition.getValueAsDouble();
+    inputs.followerMotorVoltage = followerAppliedVoltage.getValueAsDouble();
+  }
+
+  public double getElevatorPosition() {
+    return leaderPosition.getValueAsDouble();
+  }
+  public void setElevatorLevel(int number) {
+    switch(number) {
+      case 1:
+
+      case 2:
+
+      case 3:
+
+      case 4:
+    }
+  }
+
+  public void setElevatorPosition(double position) {
+    leaderMotor.setPosition(position);
+    followerMotor.setPosition(position);
+  }
+
+  public void setVolts(double volts) {
+    leaderMotor.setVoltage(volts);
+    followerMotor.setVoltage(volts);
+  }
+
+  public double getVolts() {
+    return leaderMotor.getMotorVoltage().getValueAsDouble();
+  }
+
+
+
+
+}