Made the subsystem

src/main/java/frc/robot/Commands/IntakeCommand.java

@@ -6,7 +6,7 @@
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import frc.robot.Constants;
-import frc.robot.Subsystems.IntakeSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
 
 public class IntakeCommand extends CommandBase {
 

src/main/java/frc/robot/Commands/SetElevatorHeightCommand.java

@@ -0,0 +1,46 @@
+// 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.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.subsystems.ElevatorSubsystem;
+
+public class SetElevatorHeightCommand extends CommandBase {
+
+  ElevatorSubsystem elevatorSubsystem;
+  double height;
+
+  /** Creates a new SetElevatorHeightCommand. */
+  public SetElevatorHeightCommand(ElevatorSubsystem elevatorSubsystem, double height) {
+
+    this.elevatorSubsystem = elevatorSubsystem;
+    this.height = height;
+
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(elevatorSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    //set the elevator height
+    elevatorSubsystem.setHeight(height);
+
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return elevatorSubsystem.nearTargetHeight();
+  }
+}

src/main/java/frc/robot/Commands/StopIntakeMotorCommand.java

@@ -5,7 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Subsystems.IntakeSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
 
 public class StopIntakeMotorCommand extends CommandBase {
   IntakeSubsystem intakeSubsystem;

src/main/java/frc/robot/Constants.java

@@ -30,4 +30,22 @@ public static class Intake {
 
     public static final double UNLOADING_WAIT_TIME = 2f;
   }
+
+  public static class Elevator{
+
+    public static final int RIGHT_ELEVATOR_MOTOR_NUMBER = 14;
+    public static final int LEFT_ELEVATOR_MOTOR_NUMBER = 15;
+
+    public static final int TICKS_PER_REVOLUTION = 2048;
+    public static final double GEAR_RATIO = 0.1008d;
+    public static final double GEAR_CIRCUMFERENCE = 1.432d*Math.PI;
+    public static final double CARRIAGE_RATIO = 2;
+
+    public static final double MAX_ELEVATOR_EXTENSION = 54;
+
+    public static final double NEAR_TARGET_HEIGHT_THRESHOLD = .5d;
+
+
+
+  }
 }

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

@@ -7,6 +7,8 @@
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.commands.AdvancedIntakeSequence;
+import frc.robot.commands.SetElevatorHeightCommand;
+import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 
 /**
@@ -22,6 +24,7 @@ public class RobotContainer {
   public CommandXboxController driverB = new CommandXboxController(0);
 
   private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
+  private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -66,5 +69,8 @@ private void configureButtonBindings() {
     driverB.leftTrigger().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, true));
     driverB.rightBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, false, false));
     driverB.leftBumper().onTrue(new AdvancedIntakeSequence(intakeSubsystem, true, false));
+
+    driverB.a().onTrue(new SetElevatorHeightCommand(elevatorSubsystem, 20));
+    driverB.b().onTrue(new SetElevatorHeightCommand(elevatorSubsystem, 40));
   }
 }

src/main/java/frc/robot/Subsystems/ElevatorSubsystem.java

@@ -0,0 +1,119 @@
+// 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;
+
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class ElevatorSubsystem extends SubsystemBase {
+    /** leader */
+    private TalonFX left_motor;
+    /** follower */
+    private TalonFX right_motor;
+
+    private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
+
+    private double currentHeight;
+    private double targetHeight;  
+
+    private double motorPower;
+
+    private PIDController controller;
+
+    /** Creates a new ElevatorSubsystem. */
+    public ElevatorSubsystem() {
+
+      left_motor = new TalonFX(14);
+      right_motor = new TalonFX(15);
+
+      right_motor.configFactoryDefault();
+      left_motor.configFactoryDefault();
+
+      right_motor.clearStickyFaults();
+      left_motor.clearStickyFaults();
+
+      right_motor.configForwardSoftLimitThreshold(
+          0, 0); // this is the bottom limit, we stop AT the bottom
+      right_motor.configReverseSoftLimitThreshold(-inchesToTicks(Constants.Elevator.MAX_ELEVATOR_EXTENSION), 0); // this is the top limit, we stop at the very top
+
+      right_motor.configForwardSoftLimitEnable(true, 0);
+      right_motor.configReverseSoftLimitEnable(true, 0);
+
+      right_motor.configOpenloopRamp(.5);
+
+      left_motor.setSelectedSensorPosition(0);
+      right_motor.setSelectedSensorPosition(0);
+
+      // make sure we hold our height when we get disabled
+      right_motor.setNeutralMode(NeutralMode.Coast);
+      left_motor.setNeutralMode(NeutralMode.Coast);
+
+      right_motor.follow(left_motor);
+
+      targetHeight = 0;
+
+      motorPower = 0;
+
+      controller = new PIDController(.02, 0, .01);
+
+      ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
+
+      ElevatorTab.addNumber("Left Motor Speed", left_motor::getSelectedSensorVelocity);
+      ElevatorTab.addNumber("Right Motor Speed", right_motor::getSelectedSensorVelocity);
+
+      ElevatorTab.addNumber("height", () -> this.currentHeight);
+      ElevatorTab.addNumber("target height", () -> this.targetHeight);
+      ElevatorTab.addNumber("right motor sensor value", this::getHeight);
+
+    }
+
+    public void setMotorPower(double motorPower){
+      this.motorPower = MathUtil.clamp(motorPower, -0.25, 0.25);
+    }
+
+    public double inchesToTicks(double inches){
+
+      return (inches / (Constants.Elevator.GEAR_RATIO * Constants.Elevator.GEAR_CIRCUMFERENCE * Constants.Elevator.CARRIAGE_RATIO)) * Constants.Elevator.TICKS_PER_REVOLUTION; 
+    }
+
+    public double ticksToInches(double ticks){
+      return (ticks/ Constants.Elevator.TICKS_PER_REVOLUTION) * Constants.Elevator.GEAR_RATIO * Constants.Elevator.GEAR_CIRCUMFERENCE * Constants.Elevator.CARRIAGE_RATIO;
+    }
+
+    public void setHeight(double targetHeight){
+      this.targetHeight = targetHeight;
+
+      controller.setSetpoint(targetHeight);
+    }
+
+    public double getHeight(){
+      return currentHeight;
+    }
+
+    public boolean nearTargetHeight(){
+      return (targetHeight - Constants.Elevator.NEAR_TARGET_HEIGHT_THRESHOLD <= getHeight() 
+      && getHeight() <= targetHeight + Constants.Elevator.NEAR_TARGET_HEIGHT_THRESHOLD);
+    }
+
+    @Override
+    public void periodic() {
+
+      currentHeight = ticksToInches(-left_motor.getSelectedSensorPosition());
+
+      motorPower = controller.calculate(currentHeight);
+
+
+      left_motor.set(TalonFXControlMode.PercentOutput, motorPower);
+
+    }
+}