From 68ec3d03190670395c7b975834c6a7be1342b472 Mon Sep 17 00:00:00 2001 From: mychenezpz Date: Sat, 15 Feb 2025 14:49:06 -0500 Subject: [PATCH 1/8] Some elevator safeties --- src/main/java/frc/robot/Robot.java | 52 ++++++++++++------- .../commands/elevator/ManualElevator.java | 18 +++++-- .../elevator/SetElevatorPosition.java | 16 ++++-- .../robot/commands/elevator/ZeroElevator.java | 41 +++++++++++++++ .../elevator/ElevatorConstants.java | 8 +++ .../robot/subsystems/swerve/SwerveDrive.java | 7 ++- .../subsystems/swerve/gyro/GyroInterface.java | 3 +- .../subsystems/swerve/gyro/PhysicalGyro.java | 2 + .../subsystems/swerve/gyro/SimulatedGyro.java | 2 + 9 files changed, 121 insertions(+), 28 deletions(-) create mode 100644 src/main/java/frc/robot/commands/elevator/ZeroElevator.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 6e7d4846..1191bd06 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,11 +19,13 @@ 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.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; @@ -176,20 +177,20 @@ public Robot() { } autoChooser = new AutoChooser(); // this sets up the auto factory - 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 - (SwerveSample sample) -> { - FollowSwerveSampleCommand followCommand = - new FollowSwerveSampleCommand(swerveDrive, visionSubsystem, sample); - followCommand.execute(); - if (swerveDrive.isTrajectoryFinished(sample)) { - followCommand.cancel(); - } - }, // A function that follows a choreo trajectory - AllianceFlipper.isRed(), // If alliance flipping should be enabled - swerveDrive); // The drive subsystem + // 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 + // (SwerveSample sample) -> { + // FollowSwerveSampleCommand followCommand = + // new FollowSwerveSampleCommand(swerveDrive, visionSubsystem, sample); + // followCommand.execute(); + // if (swerveDrive.isTrajectoryFinished(sample)) { + // followCommand.cancel(); + // } + // }, // A function that follows a choreo trajectory + // AllianceFlipper.isRed(), // If alliance flipping should be enabled + // swerveDrive); // The drive subsystem autos = new Autos(autoFactory); @@ -208,7 +209,7 @@ public Robot() { public void robotPeriodic() { // Switch thread to high priority to improve loop timing Threads.setCurrentThreadPriority(true, 99); - + // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled commands, running already-scheduled commands, removing // finished or interrupted commands, and running subsystem periodic() methods. @@ -313,11 +314,26 @@ private void configureOperatorController() { 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 (SwerveDrive.getRoll() >= ElevatorConstants.MAX_ANGLE_X //gyro safety + || SwerveDrive.getRoll() <= ElevatorConstants.MIN_ANGLE_X + || SwerveDrive.getPitch() >= ElevatorConstants.MAX_ANGLE_Y + || SwerveDrive.getPitch() <= ElevatorConstants.MIN_ANGLE_Y + || SwerveDrive.accelX() >= ElevatorConstants.MAX_ACCEL_X //accel safety + || SwerveDrive.accelY() >= ElevatorConstants.MAX_ACCEL_Y) //TODO if robot is not in climbing state, then do this (AND logic) + { + elevatorSubsystem.setDefaultCommand(new ZeroElevator(elevatorSubsystem));//maybe works + } + + + } /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/frc/robot/commands/elevator/ManualElevator.java b/src/main/java/frc/robot/commands/elevator/ManualElevator.java index 4a9f5867..9a9e9f6d 100644 --- a/src/main/java/frc/robot/commands/elevator/ManualElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ManualElevator.java @@ -5,16 +5,24 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorConstants; 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. */ - public ManualElevator(ElevatorSubsystem elevatorSubsystem, DoubleSupplier joystickY) { + /** + * 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; @@ -30,8 +38,8 @@ public void initialize() {} @Override public void execute() { elevatorSubsystem.setElevatorPosition(joystickY.getAsDouble()); - } - + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java b/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java index 320637c8..e20c2f15 100644 --- a/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java +++ b/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java @@ -5,15 +5,23 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.ElevatorConstants; import frc.robot.subsystems.elevator.ElevatorSubsystem; +import frc.robot.subsystems.swerve.gyro.GyroInterface; /* 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 SetElevatorPosition extends Command { ElevatorSubsystem elevatorSubsystem; double position; - /** Creates a new SetElevatorPosition. */ - public SetElevatorPosition(ElevatorSubsystem elevatorSubsystem, double position) { + /** + * Creates a new SetElevatorPosition. + * + * @param elevatorSubsystem Elevator subsystem + * @param position Position in meters + * + */ + public SetElevatorPosition(ElevatorSubsystem elevatorSubsystem, double position, ADIS16448 imu) { this.elevatorSubsystem = elevatorSubsystem; this.position = position; @@ -29,7 +37,9 @@ public void initialize() {} @Override public void execute() { elevatorSubsystem.setElevatorPosition(position); - } + + } + // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java new file mode 100644 index 00000000..d08c909b --- /dev/null +++ b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java @@ -0,0 +1,41 @@ +// 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) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 40dbb0ce..9256ba6c 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -40,4 +40,12 @@ 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; + + // The limits for the imu + public static final double MAX_ANGLE_X = 59; // degrees + public static final double MIN_ANGLE_X = 69; // degrees + public static final double MAX_ANGLE_Y = 69; // degrees + public static final double MIN_ANGLE_Y = 420; // degrees + public static final double MAX_ACCEL_X = 69; + public static final double MAX_ACCEL_Y = 432423; } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index f76379ab..87a09393 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -309,7 +309,12 @@ public boolean getZeroedSpeeds(ChassisSpeeds speeds) { public double getHeading() { return gyroInputs.yawDegrees; } - + public double getPitch() { + return gyroInputs.pitchDegrees; + } + public double getRoll() { + return gyroInputs.rollDegrees; + } /** * Gets the rate of rotation of the robot. * diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java index 4ffef1f6..6d8794c8 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/GyroInterface.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java index 129384c0..6bfcab6e 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/PhysicalGyro.java +++ b/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 diff --git a/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java b/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java index cc47b016..afaf640c 100644 --- a/src/main/java/frc/robot/subsystems/swerve/gyro/SimulatedGyro.java +++ b/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; From 2c3d07b8a213c0be78cd5df2406a6b136c0288ab Mon Sep 17 00:00:00 2001 From: mychenezpz Date: Sat, 15 Feb 2025 17:01:46 -0500 Subject: [PATCH 2/8] Work pls --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 34 ++++++++----------- .../commands/elevator/ManualElevator.java | 9 ++--- .../elevator/SetElevatorPosition.java | 9 ++--- .../elevator/ElevatorConstants.java | 4 +-- .../robot/subsystems/swerve/SwerveDrive.java | 3 ++ 6 files changed, 26 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4493d75a..9c142a1f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,7 @@ public static final class LogPaths { public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/"; } - public static final RobotType ROBOT_TYPE = RobotType.SIM_ROBOT; + public static final RobotType ROBOT_TYPE = RobotType.SIM_ROBOT_ROBOT; public static enum RobotType { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1191bd06..41a0b076 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,14 +2,12 @@ import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; -import choreo.trajectory.SwerveSample; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Threads; 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; @@ -17,10 +15,8 @@ import frc.robot.Constants.FieldConstants; import frc.robot.commands.autodrive.AutoAlign; 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.extras.util.AllianceFlipper; import frc.robot.extras.util.JoystickUtil; import frc.robot.sim.SimWorld; import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem; @@ -180,7 +176,8 @@ 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 + // the // (SwerveSample sample) -> { // FollowSwerveSampleCommand followCommand = // new FollowSwerveSampleCommand(swerveDrive, visionSubsystem, sample); @@ -209,7 +206,7 @@ public Robot() { public void robotPeriodic() { // Switch thread to high priority to improve loop timing Threads.setCurrentThreadPriority(true, 99); - + // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled commands, running already-scheduled commands, removing // finished or interrupted commands, and running subsystem periodic() methods. @@ -308,9 +305,9 @@ 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())); @@ -321,18 +318,17 @@ private void configureOperatorController() { /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { - //Elevator Safety - if (SwerveDrive.getRoll() >= ElevatorConstants.MAX_ANGLE_X //gyro safety - || SwerveDrive.getRoll() <= ElevatorConstants.MIN_ANGLE_X - || SwerveDrive.getPitch() >= ElevatorConstants.MAX_ANGLE_Y - || SwerveDrive.getPitch() <= ElevatorConstants.MIN_ANGLE_Y - || SwerveDrive.accelX() >= ElevatorConstants.MAX_ACCEL_X //accel safety - || SwerveDrive.accelY() >= ElevatorConstants.MAX_ACCEL_Y) //TODO if robot is not in climbing state, then do this (AND logic) + // Elevator Safety + if (swerveDrive.getRoll() >= ElevatorConstants.MAX_ANGLE_X // gyro safety + || swerveDrive.getRoll() <= ElevatorConstants.MIN_ANGLE_X + || swerveDrive.getPitch() >= ElevatorConstants.MAX_ANGLE_Y + || swerveDrive.getPitch() + <= ElevatorConstants + .MIN_ANGLE_Y) // TODO if robot is not in climbing state, then do this (AND logic) { - elevatorSubsystem.setDefaultCommand(new ZeroElevator(elevatorSubsystem));//maybe works + // elevatorSubsystem.setDefaultCommand(new ZeroElevator(elevatorSubsystem)); // maybe works + elevatorSubsystem.setElevatorPosition(0); } - - } /** This function is called once when test mode is enabled. */ diff --git a/src/main/java/frc/robot/commands/elevator/ManualElevator.java b/src/main/java/frc/robot/commands/elevator/ManualElevator.java index 9a9e9f6d..2714ea03 100644 --- a/src/main/java/frc/robot/commands/elevator/ManualElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ManualElevator.java @@ -5,7 +5,6 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.ElevatorConstants; import frc.robot.subsystems.elevator.ElevatorSubsystem; import java.util.function.DoubleSupplier; @@ -18,11 +17,9 @@ public class ManualElevator extends Command { * * @param elevatorSubsystem Elevator subsystem * @param position Position in meters - * * @param joystickY = Double Supplier for the joystick */ - public ManualElevator( - ElevatorSubsystem elevatorSubsystem, DoubleSupplier joystickY) { + public ManualElevator(ElevatorSubsystem elevatorSubsystem, DoubleSupplier joystickY) { this.elevatorSubsystem = elevatorSubsystem; this.joystickY = joystickY; @@ -38,8 +35,8 @@ public void initialize() {} @Override public void execute() { elevatorSubsystem.setElevatorPosition(joystickY.getAsDouble()); - } - + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java b/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java index e20c2f15..71a99940 100644 --- a/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java +++ b/src/main/java/frc/robot/commands/elevator/SetElevatorPosition.java @@ -5,9 +5,7 @@ package frc.robot.commands.elevator; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.elevator.ElevatorConstants; import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.swerve.gyro.GyroInterface; /* 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 SetElevatorPosition extends Command { @@ -19,9 +17,8 @@ public class SetElevatorPosition extends Command { * * @param elevatorSubsystem Elevator subsystem * @param position Position in meters - * */ - public SetElevatorPosition(ElevatorSubsystem elevatorSubsystem, double position, ADIS16448 imu) { + public SetElevatorPosition(ElevatorSubsystem elevatorSubsystem, double position) { this.elevatorSubsystem = elevatorSubsystem; this.position = position; @@ -37,9 +34,7 @@ public void initialize() {} @Override public void execute() { elevatorSubsystem.setElevatorPosition(position); - - } - + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 9256ba6c..3db3102d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -46,6 +46,6 @@ public class ElevatorConstants { public static final double MIN_ANGLE_X = 69; // degrees public static final double MAX_ANGLE_Y = 69; // degrees public static final double MIN_ANGLE_Y = 420; // degrees - public static final double MAX_ACCEL_X = 69; - public static final double MAX_ACCEL_Y = 432423; + public static final double MAX_ACCEL_X = 69; + public static final double MAX_ACCEL_Y = 432423; } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 87a09393..3eb8cbcc 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -309,12 +309,15 @@ public boolean getZeroedSpeeds(ChassisSpeeds speeds) { public double getHeading() { return gyroInputs.yawDegrees; } + public double getPitch() { return gyroInputs.pitchDegrees; } + public double getRoll() { return gyroInputs.rollDegrees; } + /** * Gets the rate of rotation of the robot. * From 731a1ac7e8279fdaee94819a7fa12a2baf53c5c5 Mon Sep 17 00:00:00 2001 From: Ishan1522 Date: Sat, 15 Feb 2025 19:23:57 -0500 Subject: [PATCH 3/8] Im too tired for this bro --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/Robot.java | 43 ++++++++++--------- .../elevator/ElevatorConstants.java | 14 +++--- .../elevator/SimulatedElevator.java | 2 +- 4 files changed, 30 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9c142a1f..4493d75a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,7 @@ public static final class LogPaths { public static final String APRIL_TAGS_VISION_PATH = "Vision/AprilTags/"; } - public static final RobotType ROBOT_TYPE = RobotType.SIM_ROBOT_ROBOT; + public static final RobotType ROBOT_TYPE = RobotType.SIM_ROBOT; public static enum RobotType { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 41a0b076..faebdb59 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,6 +2,7 @@ import choreo.auto.AutoChooser; import choreo.auto.AutoFactory; +import choreo.trajectory.SwerveSample; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.Threads; @@ -15,8 +16,10 @@ import frc.robot.Constants.FieldConstants; import frc.robot.commands.autodrive.AutoAlign; 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.extras.util.AllianceFlipper; import frc.robot.extras.util.JoystickUtil; import frc.robot.sim.SimWorld; import frc.robot.subsystems.algaePivot.AlgaePivotSubsystem; @@ -173,21 +176,20 @@ public Robot() { } autoChooser = new AutoChooser(); // this sets up the auto factory - // 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 - // (SwerveSample sample) -> { - // FollowSwerveSampleCommand followCommand = - // new FollowSwerveSampleCommand(swerveDrive, visionSubsystem, sample); - // followCommand.execute(); - // if (swerveDrive.isTrajectoryFinished(sample)) { - // followCommand.cancel(); - // } - // }, // A function that follows a choreo trajectory - // AllianceFlipper.isRed(), // If alliance flipping should be enabled - // swerveDrive); // The drive subsystem + autoFactory = + new AutoFactory( + swerveDrive::getEstimatedPose, // A function that returns the current robot pose + swerveDrive::resetEstimatedPose, // A function that resets the current robot pose to + (SwerveSample sample) -> { + FollowSwerveSampleCommand followCommand = + new FollowSwerveSampleCommand(swerveDrive, visionSubsystem, sample); + followCommand.execute(); + if (swerveDrive.isTrajectoryFinished(sample)) { + followCommand.cancel(); + } + }, // A function that follows a choreo trajectory + AllianceFlipper.isRed(), // If alliance flipping should be enabled + swerveDrive); // The drive subsystem autos = new Autos(autoFactory); @@ -319,12 +321,11 @@ private void configureOperatorController() { @Override public void teleopPeriodic() { // Elevator Safety - if (swerveDrive.getRoll() >= ElevatorConstants.MAX_ANGLE_X // gyro safety - || swerveDrive.getRoll() <= ElevatorConstants.MIN_ANGLE_X - || swerveDrive.getPitch() >= ElevatorConstants.MAX_ANGLE_Y - || swerveDrive.getPitch() - <= ElevatorConstants - .MIN_ANGLE_Y) // TODO if robot is not in climbing state, then do this (AND logic) + if (Math.abs(swerveDrive.getRoll()) >= ElevatorConstants.MAX_ROLL_ANGLE // gyro safety + || Math.abs(swerveDrive.getPitch()) + >= ElevatorConstants + .MAX_PITCH_ANGLE) // TODO if robot is not in climbing state, then do this (AND + // logic) { // elevatorSubsystem.setDefaultCommand(new ZeroElevator(elevatorSubsystem)); // maybe works elevatorSubsystem.setElevatorPosition(0); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 3db3102d..bbc514e9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/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; @@ -41,11 +41,9 @@ public class ElevatorConstants { public static final double REVERSE_LIMIT = 0.15; public static final boolean REVRESE_LIMIT_ENABLE = false; - // The limits for the imu - public static final double MAX_ANGLE_X = 59; // degrees - public static final double MIN_ANGLE_X = 69; // degrees - public static final double MAX_ANGLE_Y = 69; // degrees - public static final double MIN_ANGLE_Y = 420; // degrees - public static final double MAX_ACCEL_X = 69; - public static final double MAX_ACCEL_Y = 432423; + // 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; } diff --git a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java index f7fbae2f..2ec526f9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java @@ -77,4 +77,4 @@ public void setVolts(double volts) { public double getVolts() { return currentVolts; } -} +} \ No newline at end of file From ef5d23eb66a146d7230862996e4e99b9e1cfdba3 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 18 Feb 2025 01:57:56 +0000 Subject: [PATCH 4/8] Formatting fixes --- .../java/frc/robot/subsystems/elevator/SimulatedElevator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java index 2ec526f9..f7fbae2f 100644 --- a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java @@ -77,4 +77,4 @@ public void setVolts(double volts) { public double getVolts() { return currentVolts; } -} \ No newline at end of file +} From ade8c2c29dbf5a1aacec731bf59fe85abf2c808e Mon Sep 17 00:00:00 2001 From: mychenezpz Date: Sat, 22 Feb 2025 21:11:36 -0500 Subject: [PATCH 5/8] More elevavator sasdfasfety afsoijsaoijf --- src/main/java/frc/robot/Robot.java | 11 +++++--- .../elevator/ElevatorConstants.java | 4 ++- .../elevator/ElevatorInterface.java | 5 +++- .../elevator/ElevatorSubsystem.java | 4 +++ .../subsystems/elevator/PhysicalElevator.java | 25 ++++++++++++++++++- 5 files changed, 43 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index faebdb59..994b5154 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -324,13 +324,18 @@ public void teleopPeriodic() { if (Math.abs(swerveDrive.getRoll()) >= ElevatorConstants.MAX_ROLL_ANGLE // gyro safety || Math.abs(swerveDrive.getPitch()) >= ElevatorConstants - .MAX_PITCH_ANGLE) // TODO if robot is not in climbing state, then do this (AND - // logic) + .MAX_PITCH_ANGLE) { // elevatorSubsystem.setDefaultCommand(new ZeroElevator(elevatorSubsystem)); // maybe works elevatorSubsystem.setElevatorPosition(0); } - } + //extra elevator safety + + //homing + + + } + /** This function is called once when test mode is enabled. */ @Override diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index bbc514e9..df14fedd 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -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; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java index 24faf1a0..07a8cb30 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java +++ b/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() { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 7bc5838c..741cfdfa 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -31,6 +31,10 @@ public void setElevatorPosition(double position) { public void setVolts(double volts) { elevatorInterface.setVolts(volts); } + + public double elevatorStatorCurrent() { + return elevatorInterface.elevatorStatorCurrent(); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java index ff3eae30..247201a9 100644 --- a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java @@ -13,6 +13,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 +31,7 @@ public class PhysicalElevator implements ElevatorInterface { private final StatusSignal followerAppliedVoltage; private final StatusSignal followerDutyCycle; private final StatusSignal leaderDutyCycle; + private final StatusSignal elevatorStatorCurrent; private double desiredPosition; @@ -87,7 +89,7 @@ public PhysicalElevator() { followerAppliedVoltage = followerMotor.getMotorVoltage(); followerDutyCycle = followerMotor.getDutyCycle(); leaderDutyCycle = leaderMotor.getDutyCycle(); - + elevatorStatorCurrent = leaderMotor.getStatorCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( HardwareConstants.STATUS_SIGNAL_FREQUENCY, leaderPosition, @@ -100,6 +102,27 @@ public PhysicalElevator() { desiredPosition = 0.0; } + public void homing() { + if (leaderMotor.getStatorCurrent().getValueAsDouble() >= ElevatorConstants.MAX_STATOR_THRESHOLD) { + setElevatorPosition(-ElevatorConstants.MIN_HEIGHT); + setElevatorPosition(ElevatorConstants.MIN_HEIGHT); + followerMotor.set(0); + leaderMotor.set(0); + //zero encoder positions + leaderMotor.setPosition(0); + followerMotor.setPosition(0); } + } + public void setElevatorHeights() { + if (leaderMotor.getStatorCurrent().getValueAsDouble() >= ElevatorConstants.MAX_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble()>= 0.1) { + leaderMotor.setPosition(-ElevatorConstants.MAX_HEIGHT); + followerMotor.setPosition(ElevatorConstants.MAX_HEIGHT); //IDK PROB DOESNT WORK JESUS FIX IT} + else if (leaderMotor.getStatorCurrent().getValueAsDouble() <= ElevatorConstants.MIN_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble()<= -0.1) { + leaderMotor.setPosition(-ElevatorConstants.MIN_HEIGHT); + followerMotor.setPosition(ElevatorConstants.MIN_HEIGHT); + } + + } + @Override public void updateInputs(ElevatorInputs inputs) { BaseStatusSignal.refreshAll( From 9e56f81bec7cd7c2622f30911fc7434b52262100 Mon Sep 17 00:00:00 2001 From: Abigail-27 Date: Mon, 17 Mar 2025 17:08:38 -0400 Subject: [PATCH 6/8] Fixed some elevator sfaety issues --- src/main/java/frc/robot/Robot.java | 7 +++---- .../java/frc/robot/commands/elevator/ZeroElevator.java | 5 +++-- .../frc/robot/subsystems/elevator/ElevatorConstants.java | 5 ----- .../frc/robot/subsystems/elevator/PhysicalElevator.java | 4 ++-- .../java/frc/robot/subsystems/swerve/SwerveConstants.java | 7 +++++++ 5 files changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 994b5154..bdf4d9d2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -321,13 +321,12 @@ private void configureOperatorController() { @Override public void teleopPeriodic() { // Elevator Safety - if (Math.abs(swerveDrive.getRoll()) >= ElevatorConstants.MAX_ROLL_ANGLE // gyro safety + if (Math.abs(swerveDrive.getRoll()) >= SwerveConstants.MAX_ROLL_ANGLE // gyro safety || Math.abs(swerveDrive.getPitch()) - >= ElevatorConstants + >= SwerveConstants .MAX_PITCH_ANGLE) { - // elevatorSubsystem.setDefaultCommand(new ZeroElevator(elevatorSubsystem)); // maybe works - elevatorSubsystem.setElevatorPosition(0); + elevatorSubsystem.setElevatorPosition(0); } //extra elevator safety diff --git a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java index d08c909b..9545d4b7 100644 --- a/src/main/java/frc/robot/commands/elevator/ZeroElevator.java +++ b/src/main/java/frc/robot/commands/elevator/ZeroElevator.java @@ -31,8 +31,9 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} - + public void end(boolean interrupted) { + elevatorSubsystem.setVolts(0); //stop it and stuff + } // Returns true when the command should end. @Override public boolean isFinished() { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index df14fedd..9a3bcdfc 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -43,9 +43,4 @@ public class ElevatorConstants { public static final double REVERSE_LIMIT = 0.15; public static final boolean REVRESE_LIMIT_ENABLE = false; - // 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; } diff --git a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java index 247201a9..89276846 100644 --- a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java @@ -115,8 +115,8 @@ public void homing() { public void setElevatorHeights() { if (leaderMotor.getStatorCurrent().getValueAsDouble() >= ElevatorConstants.MAX_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble()>= 0.1) { leaderMotor.setPosition(-ElevatorConstants.MAX_HEIGHT); - followerMotor.setPosition(ElevatorConstants.MAX_HEIGHT); //IDK PROB DOESNT WORK JESUS FIX IT} - else if (leaderMotor.getStatorCurrent().getValueAsDouble() <= ElevatorConstants.MIN_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble()<= -0.1) { + followerMotor.setPosition(ElevatorConstants.MAX_HEIGHT); //IDK PROB DOESNT WORK JESUS FIX IT + } else if (leaderMotor.getStatorCurrent().getValueAsDouble() <= ElevatorConstants.MIN_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble()<= -0.1) { leaderMotor.setPosition(-ElevatorConstants.MIN_HEIGHT); followerMotor.setPosition(ElevatorConstants.MIN_HEIGHT); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java index 294fdfc1..b25b3963 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveConstants.java +++ b/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; } From 84529b614bc45bfea78142bccbbd97be4e3ffa93 Mon Sep 17 00:00:00 2001 From: Abigail-27 Date: Mon, 17 Mar 2025 18:13:38 -0400 Subject: [PATCH 7/8] 3/17 Completed homing, added to teleop periodic --- src/main/java/frc/robot/Robot.java | 6 ++- .../elevator/ElevatorInterface.java | 1 + .../elevator/ElevatorSubsystem.java | 4 ++ .../subsystems/elevator/PhysicalElevator.java | 37 +++++++++++-------- .../elevator/SimulatedElevator.java | 3 ++ 5 files changed, 33 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bdf4d9d2..3b32231f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -170,7 +170,9 @@ public Robot() { new ModuleInterface() {}, new ModuleInterface() {}, new ModuleInterface() {}); - elevatorSubsystem = new ElevatorSubsystem(new ElevatorInterface() {}); + elevatorSubsystem = new ElevatorSubsystem(new ElevatorInterface() { + + }); simWorld = null; } } @@ -331,7 +333,7 @@ public void teleopPeriodic() { //extra elevator safety //homing - + elevatorSubsystem.homing(); } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java index 07a8cb30..0358770f 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorInterface.java @@ -35,4 +35,5 @@ public default void setVolts(double volts) {} public default double getVolts() { return 0.0; } + public default void homing(){} } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java index 741cfdfa..6723e7a2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java @@ -35,6 +35,10 @@ public void setVolts(double volts) { public double elevatorStatorCurrent() { return elevatorInterface.elevatorStatorCurrent(); } + + public void homing() { + elevatorInterface.homing(); + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java b/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java index 89276846..6a90b3ea 100644 --- a/src/main/java/frc/robot/subsystems/elevator/PhysicalElevator.java +++ b/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; @@ -101,25 +103,27 @@ public PhysicalElevator() { 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) { - setElevatorPosition(-ElevatorConstants.MIN_HEIGHT); - setElevatorPosition(ElevatorConstants.MIN_HEIGHT); + 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); - //zero encoder positions - leaderMotor.setPosition(0); - followerMotor.setPosition(0); } - } - public void setElevatorHeights() { - if (leaderMotor.getStatorCurrent().getValueAsDouble() >= ElevatorConstants.MAX_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble()>= 0.1) { - leaderMotor.setPosition(-ElevatorConstants.MAX_HEIGHT); - followerMotor.setPosition(ElevatorConstants.MAX_HEIGHT); //IDK PROB DOESNT WORK JESUS FIX IT - } else if (leaderMotor.getStatorCurrent().getValueAsDouble() <= ElevatorConstants.MIN_STATOR_THRESHOLD && leaderMotor.getVelocity().getValueAsDouble()<= -0.1) { - leaderMotor.setPosition(-ElevatorConstants.MIN_HEIGHT); - followerMotor.setPosition(ElevatorConstants.MIN_HEIGHT); - } + } + + } @@ -167,4 +171,5 @@ public void setVolts(double volts) { public double getVolts() { return leaderAppliedVoltage.getValueAsDouble(); } + } diff --git a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java index f7fbae2f..b7805ee2 100644 --- a/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/SimulatedElevator.java @@ -77,4 +77,7 @@ public void setVolts(double volts) { public double getVolts() { return currentVolts; } + + + } From 0016bddf56a44a04fd9ccb970c54b0e45cf678db Mon Sep 17 00:00:00 2001 From: mychenezpz Date: Sun, 30 Mar 2025 11:17:48 -0400 Subject: [PATCH 8/8] eject when elevatorsafety running --- src/main/java/frc/robot/Robot.java | 6 +++++- src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3b32231f..20db0397 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,6 +19,7 @@ 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; @@ -328,10 +329,13 @@ public void teleopPeriodic() { >= SwerveConstants .MAX_PITCH_ANGLE) { + new Eject(intakeSubsystem).schedule(); elevatorSubsystem.setElevatorPosition(0); + + } //extra elevator safety - + //homing elevatorSubsystem.homing(); diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java index 3eb8cbcc..930d376f 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java @@ -309,7 +309,7 @@ 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; }