Elevator

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

@@ -8,6 +8,7 @@
 import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
 import static edu.wpi.first.units.Units.Pounds;
 import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.units.Units.Volts;
@@ -296,6 +297,46 @@ private enum Mk4iReductions {
         }
     }
 
+
+    /**
+     * Elevator Constants
+     */
+
+    public static final class Elevator {
+        public static final int RIGHT_ID = 7;
+        public static final int LEFT_ID = 4;
+        public static final int LIMIT_ID = 1;
+
+        public static final NeutralModeValue BREAK = NeutralModeValue.Brake;
+
+        // PID and feedforward
+        public static final double KP = 7.0;
+        public static final double KI = 2.0;
+        public static final double KD = 0.0;
+        public static final double KS = 0.1675;
+        public static final double KV = 0.0;
+        public static final double KA = 0.0;
+        public static final double KG = 0.3375;
+        public static final AngularVelocity MAX_VELOCITY = RotationsPerSecond.of(0.0);
+
+        // positions
+        public static final Distance HOME = Inches.of(2);
+        public static final Distance P0 = Inches.of(26); // Coral L2
+        public static final Distance P1 = Inches.of(16.5); // Algae L2-L3
+        public static final Distance P2 = Inches.of(43); // Coral L3
+        public static final Distance P3 = Inches.of(33); // Algae L3-L4
+        public static final Distance P4 = Inches.of(67); // Coral L4
+
+
+        public static final double gearRatio = 1.0;
+        public static final Distance INCHES_AT_TOP = Inches.of(72.0);
+        public static final Angle ROTATIONS_AT_TOP = Radians.of(220);
+        public static final double SensorToMechanismRatio =
+            Constants.Elevator.ROTATIONS_AT_TOP.in(Rotations)
+                / Constants.Elevator.INCHES_AT_TOP.in(Meters);
+    }
+
+
     /** Vision Constants */
     public static class Vision {
 
@@ -320,6 +361,7 @@ public double centerPixelYawRads() {
         public static final double fieldBorderMargin = 0.5;
     }
 
+
     /** State Estimator Constants */
     public static class StateEstimator {
         public static final boolean keepInField = true;
@@ -336,5 +378,6 @@ public static final class CoralScoringConstants {
         public static final int Coral_Touch_Sensor_DIO_Port = 3;
         public static final int Random_Touch_Sensor = 1;
     }
-
 }
+
+

src/main/java/frc/robot/Main.java

@@ -16,7 +16,7 @@ private Main() {}
 
     /**
      * Main initialization function. Do not perform any initialization here.
-     *
+     * 
      * <p>
      * If you change your main robot class, change the parameter type.
      *

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

@@ -7,8 +7,8 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -24,6 +24,9 @@
 import frc.robot.subsystems.coral.CoralScoring;
 import frc.robot.subsystems.coral.CoralScoringIO;
 import frc.robot.subsystems.coral.CoralScoringReal;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.elevator.ElevatorIO;
+import frc.robot.subsystems.elevator.ElevatorReal;
 import frc.robot.subsystems.elevator_algae.ElevatorAlgae;
 import frc.robot.subsystems.elevator_algae.ElevatorAlgaeReal;
 import frc.robot.subsystems.swerve.Swerve;
@@ -62,9 +65,11 @@ public class RobotContainer {
     /** State */
     private final RobotState state;
 
+
     /* Subsystems */
     private ElevatorAlgae s_ElevatorAlgae;
     private LEDs leds = new LEDs();
+    private Elevator elevator;
     private final Swerve s_Swerve;
     private final Vision s_Vision;
     private CoralScoring coralScoring;
@@ -83,6 +88,7 @@ public RobotContainer(RobotRunType runtimeType) {
         state = new RobotState(vis);
         switch (runtimeType) {
             case kReal:
+                elevator = new Elevator(new ElevatorReal());
                 s_Swerve = new Swerve(state, new SwerveReal());
                 s_Vision = new Vision(state, VisionReal::new);
                 coralScoring = new CoralScoring(new CoralScoringReal());
@@ -95,20 +101,24 @@ public RobotContainer(RobotRunType runtimeType) {
                 SimulatedArena.getInstance().addDriveTrainSimulation(driveSimulation);
                 s_Swerve = new Swerve(state, new SwerveSim(driveSimulation));
                 s_Vision = new Vision(state, VisionSimPhoton.partial(driveSimulation));
+                elevator = new Elevator(new ElevatorIO() {});
                 coralScoring = new CoralScoring(new CoralScoringIO() {});
                 climb = new Climber(new ClimberIO.Empty());
                 break;
             default:
+                elevator = new Elevator(new ElevatorIO() {});
                 s_Swerve = new Swerve(state, new SwerveIO.Empty() {});
                 s_Vision = new Vision(state, VisionIO::empty);
                 coralScoring = new CoralScoring(new CoralScoringIO() {});
                 climb = new Climber(new ClimberIO.Empty());
         }
+
         /* Default Commands */
         s_Swerve.setDefaultCommand(s_Swerve.teleOpDrive(driver, Constants.Swerve.isFieldRelative,
             Constants.Swerve.isOpenLoop));
         leds.setDefaultCommand(leds.setLEDsBreathe(Color.kRed).ignoringDisable(true));
         /* Button and Trigger Bindings */
+
         configureButtonBindings(runtimeType);
         configureTriggerBindings();
     }
@@ -117,60 +127,32 @@ public RobotContainer(RobotRunType runtimeType) {
      * Use this method to vol your button->command mappings. Buttons can be created by instantiating
      * a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or
      * {@link XboxController}), and then passing it to a
-     * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     * {@link edu1.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings(RobotRunType runtimeType) {
         algaeInIntake.onTrue(leds.blinkLEDs(Color.kCyan));
-        operator.rightTrigger()
+        driver.rightTrigger()
             .whileTrue(s_ElevatorAlgae.setMotorVoltageCommand(Constants.Algae.VOLTAGE));
-        operator.leftTrigger()
+        driver.leftTrigger()
             .whileTrue(s_ElevatorAlgae.setMotorVoltageCommand(Constants.Algae.NEGATIVE_VOLTAGE));
 
 
         driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));
-        driver.a().onTrue(new Command() {
-            Timer timer = new Timer();
-
-            @Override
-            public void initialize() {
-                timer.reset();
-                timer.start();
-            }
-
-            @Override
-            public void execute() {
-                vis.setElevatorHeight(Inches.of(timer.get() * 30.0));
-            }
-
-            @Override
-            public boolean isFinished() {
-                return timer.hasElapsed(3.0);
-            }
-        });
-        driver.b().onTrue(new Command() {
-            Timer timer = new Timer();
-
-            @Override
-            public void initialize() {
-                timer.reset();
-                timer.start();
-            }
-
-            @Override
-            public void execute() {
-                vis.setElevatorHeight(Inches.of(Math.max(72.0 - timer.get() * 30.0, 0)));
-            }
-
-            @Override
-            public boolean isFinished() {
-                return timer.hasElapsed(3.0);
-            }
-        });
-
-        driver.x().onTrue(new InstantCommand(() -> {
-            s_Swerve.resetOdometry(new Pose2d(7.24, 4.05, Rotation2d.kZero));
-        }));
-        operator.x().whileTrue(coralScoring.runScoringMotor(2));
+
+        driver.povDown().onTrue(elevator.home());
+        driver.povLeft().onTrue(elevator.p0());
+        SmartDashboard.putNumber("elevatorVoltage", 1.0);
+        SmartDashboard.putNumber("elevatorTargetHeight", 20);
+        driver.a().whileTrue(
+            elevator.moveTo(() -> Inches.of(SmartDashboard.getNumber("elevatorTargetHeight", 20))));
+        driver.povUp().whileTrue(elevator.moveUp());
+        driver.povRight().whileTrue(elevator.moveDown());
+
+
+        // driver.x().onTrue(new InstantCommand(() -> {
+        // s_Swerve.resetOdometry(new Pose2d(7.24, 4.05, Rotation2d.kZero));
+        // }));
+        driver.x().whileTrue(coralScoring.runScoringMotor(2));
         driver.rightStick().whileTrue(climb.runClimberMotorCommand());
         controllerThree.y().whileTrue(climb.resetClimberCommand());
 
@@ -226,5 +208,6 @@ public void updateSimulation() {
 
         }
     }
-
 }
