Adding an expanded Elevator subsystem and adjusting the supporting players

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

@@ -4,32 +4,73 @@
 
 package frc.robot;
 
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.constants.OIConstants;
+import frc.robot.constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.subsystems.Elevator;
 
 public class RobotContainer {
   private Elevator elevator;
 
+  private CommandXboxController driver;
+
   public RobotContainer() {
     elevator = new Elevator();
 
+    driver = new CommandXboxController(OIConstants.kDriverUSB);
+
     configureBindings();
     configureShuffleboard();
   }
 
   private void configureBindings() {
     elevator.setDefaultCommand(
-      elevator.debugSetpoints()
+      elevator.assistedManualControl(driver::getRightY)
     );
+
+    driver.a().onTrue(elevator.goToPosition(ElevatorPositions.kBottom, 3));
+    driver.b().onTrue(elevator.goToPosition(ElevatorPositions.kLowMid, 3));
+    driver.x().onTrue(elevator.goToPosition(ElevatorPositions.kHighMid, 3));
+    driver.y().onTrue(elevator.goToPosition(ElevatorPositions.kTop, 3));
   }
 
   private void configureShuffleboard() {
     ShuffleboardTab tab = Shuffleboard.getTab("MyTab");
 
-    tab.add("Elevator Subsystem", elevator);
+    tab.addDouble("Elevator Encoder Distance", elevator::getEncoderDistance)
+      .withPosition(0, 0)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kTextView);
+
+    tab.addDouble("Elevator Encoder Rate", elevator::getEncoderRate)
+      .withPosition(2, 1)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kTextView);
+
+    tab.addDouble("Elevator Bottom Setpoint", ElevatorPositions.kBottom::getPositionValue)
+      .withPosition(0, 1)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kTextView);
+
+    tab.addDouble("Elevator Low-Mid Setpoint", ElevatorPositions.kLowMid::getPositionValue)
+      .withPosition(2, 1)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kTextView);
+
+    tab.addDouble("Elevator High-Mid Setpoint", ElevatorPositions.kHighMid::getPositionValue)
+      .withPosition(0, 2)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kTextView);
+
+    tab.addDouble("Elevator Top Setpoint", ElevatorPositions.kTop::getPositionValue)
+      .withPosition(2, 2)
+      .withSize(2, 1)
+      .withWidget(BuiltInWidgets.kTextView);
   }
 
   public Command getAutonomousCommand() {

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

@@ -1,13 +1,33 @@
 package frc.robot.constants;
 
 public interface ElevatorConstants {
+    public enum ElevatorPositions {
+        kBottom(0),
+        kLowMid(1),
+        kHighMid(2),
+        kTop(3);
+
+        private double position;
+
+        private ElevatorPositions(double position) {
+            this.position = position;
+        }
+
+        public double getPositionValue() {
+            return position;
+        }
+
+        public void setPositionValue(double newPosition) {
+            position = newPosition;
+        }
+    }
     public static final int kMotor1CANID = 0;
     public static final int kMotor2CANID = 1;
 
     public static final int kEncoderChannelADIO = 0;
     public static final int kEncoderChannelBDIO = 1;
 
-    public static final double kEncoderDistancePerPulse = 0;
+    public static final double kEncoderDistancePerPulse = 0.0000000001; //Avoids UncleanStatusException distancPerPulse must not be 0
 
     public static final double kPPositional = 0;
     public static final double kIPositional = 0;
@@ -24,8 +44,5 @@ public interface ElevatorConstants {
     public static final double kFFG = 0;
     public static final double kFFV = 0;
 
-    public static final double kDefaultBottomPositionalSetpoint = 0;
+    public static final double kElevatorMaxVelocity = 0;
-    public static final double kDefaultLowMidPositionalSetpoint = 1;
-    public static final double kDefaultHighMidPositionalSetpoint = 2;
-    public static final double kDefaultTopPositionalSetpoint = 3;
 }

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

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class OIConstants {
+    public static final int kDriverUSB = 0;
+}

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

@@ -1,5 +1,7 @@
 package frc.robot.subsystems;
 
+import java.util.function.DoubleSupplier;
+
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
 
 import edu.wpi.first.math.controller.ElevatorFeedforward;
@@ -7,11 +9,11 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.RepeatCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.constants.ElevatorConstants;
+import frc.robot.constants.ElevatorConstants.ElevatorPositions;
 
 public class Elevator extends SubsystemBase {
     private WPI_VictorSPX motor1;
@@ -24,16 +26,14 @@ public class Elevator extends SubsystemBase {
 
     private ElevatorFeedforward eff;
 
-    private double currentBottomPositionalSetpoint;
-    private double currentLowMidPositionalSetpoint;
-    private double currentHighMidPositionalSetpoint;
-    private double currentTopPositionalSetpoint;
-
     public Elevator() {
         motor1 = new WPI_VictorSPX(ElevatorConstants.kMotor1CANID);
         motor2 = new WPI_VictorSPX(ElevatorConstants.kMotor2CANID);
 
+        motor2.follow(motor1);
+
         encoder = new Encoder(ElevatorConstants.kEncoderChannelADIO, ElevatorConstants.kEncoderChannelBDIO);
+        encoder.setDistancePerPulse(ElevatorConstants.kEncoderDistancePerPulse);
 
         positionalController = new PIDController(
             ElevatorConstants.kPPositional, 
@@ -52,20 +52,47 @@ public class Elevator extends SubsystemBase {
             ElevatorConstants.kFFG, 
             ElevatorConstants.kFFV
         );
+    }
 
-        currentBottomPositionalSetpoint = ElevatorConstants.kDefaultBottomPositionalSetpoint;
+    public double getEncoderDistance() {
-        currentLowMidPositionalSetpoint = ElevatorConstants.kDefaultLowMidPositionalSetpoint;
+        return encoder.getDistance();
-        currentHighMidPositionalSetpoint = ElevatorConstants.kDefaultHighMidPositionalSetpoint;
+    }
-        currentTopPositionalSetpoint = ElevatorConstants.kDefaultTopPositionalSetpoint;
+
+    public double getEncoderRate() {
+        return encoder.getRate();
+    }
+
+    public Command goToPosition(ElevatorPositions position, double timeout) {
+        return run(() -> {
+            double voltsOut = positionalController.calculate(
+                encoder.getDistance(),
+                position.getPositionValue()
+            ) + eff.calculate(0);
+
+            motor1.setVoltage(voltsOut);
+        }).until(positionalController::atSetpoint).withTimeout(timeout);
+    }
+
+    public Command assistedManualControl(DoubleSupplier speed) {
+        return run(() -> {
+            double realSpeedTarget = speed.getAsDouble() * ElevatorConstants.kElevatorMaxVelocity;
+
+            double voltsOut = velocityController.calculate(
+                encoder.getRate(), 
+                realSpeedTarget
+            ) + eff.calculate(realSpeedTarget);
+
+            motor1.setVoltage(voltsOut);
+        });
     }
 
     public Command debugSetpoints() {
         return new RepeatCommand(
             runOnce(() -> {
-                System.out.println("Current Bottom Positional Setpoint: " + currentBottomPositionalSetpoint);
+                System.out.println("Current Bottom Positional Setpoint: " + ElevatorPositions.kBottom.getPositionValue());
-                System.out.println("Current Low-Mid Positional Setpoint: " + currentLowMidPositionalSetpoint);
+                System.out.println("Current Low-Mid Positional Setpoint: " + ElevatorPositions.kLowMid.getPositionValue());
-                System.out.println("Current High-Mid Positional Setpoint: " + currentHighMidPositionalSetpoint);
+                System.out.println("Current High-Mid Positional Setpoint: " + ElevatorPositions.kHighMid.getPositionValue());
-                System.out.println("Current Top Positional Setpoint: " + currentTopPositionalSetpoint);
+                System.out.println("Current Top Positional Setpoint: " + ElevatorPositions.kTop.getPositionValue());
                 System.out.println("");
             }).andThen(new WaitCommand(5))
         );
@@ -77,30 +104,26 @@ public class Elevator extends SubsystemBase {
 
         builder.addDoubleProperty(
             ".currentBottomPositionalSetpoint", 
-            () -> currentBottomPositionalSetpoint, 
+            () -> ElevatorPositions.kBottom.getPositionValue(), 
-            (d) -> {
+            (d) -> ElevatorPositions.kBottom.setPositionValue(d)
-                System.out.println("Current Bottom Positional Setpoint Changed!");
-                System.out.println("Old Value: " + currentBottomPositionalSetpoint + " New Value: " + d);
-                currentBottomPositionalSetpoint = d;
-            }
         );
 
         builder.addDoubleProperty(
             ".currentLowMidPositionalSetpoint", 
-            () -> currentLowMidPositionalSetpoint, 
+            () -> ElevatorPositions.kLowMid.getPositionValue(), 
-            (d) -> currentLowMidPositionalSetpoint = d
+            (d) -> ElevatorPositions.kLowMid.setPositionValue(d)
         );
 
         builder.addDoubleProperty(
             ".currentHighMidPositionalSetpoint", 
-            () -> currentHighMidPositionalSetpoint, 
+            () -> ElevatorPositions.kHighMid.getPositionValue(), 
-            (d) -> currentHighMidPositionalSetpoint = d
+            (d) -> ElevatorPositions.kHighMid.setPositionValue(d)
         );
 
         builder.addDoubleProperty(
             ".currentTopPositionalSetpoint", 
-            () -> currentTopPositionalSetpoint, 
+            () -> ElevatorPositions.kTop.getPositionValue(), 
-            (d) -> currentTopPositionalSetpoint = d
+            (d) -> ElevatorPositions.kTop.setPositionValue(d)
         );
     }
 }