From 2d9d4ba5a646204af1592168360b7cbdf331ecb9 Mon Sep 17 00:00:00 2001 From: Maryam Date: Wed, 8 Oct 2025 16:55:02 -0700 Subject: [PATCH] new code cuz i lost the other one. dunno if it works --- .../ArmToAngleAndSpinFlywheel.java | 2 +- .../ElevatorCommands/DefaultElevator.java | 2 +- .../ElevatorCommands/ElevatorHoldL4.java | 2 +- .../ElevatorCommands/SetElevatorLevel.java | 4 +- .../SetElevatorLevelInstant.java | 2 +- .../ZeroElevatorHardStop.java | 2 +- .../ZeroElevatorToFFiltered.java | 2 +- .../robot/subsystems/ElevatorSubsystem.java | 285 ++++-------------- 8 files changed, 74 insertions(+), 227 deletions(-) diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java index a5d410af..cc628d81 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java @@ -34,7 +34,7 @@ public void execute() { @Override public void end(boolean interrupted) { armPlusFlywheel.stopFlywheel(); // Stop the flywheel - elevatorSubsystem.elevateTo(ElevatorPositions.safePosition); + elevatorSubsystem.setHeight(ElevatorPositions.safePosition); } @Override diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java b/src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java index 73e1b905..05ab307f 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java @@ -41,7 +41,7 @@ public void execute() { // } if (!CoralPosition.isCoralInTootsieSlide()) { - elevatorSubsystem.elevateTo(ElevatorPositions.Intake); + elevatorSubsystem.setHeight(ElevatorPositions.Intake); } } diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java index f61019cc..e484115f 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java @@ -15,7 +15,7 @@ public ElevatorHoldL4(ElevatorSubsystem elevatorSubsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - elevatorSubsystem.elevateTo(ElevatorPositions.LIMIT_OF_TRAVEL); + elevatorSubsystem.setHeight(ElevatorPositions.LIMIT_OF_TRAVEL); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java index 88a4e8de..2a6c1c34 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevel.java @@ -25,10 +25,10 @@ public void initialize() {} public void execute() { if (checkIfCoralInTootsie) { if (CoralPosition.isCoralInTootsieSlide()) { - elevatorSubsystem.elevateTo(pos); + elevatorSubsystem.setHeight(pos); } } else { - elevatorSubsystem.elevateTo(pos); + elevatorSubsystem.setHeight(pos); } } diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevelInstant.java b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevelInstant.java index f8257efe..b13ab6f5 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevelInstant.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/SetElevatorLevelInstant.java @@ -19,7 +19,7 @@ public void initialize() {} @Override public void execute() { - elevatorSubsystem.elevateTo(pos); + elevatorSubsystem.setHeight(pos); } @Override diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java index c19e9668..4b2d0669 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java @@ -34,7 +34,7 @@ public void execute() { public void end(boolean interrupted) { DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false); if (!interrupted) { - elevatorSubsystem.resetElevatorPositionToZero(); + elevatorSubsystem.zeroElevator(); } elevatorSubsystem.resetCurrentLimits(); elevatorSubsystem.elevatorHasBeenZeroed(); diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java index 4b265808..d30a3998 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java @@ -19,7 +19,7 @@ public ZeroElevatorToFFiltered(ElevatorSubsystem subsystem) { public void initialize() { ticksAtPosition = 0; DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", true); - elevatorSubsystem.elevateTo(ElevatorPositions.Intake); + elevatorSubsystem.setHeight(ElevatorPositions.Intake); } @Override diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a00efcb2..fdc7ecf1 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -5,7 +5,6 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfigurator; @@ -14,9 +13,6 @@ import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANrange; -import com.ctre.phoenix6.signals.GravityTypeValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import dev.doglog.DogLog; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,109 +21,69 @@ import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.util.LoggedTalonFX; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; - public class ElevatorSubsystem extends SubsystemBase { private static ElevatorSubsystem instance; private LoggedTalonFX motor1; private LoggedTalonFX motor2; public LoggedTalonFX master; - + private CANrange distance; private LinearFilter elevatorFilter; - private double currentHeightToF; - private boolean elevatorZeroed; - private MotionMagicConfigs mmc; - private ElevatorPositions currentLevel; - private CANrange distance; // Time of Flight (ToF) sensor + private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(null); + private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(null); + private final VelocityVoltage velocityRequest = new VelocityVoltage(null); - private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); - private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); - private final VelocityVoltage velocityRequest = new VelocityVoltage(0); - - private ElevatorSubsystem() { - // Initialize motors - elevatorZeroed = false; - elevatorFilter = LinearFilter.singlePoleIIR(0.1, 0.02); + private ElevatorPositions currLevel; + private double tolerance = 3.0; + private boolean elevatorZeroed; + private double currentHeightToF; + public ElevatorSubsystem() { + motor1 = new LoggedTalonFX(ElevatorConstants.MOTOR1_PORT); + motor2 = new LoggedTalonFX(ElevatorConstants.MOTOR2_PORT); distance = new CANrange( ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + elevatorZeroed = false; + elevatorFilter = LinearFilter.singlePoleIIR(0.1, 0.02); - motor1 = - new LoggedTalonFX( - "subsystems/Elevator/motor1", - ElevatorConstants.MOTOR1_PORT, - Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - motor2 = - new LoggedTalonFX( - "subsystems/Elevator/motor2", - ElevatorConstants.MOTOR2_PORT, - Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - - currentLevel = ElevatorPositions.Intake; - - // Set up motor followers and deal with inverted motors + master = motor1; Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); motor2.setControl(follower); - Slot1Configs s1c = - new Slot1Configs() - .withKP(ElevatorConstants.S1C_KP) - .withKI(ElevatorConstants.S1C_KI) - .withKD(ElevatorConstants.S1C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + TalonFXConfigurator m1Config = motor1.getConfigurator(); Slot0Configs s0c = new Slot0Configs() .withKP(ElevatorConstants.S0C_KP) .withKI(ElevatorConstants.S0C_KI) - .withKD(ElevatorConstants.S0C_KD) + .withKD(ElevatorConstants.S0C_KD); + + Slot1Configs s1c = + new Slot1Configs() + .withKP(ElevatorConstants.S1C_KP) + .withKI(ElevatorConstants.S1C_KI) + .withKD(ElevatorConstants.S1C_KD) .withKS(ElevatorConstants.S0C_KS) .withKG(ElevatorConstants.S0C_KG) .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + .withKV(ElevatorConstants.S0C_KV); motor1.updateCurrentLimits( ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - motor2.updateCurrentLimits( - ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - TalonFXConfigurator m1Config = motor1.getConfigurator(); - TalonFXConfigurator m2Config = motor2.getConfigurator(); + MotionMagicConfigs mmc = + new MotionMagicConfigs() + .withMotionMagicAcceleration(ElevatorConstants.ACCELERATION) + .withMotionMagicCruiseVelocity(ElevatorConstants.CRUISE_VELOCITY); m1Config.apply(s0c); - m2Config.apply(s0c); - m1Config.apply(s1c); - m2Config.apply(s1c); - - MotorOutputConfigs moc = new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); - - // Apply MotionMagic to motors - mmc = new MotionMagicConfigs(); - mmc.MotionMagicCruiseVelocity = ElevatorConstants.MOTIONMAGIC_MAX_VELOCITY; - mmc.MotionMagicAcceleration = ElevatorConstants.MOTIONMAGIC_MAX_ACCELERATION; - m1Config.apply(mmc); - m2Config.apply(mmc); - - m1Config.apply(moc); - m2Config.apply(moc); - master = motor1; currentHeightToF = elevatorFilter.calculate(getToFDistance()); - resetPositionFiltered(); } - // instance for elevator subsystem public static ElevatorSubsystem getInstance() { if (instance == null) { instance = new ElevatorSubsystem(); @@ -135,42 +91,43 @@ public static ElevatorSubsystem getInstance() { return instance; } - public boolean tofIsConnected() { - return distance.isConnected(); + public void setHeight(ElevatorPositions level) { + currLevel = level; + master.setControl(controlRequest.withPosition(level.height)); } - public void resetPositionFiltered() { - master.setPosition( - currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - DogLog.log( - "subsystems/Elevator/resetElevatorPosition", - currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + public ElevatorPositions getLevel() { + return currLevel; } - public void resetPosition() { - if (tofIsConnected()) { - master.setPosition( - this.getToFDistance() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - DogLog.log( - "subsystems/Elevator/resetElevatorPosition", - this.getToFDistance() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - } + public double getErrorDist() { + return ((currLevel.height * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS) + / ElevatorConstants.CARRAIGE_UPDUCTION) + - master.getPosition().getValueAsDouble(); } - public void resetPosition(double posInHeight) { - // TODO: add constant to convert distance to encoder values - master.setPosition( - posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); - DogLog.log( - "subsystems/Elevator/resetElevatorPosition", - posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + public boolean isAtPosition() { + return (Math.abs(getErrorDist())) <= tolerance; } - // Hardstop Zeroing functions: - public void moveElevatorNegative() { - master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); + public boolean atIntake() { + return currLevel.equals(ElevatorPositions.Intake); + } + + public void zeroElevator() { + master.setPosition(0); + } + + public void elevatorHasBeenZeroed() { + elevatorZeroed = true; + } + + public boolean isElevatorZeroed() { + return elevatorZeroed; + } + + public void ElevatorTorqueMode() { + master.setControl(torqueRequest.withOutput(ElevatorConstants.ELEVATOR_TORQUE)); } public void reduceCurrentLimits() { @@ -179,151 +136,41 @@ public void reduceCurrentLimits() { public void resetCurrentLimits() { master.updateCurrentLimits( - Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, - Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); } - public void resetElevatorPositionToZero() { - master.setPosition(0); - // master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0)); - // master.setPosition(0); - // master.setControl(controlRequest.withPosition(0).withSlot(0)); - // master.setPosition(0); + public void moveElevatorNegative() { + master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); } public boolean checkCurrent() { double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); - if (Supplycurrent > 1.0 && Statorcurrent > 20) { return true; } return false; } - public double getError() { - return currentLevel.height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / Constants.ElevatorConstants.CARRAIGE_UPDUCTION - - master.getPosition().getValueAsDouble(); - } - - public ElevatorPositions getLevel() { - return currentLevel; - } - - public boolean atIntake() { - return currentLevel.equals(ElevatorPositions.Intake); - } - - public void elevateTo(ElevatorPositions level) { - this.currentLevel = level; - this.setPosition(level.height); - } - - public void setPosition(double height) { - master.setControl( - controlRequest - .withPosition( - height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / ElevatorConstants.CARRAIGE_UPDUCTION) - .withSlot(0)); - DogLog.log( - "subsystems/Elevator/elevatorSetpoint(rot)", - height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / ElevatorConstants.CARRAIGE_UPDUCTION); - } - - public void ElevatorTorqueMode() { - DogLog.log("subsystems/Elevator/usingTorqueMode", true); - master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); - // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); - } - - // TODO: ONLY FOR DEBUGGING - public void testElevator(double height) { - this.setPosition(height); - } - - public boolean isAtPosition() { - return (Math.abs(getError()) <= ElevatorConstants.SETPOINT_TOLERANCE); - } - - public boolean canFunnelTransferCoralToScoring() { - return this.getLevel().equals(Constants.ElevatorConstants.ElevatorPositions.Intake) - && this.getError() < Constants.ElevatorConstants.MAX_POSITIONAL_ERROR; + public void resetPositionFiltered() { + master.setPosition( + currentHeightToF * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } public double getToFDistance() { // 0.11 is the sensor offset - DogLog.log( - "subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET; } - public boolean isElevatorZeroed() { - return elevatorZeroed; - } - - public void elevatorHasBeenZeroed() { - elevatorZeroed = true; - } - @Override public void periodic() { - currentHeightToF = elevatorFilter.calculate(getToFDistance()); - // Time of Flight Sensor - DogLog.log("subsystems/Elevator/getError", getError()); - DogLog.log("subsystems/Elevator/ToF/Distance", getToFDistance()); - DogLog.log("subsystems/Elevator/ToF/Connected", distance.isConnected()); - DogLog.log("subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); - - DogLog.log("subsystems/Elevator/isAtPosition", this.isAtPosition()); - DogLog.log("subsystems/Elevator/targetPosition", currentLevel.getPosition()); - DogLog.log("subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); - DogLog.log( - "subsystems/Elevator/targetHeightRot", - currentLevel.getHeight() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log( - "subsystems/Elevator/currentHeightDist", - master.getPosition().getValueAsDouble() - * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_ROTATIONS_TO_DISTANCE - * Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log("subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); - DogLog.log( - "subsystems/Elevator/command", - this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); - DogLog.log( - "subsystems/Elevator/resetPositionBoolean", - this.isAtPosition() && this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log( - "subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log("subsystems/Elevator/targetLevel", this.getLevel().toString()); - DogLog.log( - "subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); - DogLog.log( - "subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); + // This method will be called once per scheduler run + DogLog.log("Current angle", getLevel()); + DogLog.log("Is at target", isAtPosition()); } @Override public void simulationPeriodic() { - // Simulate encoder behavior based on motor speed - double simulatedSpeed = master.getVelocity().getValueAsDouble(); - double currentPosition = master.getPosition().getValueAsDouble(); - - // Update simulated position based on speed (simplified example) - double newPosition = currentPosition + simulatedSpeed * 0.02; // Assuming a 20ms loop - master.setPosition( - newPosition); // Alarming to have this since running this on the robot will lead to - - // Log simulation data for debugging - DogLog.log("Simulated Position", newPosition); - DogLog.log("Simulated Speed", simulatedSpeed); + // This method will be called once per scheduler run during simulation } }