Merge pull request #82 from 4201VitruvianBots/elevator

elevatorrrr

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

@@ -18,11 +18,13 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.RunClimberIntake;
 import frc.robot.commands.RunEndEffectorIntake;
+import frc.robot.commands.ToggleGamePiece;
 import frc.robot.commands.alphabot.RunAlgaeIntake;
 import frc.robot.commands.alphabot.RunCoralOuttake;
 import frc.robot.commands.autos.DriveForward;
@@ -34,10 +36,10 @@
 import frc.robot.commands.swerve.ResetGyro;
 import frc.robot.commands.swerve.SwerveCharacterization;
 import frc.robot.constants.CLIMBER.CLIMBER_SETPOINT;
-import frc.robot.constants.ELEVATOR.ELEVATOR_SETPOINT;
 import frc.robot.constants.ENDEFFECTOR.PIVOT_SETPOINT;
 import frc.robot.constants.FIELD;
 import frc.robot.constants.ROBOT;
+import frc.robot.constants.ROBOT.SUPERSTRUCTURE_STATES;
 import frc.robot.constants.SWERVE;
 import frc.robot.constants.SWERVE.ROUTINE_TYPE;
 import frc.robot.constants.USB;
@@ -75,20 +77,23 @@ public class RobotContainer {
   private CoralOuttake m_coralOuttake;
   private AlgaeIntake m_algaeIntake;
 
-  @Logged(name = "EndEffectorPivot")
-  private EndEffectorPivot m_endEffectorPivot;
+  // V2 subsystems
+  private Climber m_climber;
+  private ClimberIntake m_climberIntake;
+  private Elevator m_elevator;
 
   @Logged(name = "EndEffector")
   private EndEffector m_endEffector;
 
-  // V2 subsystems
-  private Elevator m_elevator;
-  private ClimberIntake m_climberIntake;
-  private Climber m_climber;
+  @Logged(name = "EndEffectorPivot")
+  private EndEffectorPivot m_endEffectorPivot;
+
   private HopperIntake m_hopperIntake;
 
   private final Robot2d m_robot2d = new Robot2d();
 
+  private ROBOT.GAME_PIECE m_selectedGamePiece = ROBOT.GAME_PIECE.CORAL;
+
   // Replace with CommandPS4Controller or CommandJoystick if needed
   private final Joystick leftJoystick = new Joystick(USB.leftJoystick);
   @NotLogged private final SendableChooser<Command> m_sysidChooser = new SendableChooser<>();
@@ -284,28 +289,37 @@ private void configureAlphaBotBindings() {
   }
 
   private void configureV2Bindings() {
-    if (m_elevator != null) {
+    // Algae Toggle
+    m_driverController
+        .leftBumper()
+        .onTrue(new ToggleGamePiece(() -> m_selectedGamePiece, gp -> m_selectedGamePiece = gp));
+
+    if (m_elevator != null && m_endEffectorPivot != null) {
       m_driverController
           .a()
-          .whileTrue(new SetElevatorSetpoint(m_elevator, ELEVATOR_SETPOINT.LEVEL_2));
+          .whileTrue(
+              new ParallelCommandGroup(
+                  new EndEffectorSetpoint(m_endEffectorPivot, PIVOT_SETPOINT.L3_L2),
+                  new SetElevatorSetpoint(
+                      m_elevator, SUPERSTRUCTURE_STATES.L2, () -> m_selectedGamePiece)));
       m_driverController
           .x()
-          .whileTrue(new SetElevatorSetpoint(m_elevator, ELEVATOR_SETPOINT.PROCESSOR));
+          .whileTrue(
+              new SetElevatorSetpoint(
+                  m_elevator, SUPERSTRUCTURE_STATES.L1, () -> m_selectedGamePiece));
       m_driverController
           .y()
-          .whileTrue(new SetElevatorSetpoint(m_elevator, ELEVATOR_SETPOINT.LEVEL_4));
+          .whileTrue(
+              new SetElevatorSetpoint(
+                  m_elevator, SUPERSTRUCTURE_STATES.L4, () -> m_selectedGamePiece));
       m_driverController
           .b()
-          .whileTrue(new SetElevatorSetpoint(m_elevator, ELEVATOR_SETPOINT.LEVEL_3));
+          .whileTrue(
+              new SetElevatorSetpoint(
+                  m_elevator, SUPERSTRUCTURE_STATES.L3, () -> m_selectedGamePiece));
       m_driverController.povLeft().whileTrue(new RunClimberIntake(m_climberIntake, 0.25));
     }
 
-    if (m_endEffectorPivot != null) {
-      m_driverController
-          .a()
-          .whileTrue(new EndEffectorSetpoint(m_endEffectorPivot, PIVOT_SETPOINT.L3_L2));
-    }
-
     if (m_endEffector != null) {
       m_driverController
           .leftTrigger()

src/main/java/frc/robot/commands/ToggleGamePiece.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.commands;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.constants.ROBOT;
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+public class ToggleGamePiece extends InstantCommand {
+  private Supplier<ROBOT.GAME_PIECE> m_getGamePiece;
+  private Consumer<ROBOT.GAME_PIECE> m_toggleGamePiece;
+
+  /** Creates a new ToggleGamePiece. */
+  public ToggleGamePiece(
+      Supplier<ROBOT.GAME_PIECE> getGamePiece, Consumer<ROBOT.GAME_PIECE> toggleGamePiece) {
+    m_getGamePiece = getGamePiece;
+    m_toggleGamePiece = toggleGamePiece;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    if (m_getGamePiece.get() == ROBOT.GAME_PIECE.CORAL) {
+      m_toggleGamePiece.accept(ROBOT.GAME_PIECE.ALGAE);
+    } else {
+      m_toggleGamePiece.accept(ROBOT.GAME_PIECE.CORAL);
+    }
+  }
+}

src/main/java/frc/robot/commands/elevator/SetElevatorSetpoint.java

@@ -3,25 +3,78 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.constants.ELEVATOR;
 import frc.robot.constants.ELEVATOR.ELEVATOR_SETPOINT;
+import frc.robot.constants.ROBOT;
 import frc.robot.constants.ROBOT.CONTROL_MODE;
 import frc.robot.subsystems.Elevator;
+import java.util.function.Supplier;
 
 public class SetElevatorSetpoint extends Command {
 
   private final Elevator m_elevator;
-  private final ELEVATOR_SETPOINT m_setpoint;
+  private final ROBOT.SUPERSTRUCTURE_STATES m_stage;
+  private final Supplier<ROBOT.GAME_PIECE> m_selectedGamePiece;
 
-  public SetElevatorSetpoint(Elevator elevator, ELEVATOR_SETPOINT elevatorSetpoint) {
+  public SetElevatorSetpoint(
+      Elevator elevator,
+      ROBOT.SUPERSTRUCTURE_STATES stage,
+      Supplier<ROBOT.GAME_PIECE> selectedGamePiece) {
     m_elevator = elevator;
-    m_setpoint = elevatorSetpoint;
+    m_stage = stage;
+    m_selectedGamePiece = selectedGamePiece;
     addRequirements(m_elevator);
   }
 
+  /* This function is the main bulk of the algae toggle logic for elevator. */
   @Override
   public void initialize() {
     m_elevator.setClosedLoopControlMode(CONTROL_MODE.CLOSED_LOOP);
-    m_elevator.setDesiredPosition(
-        (m_setpoint.getSetpointMeters()) / ELEVATOR.sprocketRotationsToMeters);
+
+    /* If we want to be able to algae toggle in the middle of an outtake cycle, we need to put this in execute().
+    No idea why would you want to do that though */
+    switch (m_stage) {
+      case STOWED:
+        m_elevator.setDesiredPosition(
+            ELEVATOR.ELEVATOR_SETPOINT.START_POSITION.getSetpointMeters()
+                / ELEVATOR.drumRotationsToMeters);
+        break;
+      case L1:
+        if (m_selectedGamePiece.get() == ROBOT.GAME_PIECE.ALGAE)
+          m_elevator.setDesiredPosition(
+              ELEVATOR_SETPOINT.PROCESSOR.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
+        // No L1 elevator setpoint right now
+        break;
+      case L2:
+        if (m_selectedGamePiece.get() == ROBOT.GAME_PIECE.ALGAE) {
+          m_elevator.setDesiredPosition(
+              ELEVATOR_SETPOINT.ALGAE_REEF_INTAKE_LOWER.getSetpointMeters()
+                  / ELEVATOR.drumRotationsToMeters);
+        } else {
+          m_elevator.setDesiredPosition(
+              ELEVATOR_SETPOINT.LEVEL_2.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
+        }
+        break;
+      case L3:
+        if (m_selectedGamePiece.get() == ROBOT.GAME_PIECE.ALGAE) {
+          m_elevator.setDesiredPosition(
+              ELEVATOR_SETPOINT.ALGAE_REEF_INTAKE_UPPER.getSetpointMeters()
+                  / ELEVATOR.drumRotationsToMeters);
+        } else {
+          m_elevator.setDesiredPosition(
+              ELEVATOR_SETPOINT.LEVEL_3.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
+        }
+        break;
+      case L4:
+        if (m_selectedGamePiece.get() == ROBOT.GAME_PIECE.ALGAE) {
+          m_elevator.setDesiredPosition(
+              ELEVATOR_SETPOINT.NET.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
+        } else {
+          m_elevator.setDesiredPosition(
+              ELEVATOR_SETPOINT.LEVEL_4.getSetpointMeters() / ELEVATOR.drumRotationsToMeters);
+        }
+        break;
+      default:
+        break;
+    }
   }
 
   @Override

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

@@ -1,13 +1,14 @@
 package frc.robot.constants;
 
 import static edu.wpi.first.units.Units.*;
-import static edu.wpi.first.units.Units.Inches;
 
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Distance;
+import frc.robot.constants.ROBOT.GAME_PIECE;
+import frc.robot.constants.ROBOT.SUPERSTRUCTURE_STATES;
 
 public class ELEVATOR {
 
@@ -23,46 +24,62 @@ public class ELEVATOR {
   // public static final double kMaxVel = Units.inchesToMeters(10);
   // public static final double kMaxAccel = Units.inchesToMeters(18);
 
-  public static final double kP = 15.0; // these are extremely loosely tuned so tune them later
+  public static final double kP = 5.0; // these are extremely loosely tuned so tune them later
   public static final double kI = 0.0;
   public static final double kD = 0.0;
   public static final double kA = 0.2;
   public static final double kV = 0.1;
+  public static final double offset = 0.03;
   public static final double motionMagicCruiseVelocity = 100;
   public static final double motionMagicAcceleration = 200;
   public static final double motionMagicJerk = 4000;
   public static final double kPercentOutputMultiplier = 1.0;
   public static final double kLimitedPercentOutputMultiplier = 0.5;
-  public static final double sprocketRadiusMeters = Units.inchesToMeters(1.432) / 2.0;
-  public static final double sprocketRotationsToMeters =
-      sprocketRadiusMeters
-          * 2
+  public static final double kElevatorDrumDiameter = Units.inchesToMeters(2.2557);
+  public static final double drumRotationsToMeters =
+      kElevatorDrumDiameter
           * Math.PI; // Divide the setpoint in meters by this to get rotations. Vice versa to get
-  // meters
-  public static final double kElevatorGearing = 48.0 / 10;
-  public static final double kCarriageMassPounds = 15; // TODO: Change values after CAD done
-  public static final double gearRatio = 1.0 / 1.0; // TODO: Change values after CAD done
-  public static final double kElevatorDrumRadius = Units.inchesToMeters(1);
+  public static final double kCarriageMassPounds = 15.0; // TODO: Change values after CAD done
+  public static final double gearRatio = 48.0 / 10.0; // TODO: Change values after CAD done
+
   public static final DCMotor gearbox = DCMotor.getKrakenX60(2);
 
   public enum ELEVATOR_SETPOINT {
-    START_POSITION(Units.inchesToMeters(0.0)),
-    ALGAE_REEF_INTAKE_LOWER(Units.inchesToMeters(13.5)),
-    ALGAE_REEF_INTAKE_UPPER(Units.inchesToMeters(28)),
-    PROCESSOR(Units.inchesToMeters(10)),
-    LEVEL_2(Units.inchesToMeters(13)),
-    LEVEL_3(Units.inchesToMeters(27)),
-    LEVEL_4(Units.inchesToMeters(56.5)),
-    NET(Units.inchesToMeters(78));
+    START_POSITION(Units.inchesToMeters(0.0), SUPERSTRUCTURE_STATES.STOWED),
+    ALGAE_REEF_INTAKE_LOWER(Units.inchesToMeters(13.5), SUPERSTRUCTURE_STATES.L2, GAME_PIECE.ALGAE),
+    ALGAE_REEF_INTAKE_UPPER(Units.inchesToMeters(28), SUPERSTRUCTURE_STATES.L3, GAME_PIECE.ALGAE),
+    PROCESSOR(Units.inchesToMeters(10), SUPERSTRUCTURE_STATES.L1, GAME_PIECE.ALGAE),
+    LEVEL_2(Units.inchesToMeters(13), SUPERSTRUCTURE_STATES.L2, GAME_PIECE.CORAL),
+    LEVEL_3(Units.inchesToMeters(27), SUPERSTRUCTURE_STATES.L3, GAME_PIECE.CORAL),
+    LEVEL_4(Units.inchesToMeters(56.5), SUPERSTRUCTURE_STATES.L4, GAME_PIECE.CORAL),
+    NET(Units.inchesToMeters(78), SUPERSTRUCTURE_STATES.L4, GAME_PIECE.ALGAE);
+
     private final double setpointMeters;
+    private final SUPERSTRUCTURE_STATES superstructureState;
+    private final GAME_PIECE gamePiece;
 
-    ELEVATOR_SETPOINT(double setpointMeters) {
+    ELEVATOR_SETPOINT(
+        double setpointMeters, SUPERSTRUCTURE_STATES superstructureState, GAME_PIECE gamePiece) {
       this.setpointMeters = setpointMeters;
+      this.superstructureState = superstructureState;
+      this.gamePiece = gamePiece;
+    }
+
+    ELEVATOR_SETPOINT(double setpointMeters, SUPERSTRUCTURE_STATES superstructureState) {
+      this(setpointMeters, superstructureState, GAME_PIECE.NONE);
     }
 
     public double getSetpointMeters() {
       return setpointMeters;
     }
+
+    public SUPERSTRUCTURE_STATES getSuperstructureState() {
+      return superstructureState;
+    }
+
+    public GAME_PIECE getGamePiece() {
+      return gamePiece;
+    }
   }
 
   public enum ELEVATOR_ACCEL_SETPOINT {

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

@@ -20,6 +20,20 @@ public enum CONTROL_MODE {
     CLOSED_LOOP_NET
   }
 
+  public enum SUPERSTRUCTURE_STATES {
+    STOWED,
+    L1,
+    L2,
+    L3,
+    L4,
+  }
+
+  public enum GAME_PIECE {
+    CORAL,
+    ALGAE,
+    NONE
+  }
+
   public enum ROBOT_ID {
     // Robot Serial Numbers (2023-2024)
     // FORTE - 030cbc95