Added elevator and command and constants. Maybe good dunno

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

@@ -139,6 +139,16 @@ public AnglerConstants(
     }
   }
 
+  public static class ElevatorConstants {
+    public static final double maxHeight = 4; // Temp 
+    public static final double minHeight = 0;
+
+    public static final MotionMagic pid = new MotionMagic(.05, 0, .05, 0); // temp
+
+    public static final double roboWeight = 120; // Temp
+    public static final boolean enableFF = true;
+  }
+
   public static class DrivebaseConstant {
 
     public static final int ID_PIGEON2 = 13;

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

@@ -18,6 +18,7 @@
 import frc.robot.commands.RunManipulatorCommand;
 import frc.robot.subsystems.Angler;
 import frc.robot.subsystems.Drivebase;
+import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Manipulator;
 import frc.robot.subsystems.PheonixDrivebase;
@@ -35,7 +36,8 @@ public class RobotContainer {
   private final Angler pivot = systems.getPivot();
   private final Intake intake = systems.getIntake();
   private final Shooter shooter = systems.getShooter();
-
+  private final Elevator elevator = systems.getElevator();
+
   private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
       //.withDeadband(TunerConstatns.kSpeedAt12VoltsMps * 0.2).withRotationalDeadband(TunerConstatns.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND * 0.1) // Add a 10% deadband
       .withDriveRequestType(DriveRequestType.OpenLoopVoltage);

src/main/java/frc/robot/Systems.java

@@ -10,6 +10,7 @@
 import frc.robot.Constants.TunerConstatns;
 import frc.robot.subsystems.Angler;
 import frc.robot.subsystems.Drivebase;
+import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.PheonixDrivebase;
 import frc.robot.subsystems.Shooter;
@@ -23,12 +24,15 @@ public class Systems {
   private Angler shooterAngler;
   private Drivebase drivebase;
   private Shooter shooter;
-
+  private Elevator elevator;
   private AnglerConstants intakeAnglerConst;
 
   private CANSparkFlex shooterUpper; 
   private CANSparkFlex shooterLower; 
 
+  private CANSparkFlex elevatorLeft; 
+  private CANSparkFlex elevatorRight; 
+
   private CANSparkMax leftIntakeMotor;
   private CANSparkMax rightIntakeMotor;
   private CANSparkMax intakeAnglerMotor;
@@ -61,6 +65,8 @@ public Systems() {
 
     intake = new Intake(leftIntakeMotor, rightIntakeMotor);
     pivot = new Angler(intakeAnglerMotor, intakeAnglerConst, "pivot");
+
+    // elevator = new Elevator(elevatorLeft, elevatorRight);
   }
 
   public PheonixDrivebase getDrivebase() {
@@ -86,4 +92,8 @@ public Angler getPivot() {
   public Angler getShooterAngler() {
     return shooterAngler;
   }
+
+  public Elevator getElevator() {
+    return elevator;
+  }
 }

src/main/java/frc/robot/commands/RunElevatorCommand.java

@@ -0,0 +1,27 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Elevator;
+
+public class RunElevatorCommand extends Command{
+  private Elevator elevator;
+
+  public RunElevatorCommand (Elevator elevator) {
+    this.elevator = elevator;
+  }
+
+  @Override
+  public void initialize() {
+
+  }
+
+  @Override
+  public void execute() {
+    elevator.setPosition();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

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

@@ -0,0 +1,48 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkFlex;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.SparkPIDController;
+import com.revrobotics.SparkPIDController.ArbFFUnits;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ElevatorConstants;
+
+public class Elevator extends SubsystemBase {
+
+  private CANSparkFlex left;
+  private CANSparkFlex right;
+  private RelativeEncoder relativeEncoder;
+  public SparkPIDController controller;
+
+  public Elevator (CANSparkFlex left, CANSparkFlex right) {
+    this.left = left;
+    this.right = right;
+    this.controller = right.getPIDController(); 
+    this.controller.setP(ElevatorConstants.pid.p());
+    this.controller.setI(ElevatorConstants.pid.i());
+    this.controller.setD(ElevatorConstants.pid.d());
+
+    left.setInverted(true);  // dunno if needed
+    right.setInverted(true); // dunno if needed
+    left.follow(right);
+    controller.setFeedbackDevice(relativeEncoder);
+
+    this.left.burnFlash();
+    this.right.burnFlash();
+  }
+
+  public void setPosition () {
+    double downForce = Units.lbsToKilograms(ElevatorConstants.roboWeight) * 9.81;
+
+    controller.setReference(
+      ElevatorConstants.maxHeight,
+      CANSparkBase.ControlType.kPosition,
+      0,
+      ElevatorConstants.enableFF ? downForce : 0,
+      ArbFFUnits.kPercentOut
+    );
+  }
+}

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

@@ -8,17 +8,17 @@
 
 public class Intake extends Manipulator {
 
-  CANSparkBase motor;
-  CANSparkBase kingBob; // as in the rubber duck not the stupid filet minion
+  CANSparkBase duckPrime; // the second duck!
+  CANSparkBase kingBob;  // as in the rubber duck not the stupid filet minion
 
-  public Intake(CANSparkBase motor, CANSparkBase kingBob) {
-    this.motor = motor;
+  public Intake(CANSparkBase duckPrime, CANSparkBase kingBob) {
+    this.duckPrime = duckPrime;
     this.kingBob = kingBob;
-    //kingBob.follow(motor);
+    //kingBob.follow(duckPrime);
     kingBob.setInverted(false);
     kingBob.burnFlash();
-     motor.setInverted(false);
-    motor.burnFlash();
+     duckPrime.setInverted(false);
+    duckPrime.burnFlash();
 
     this.setName("Intake");
   }
@@ -35,30 +35,30 @@ public double getReverseVelocityMultiplier() {
 
   @Override
   public void runNeutral() {
-    motor.setIdleMode(IdleMode.kCoast);
+    duckPrime.setIdleMode(IdleMode.kCoast);
     kingBob.setIdleMode(IdleMode.kCoast);
-    motor.burnFlash();
+    duckPrime.burnFlash();
     kingBob.burnFlash();
   }
 
   @Override
   public void stopNeutral() {
-    motor.setIdleMode(IdleMode.kBrake);
+    duckPrime.setIdleMode(IdleMode.kBrake);
     kingBob.setIdleMode(IdleMode.kCoast);
-    motor.burnFlash();
+    duckPrime.burnFlash();
     kingBob.burnFlash();
   }
 
   @Override
   public void runWithPower(double power) {
     super.runWithPower(power);
     kingBob.set(power);
-    motor.set(power);
+    duckPrime.set(power);
   }
 
   @Override
   public void periodic() {
-      SmartDashboard.putNumber("Intake currentOut", motor.getOutputCurrent());
+      SmartDashboard.putNumber("Intake currentOut", duckPrime.getOutputCurrent());
       SmartDashboard.putBoolean("Has Gamepice", checkGamePieceStatus());
   }
 
@@ -69,9 +69,9 @@ public void periodic() {
    */
   @Override
   public boolean checkGamePieceStatus() {
-    if(motor.getOutputCurrent() > 30) {
+    if(duckPrime.getOutputCurrent() > 30) {
       hadGamePiece = true;
-    }else if(motor.get() < 0) {
+    }else if(duckPrime.get() < 0) {
       hadGamePiece = false;
     }