Elevator safety - max Chen

src/main/java/frc/robot/Robot.java

@@ -9,7 +9,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
@@ -19,11 +18,14 @@
 import frc.robot.commands.drive.DriveCommand;
 import frc.robot.commands.drive.FollowSwerveSampleCommand;
 import frc.robot.commands.elevator.ManualElevator;
+import frc.robot.commands.elevator.ZeroElevator;
+import frc.robot.commands.intake.Eject;
 import frc.robot.extras.util.AllianceFlipper;
 import frc.robot.extras.util.JoystickUtil;
 import frc.robot.sim.SimWorld;
 import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem;
 import frc.robot.subsystems.algaePivot.PhysicalAlgaePivot;
+import frc.robot.subsystems.elevator.ElevatorConstants;
 import frc.robot.subsystems.elevator.ElevatorInterface;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.elevator.PhysicalElevator;
@@ -57,7 +59,6 @@
  * project.
  */
 public class Robot extends LoggedRobot {
-
   private final VisionSubsystem visionSubsystem;
   private final SwerveDrive swerveDrive;
   private final ElevatorSubsystem elevatorSubsystem;
@@ -69,7 +70,7 @@ public class Robot extends LoggedRobot {
       new AlgaePivotSubsystem(new PhysicalAlgaePivot());
 
   private final SimWorld simWorld;
-
+  // private final ZeroElevator zeroElevator = new ZeroElevator();
   public AutoFactory autoFactory;
   public final AutoChooser autoChooser;
   public Autos autos;
@@ -170,7 +171,9 @@ public Robot() {
                 new ModuleInterface() {},
                 new ModuleInterface() {},
                 new ModuleInterface() {});
-        elevatorSubsystem = new ElevatorSubsystem(new ElevatorInterface() {});
+        elevatorSubsystem = new ElevatorSubsystem(new ElevatorInterface() {
+
+          });
         simWorld = null;
       }
     }
