make elevator ready to test

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

@@ -15,10 +15,18 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.subsystems.Elevator.ElevatorSubsystem;
 import frc.robot.subsystems.Vision.VisionSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.Commands.Elevator.ElevatorToGroundCommand;
+import frc.robot.Commands.Elevator.ElevatorToL1Command;
+import frc.robot.Commands.Elevator.ElevatorToL2Command;
+import frc.robot.Commands.Elevator.ElevatorToL3Command;
+import frc.robot.Commands.Elevator.ElevatorToL4Command;
+import frc.robot.Commands.Elevator.ElevatorToSourceCommand;
 import frc.robot.Constants.VisionConstants;
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -29,7 +37,9 @@
 public class RobotContainer {
 
   private BaseDriveController driveController;
+  private CommandPS5Controller mechController;
 
+  private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
   private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem();
   private final VisionSubsystem visionSubsystem2 = new VisionSubsystem(
     VisionConstants.cameraConfigs[1]
@@ -44,6 +54,7 @@ public class RobotContainer {
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     constructDriveController(); 
+    constructMechController();
     // startLog();
     setVisionDataInterface();
     configureBindings();
@@ -99,18 +110,36 @@ public Command getAutonomousCommand() {
    */
   private void constructDriveController(){
     DriverStation.refreshData();
-    if(DriverStation.getJoystickName(0).equals("Controller (Xbox One For Windows)")) {
-        driveController = new XboxDriveController();
-    }
-    else if(DriverStation.getJoystickName(0).equals("DualSense Wireless Controller")){
+    // if(DriverStation.getJoystickName(0).equals("Controller (Xbox One For Windows)")) {
+    //     driveController = new XboxDriveController();
+    // }
+    // else if(DriverStation.getJoystickName(0).equals("DualSense Wireless Controller")){
         driveController = new PS5DriveController();
-    }
-    else{
-        driveController = new DualJoystickDriveController();
-    }
+    // }
+    // else{
+    //     driveController = new DualJoystickDriveController();
+    // }
     driveController.setDeadZone(0.05);
   }
 
+  private void constructMechController(){
+    mechController = new CommandPS5Controller(0);
+    elevatorSubsystem.setDefaultCommand(
+      new RunCommand(
+        () -> {
+          elevatorSubsystem.setVelocityReference(mechController.getLeftY());
+        },
+        elevatorSubsystem
+      )
+    ); 
+    mechController.circle().onTrue(new ElevatorToGroundCommand(elevatorSubsystem));
+    mechController.cross().onTrue(new ElevatorToL1Command(elevatorSubsystem));
+    mechController.square().onTrue(new ElevatorToL2Command(elevatorSubsystem));
+    mechController.triangle().onTrue(new ElevatorToL3Command(elevatorSubsystem));
+    mechController.L1().onTrue(new ElevatorToL4Command(elevatorSubsystem));
+    mechController.R1().onTrue(new ElevatorToSourceCommand(elevatorSubsystem));
+  }
+
   /**
    * Starts datalog at /u/logs
    */

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

@@ -80,6 +80,10 @@ public void periodic() {
   public void setPositionReference(double positionReference){
     motor.setPositionReference(positionReference);
   }
+
+  public void setVelocityReference(double velocityReference){
+    motor.setVelocityReference(velocityReference);
+  }
 
   /**
    * Gets the closed loop error of the motor.