+
+

src/main/java/frc/robot/subsystems/coral/CoralScoring.java

@@ -30,10 +30,6 @@ public boolean getGrabingRightBeamBrakeStatus() {
         return coralScoringAutoLogged.grabingBeamBrakeRight;
     }
 
-    public boolean getRandomStatus() {
-        return coralScoringAutoLogged.randomTouchSensor;
-    }
-
     @Override
     public void periodic() {
         io.updateInputs(coralScoringAutoLogged);

src/main/java/frc/robot/subsystems/coral/CoralScoringIO.java

@@ -18,7 +18,6 @@ public static class CoralScoringInputs {
         public double scoringRPM;
         public boolean scoringBeamBrake;
         public boolean grabingBeamBrakeRight;
-        public boolean randomTouchSensor;
     }
 
     public default void updateInputs(CoralScoringInputs inputs) {}

src/main/java/frc/robot/subsystems/coral/CoralScoringReal.java

@@ -23,8 +23,6 @@ public class CoralScoringReal implements CoralScoringIO {
     private final DigitalInput coralTouchSensor =
         new DigitalInput(Constants.CoralScoringConstants.Coral_Touch_Sensor_DIO_Port);
     SparkMaxConfig motorConfig = new SparkMaxConfig();
-    private final DigitalInput randomTouchSensor =
-        new DigitalInput(Constants.CoralScoringConstants.Random_Touch_Sensor);
 
     /**
      * Coral Scoring Real
@@ -40,7 +38,6 @@ public CoralScoringReal() {
      */
 
     public void updateInputs(CoralScoringInputs inputs) {
-        inputs.randomTouchSensor = randomTouchSensor.get();
         inputs.scoringBeamBrake = !scoringBeamBrake.get();
         inputs.grabingBeamBrakeRight = coralTouchSensor.get();
     }

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

@@ -0,0 +1,110 @@
+package frc.robot.subsystems.elevator;
+
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * Elevator Subsystem
+ */
+public class Elevator extends SubsystemBase {
+    ElevatorIO io;
+    Timer time = new Timer();
+    private ElevatorInputsAutoLogged inputs = new ElevatorInputsAutoLogged();
+
+    public Elevator(ElevatorIO io) {
+        this.io = io;
+        io.updateInputs(inputs);
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Elevator", inputs);
+        Logger.recordOutput("Elevator/position_in", inputs.position.in(Inches));
+        if (inputs.limitSwitch) {
+            io.resetHome();
+        }
+    }
+
+    /**
+     * moves elevator to home with time out
+     *
+     * @return home position
+     */
+    public Command homeTimer() {
+        Command slowLower = Commands.run(() -> io.setVoltage(-0.1)).withTimeout(1);
+        return moveTo(() -> Constants.Elevator.HOME).until(() -> inputs.position.in(Inches) < 5.0)
+            .andThen(slowLower);
+    }
+
+    /**
+     * moves elevator to home
+     *
+     * @return elevator at home
+     * 
+     */
+    public Command home() {
+
+        Command slowLower = Commands.runEnd(() -> io.setVoltage(-0.7), () -> io.setVoltage(0.0));
+        return moveTo(() -> Constants.Elevator.HOME).until(() -> inputs.position.in(Inches) < 5.0)
+            .andThen(slowLower).until(() -> (inputs.limitSwitch == true));
+    }
+
+    /**
+     * moves elevator to l2
+     *
+     * @return elevator at l2
+     * 
+     */
+    public Command p0() {
+        return moveTo(() -> Constants.Elevator.P0);
+    }
+
+    public Command p1() {
+        return moveTo(() -> Constants.Elevator.P1);
+    }
+
+    public Command p2() {
+        return moveTo(() -> Constants.Elevator.P2);
+    }
+
+    public Command p3() {
+        return moveTo(() -> Constants.Elevator.P3);
+    }
+
+    public Command p4() {
+        return moveTo(() -> Constants.Elevator.P4);
+    }
+
+    /**
+     * sets height of elevator
+     *
+     * @param height desired height of elevator
+     * @return elevator height change
+     * 
+     */
+    public Command moveTo(Supplier<Distance> height) {
+        return run(() -> {
+            Logger.recordOutput("targetHeight", height.get().in(Meters));
+            io.setPositon(height.get().in(Meters));
+        }).until(() -> Math.abs(inputs.position.in(Inches) - height.get().in(Inches)) < 1);
+    }
+
+    public Command moveUp() {
+        return runEnd(() -> io.setVoltage(SmartDashboard.getNumber("elevatorVoltage", 1.0)),
+            () -> io.setVoltage(0));
+    }
+
+    public Command moveDown() {
+        return runEnd(() -> io.setVoltage(-1.0), () -> io.setVoltage(0));
+    }
+}