@@ -179,7 +182,7 @@ public Robot() {
     autoFactory =
         new AutoFactory(
             swerveDrive::getEstimatedPose, // A function that returns the current robot pose
-            swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to the
+            swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to
             (SwerveSample sample) -> {
               FollowSwerveSampleCommand followCommand =
                   new FollowSwerveSampleCommand(swerveDrive, visionSubsystem, sample);
@@ -307,17 +310,37 @@ private void configureDriverController() {
   }
 
   private void configureOperatorController() {
-    operatorController.b().whileTrue(Commands.none());
-    operatorController.y().whileTrue(Commands.none());
-    operatorController.x().whileTrue(Commands.none());
+    // operatorController.b().whileTrue(Commands.none());
+    // operatorController.y().whileTrue(Commands.none());
+    // operatorController.x().whileTrue(Commands.none());
     operatorController
         .a()
         .whileTrue(new ManualElevator(elevatorSubsystem, () -> operatorController.getLeftY()));
+    // Manual zero
+    operatorController.leftBumper().whileTrue(new ZeroElevator(elevatorSubsystem));
   }
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+    // Elevator Safety
+    if (Math.abs(swerveDrive.getRoll()) >= SwerveConstants.MAX_ROLL_ANGLE // gyro safety
+        || Math.abs(swerveDrive.getPitch())
+            >= SwerveConstants
+                .MAX_PITCH_ANGLE) 
+    {
+        new Eject(intakeSubsystem).schedule();
+        elevatorSubsystem.setElevatorPosition(0);
+
+
+    }
+    //extra elevator safety
+
+    //homing
+    elevatorSubsystem.homing();
+
+    }
+
 
   /** This function is called once when test mode is enabled. */
   @Override

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

@@ -8,12 +8,17 @@
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import java.util.function.DoubleSupplier;
 
-/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
 public class ManualElevator extends Command {
   ElevatorSubsystem elevatorSubsystem;
   DoubleSupplier joystickY;
 
-  /** Creates a new ManualElevator. */
+  /**
+   * Creates a new ManualElevator.
+   *
+   * @param elevatorSubsystem Elevator subsystem
+   * @param position Position in meters
+   * @param joystickY = Double Supplier for the joystick
+   */
   public ManualElevator(ElevatorSubsystem elevatorSubsystem, DoubleSupplier joystickY) {
     this.elevatorSubsystem = elevatorSubsystem;
     this.joystickY = joystickY;

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

@@ -12,7 +12,12 @@ public class SetElevatorPosition extends Command {
   ElevatorSubsystem elevatorSubsystem;
   double position;
 
-  /** Creates a new SetElevatorPosition. */
+  /**
+   * Creates a new SetElevatorPosition.
+   *
+   * @param elevatorSubsystem Elevator subsystem
+   * @param position Position in meters
+   */
   public SetElevatorPosition(ElevatorSubsystem elevatorSubsystem, double position) {
     this.elevatorSubsystem = elevatorSubsystem;
     this.position = position;

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

@@ -0,0 +1,42 @@
+// 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.elevator;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.elevator.ElevatorConstants;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+
+/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
+public class ZeroElevator extends Command {
+  /** Creates a new ZeroElevator. */
+  ElevatorSubsystem elevatorSubsystem;
+
+  public ZeroElevator(ElevatorSubsystem elevatorSubsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    this.elevatorSubsystem = elevatorSubsystem;
+    addRequirements(elevatorSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    elevatorSubsystem.setElevatorPosition(ElevatorConstants.MIN_HEIGHT); // zeroes it
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    elevatorSubsystem.setVolts(0); //stop it and stuff
+  }
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}

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

@@ -11,7 +11,7 @@ public class ElevatorConstants {
   public static final int ELEVATOR_LEADER_MOTOR_ID = 0;
   public static final int ELEVATOR_FOLLOWER_MOTOR_ID = 28;
 
-  public static final double ELEVATOR_P = 1;
+  public static final double ELEVATOR_P = 2;
   public static final double ELEVATOR_I = 0;
   public static final double ELEVATOR_D = 0;
   public static final double ELEVATOR_S = 0;
@@ -26,11 +26,13 @@ public class ElevatorConstants {
   public static final double MAX_HEIGHT = 3;
   public static final double INCLINE_ANGLE_RADIANS = Units.degreesToRadians(8);
   public static final boolean SIMULATE_GRAVITY = true;
-
+  //stator and supply
   public static final double STATOR_CURRENT_LIMIT = 0;
   public static final double SUPPLY_CURRENT_LIMIT = 0;
   public static final boolean STATOR_CURRENT_LIMIT_ENABLE = false;
   public static final boolean SUPPLY_CURRENT_LIMIT_ENABLE = false;
+  public static final double MAX_STATOR_THRESHOLD = 59343494;
+  public static final double MIN_STATOR_THRESHOLD = 59343494;
 
   public static final double MOTION_MAGIC_MAX_ACCELERATION = 2;
   public static final double MOTION_MAGIC_CRUISE_VELOCITY = 2;
@@ -40,4 +42,5 @@ public class ElevatorConstants {
   public static final boolean LIMIT_ENABLE = false;
   public static final double REVERSE_LIMIT = 0.15;
   public static final boolean REVRESE_LIMIT_ENABLE = false;
+
 }

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

@@ -18,7 +18,10 @@ public static class ElevatorInputs { // For values
     public double followerDutyCycle = 0.0;
     public double desiredPosition = 0.0;
   }
-
+
+  public default double elevatorStatorCurrent() {
+    return 0.0;
+  }
   public default void updateInputs(ElevatorInputs inputs) {}
 
   public default double getElevatorPosition() {
@@ -32,4 +35,5 @@ public default void setVolts(double volts) {}
   public default double getVolts() {
     return 0.0;
   }
+  public default void homing(){}
 }

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

@@ -31,6 +31,14 @@ public void setElevatorPosition(double position) {
   public void setVolts(double volts) {
     elevatorInterface.setVolts(volts);
   }
+
+  public double elevatorStatorCurrent() {
+    return elevatorInterface.elevatorStatorCurrent();
+  }
+
+  public void homing() {
+    elevatorInterface.homing();
+  }
 
   @Override
   public void periodic() {

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

@@ -4,6 +4,8 @@
 
 package frc.robot.subsystems.elevator;
 
+import static edu.wpi.first.units.Units.Rotations;
+
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
@@ -13,6 +15,7 @@
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
 import frc.robot.Constants.HardwareConstants;
 
@@ -30,6 +33,7 @@ public class PhysicalElevator implements ElevatorInterface {
   private final StatusSignal<Voltage> followerAppliedVoltage;
   private final StatusSignal<Double> followerDutyCycle;
   private final StatusSignal<Double> leaderDutyCycle;
+  private final StatusSignal<Current> elevatorStatorCurrent;
 
   private double desiredPosition;
 
@@ -87,7 +91,7 @@ public PhysicalElevator() {
     followerAppliedVoltage = followerMotor.getMotorVoltage();
     followerDutyCycle = followerMotor.getDutyCycle();
     leaderDutyCycle = leaderMotor.getDutyCycle();
-
+    elevatorStatorCurrent = leaderMotor.getStatorCurrent();
     BaseStatusSignal.setUpdateFrequencyForAll(
         HardwareConstants.STATUS_SIGNAL_FREQUENCY,
         leaderPosition,
@@ -98,6 +102,29 @@ public PhysicalElevator() {
         followerDutyCycle);
 
     desiredPosition = 0.0;
+  }
+  /**
+   * Homing to prevent stalling. If there is too much stator current, that shows stalling, so we just set that to the max/or min positions and stop the motor
+   * 
+   */
+  public void homing() {
+   if (leaderMotor.getStatorCurrent().getValueAsDouble() >= ElevatorConstants.MAX_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble() >= 0.0) { //below stator threshold AND going in positive direction (going down)
+      leaderMotor.setPosition(0);//set encoder back to 0
+      followerMotor.setPosition(0); // set encoder back to 0
+      //leaderMotor.setPosition(Rotations.of(0.0));
+      followerMotor.set(0);
+      leaderMotor.set(0);} 
+
+   else if (leaderMotor.getStatorCurrent().getValueAsDouble() <= ElevatorConstants.MIN_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble() <= 0.0) { //above stator threshold AND going in negative direction (going up)
+      leaderMotor.setPosition(ElevatorConstants.MAX_HEIGHT);//set encoder to max height
+      followerMotor.setPosition(ElevatorConstants.MAX_HEIGHT); // set encoder to max height
+      //leaderMotor.setPosition(Rotations.of(0.0));
+      followerMotor.set(0);
+      leaderMotor.set(0);
+    }
+
+
+
   }
 
   @Override
@@ -144,4 +171,5 @@ public void setVolts(double volts) {
   public double getVolts() {
     return leaderAppliedVoltage.getValueAsDouble();
   }
+
 }

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

@@ -77,4 +77,7 @@ public void setVolts(double volts) {
   public double getVolts() {
     return currentVolts;
   }
+
+
+
 }

src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java

@@ -181,4 +181,11 @@ public record ModuleConfig(
       SensorDirectionValue encoderReversed,
       InvertedValue turnReversed,
       InvertedValue driveReversed) {}
+
+
+  // The limits for the gyro
+  public static final double MAX_ROLL_ANGLE = 30; // degrees
+  public static final double MAX_PITCH_ANGLE = 20; // degrees
+  // public static final double MAX_ACCEL_X = 69;
+  // public static final double MAX_ACCEL_Y = 432423;
 }

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -309,6 +309,14 @@ public boolean getZeroedSpeeds(ChassisSpeeds speeds) {
   public double getHeading() {
     return gyroInputs.yawDegrees;
   }
+  /** Returns the pitch of the robot as double*/
+  public double getPitch() {
+    return gyroInputs.pitchDegrees;
+  }
+
+  public double getRoll() {
+    return gyroInputs.rollDegrees;
+  }
 
   /**
    * Gets the rate of rotation of the robot.

src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java

@@ -7,7 +7,8 @@ public interface GyroInterface {
   @AutoLog
   public static class GyroInputs {
     public boolean isConnected = false;
-
+    public double rollDegrees = 0.0;
+    public double pitchDegrees = 0.0;
     public double yawDegrees = 0.0;
     public double yawVelocityDegreesPerSecond = 0.0;
     public double accelX = 0.0;

src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java

@@ -20,6 +20,8 @@ public void updateInputs(GyroInputs inputs) {
     inputs.yawDegrees = -gyro.getAngle();
     inputs.accelX = -gyro.getWorldLinearAccelX();
     inputs.accelY = -gyro.getWorldLinearAccelY();
+    inputs.pitchDegrees = gyro.getPitch();
+    inputs.rollDegrees = gyro.getRoll();
   }
 
   @Override

src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java

@@ -24,6 +24,8 @@ public SimulatedGyro(SimGyro simGyro) {
   public void updateInputs(GyroInputs inputs) {
     inputs.isConnected = false;
     inputs.yawDegrees = -yawRads;
+    inputs.rollDegrees = 0.0;
+    inputs.pitchDegrees = 0.0;
     inputs.yawVelocityDegreesPerSecond = -yawVelRadsPerSec;
     inputs.accelX = accelX;
     inputs.accelY = accelY;