From f05be896ee8c0c1b499e9d96912133a44dfe4457 Mon Sep 17 00:00:00 2001 From: arj2cool Date: Mon, 6 Oct 2025 20:16:11 -0700 Subject: [PATCH 1/5] my branch --- .../pathplanner/ElevatorSubsystem2.java | 138 ++++++++++++++++++ 1 file changed, 138 insertions(+) create mode 100644 src/main/deploy/pathplanner/ElevatorSubsystem2.java diff --git a/src/main/deploy/pathplanner/ElevatorSubsystem2.java b/src/main/deploy/pathplanner/ElevatorSubsystem2.java new file mode 100644 index 00000000..5c5194ed --- /dev/null +++ b/src/main/deploy/pathplanner/ElevatorSubsystem2.java @@ -0,0 +1,138 @@ +// 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.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; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +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; +import frc.robot.Constants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.ElevatorConstants.ElevatorPositions; +import frc.robot.util.LoggedTalonFX; + +public class ElevatorSubsystem2 extends SubsystemBase { + private LoggedTalonFX motor1; //initializing motors, setting master variable & target height + private LoggedTalonFX motor2; + public LoggedTalonFX master; + public double targetHeight; + + private MotionMagicConfigs mmc; //motion magic & control request + + private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); + + public ElevatorSubsystem() { + motor1 = //init motors + 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); + + Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); //set follower to motor 2 + motor2.setControl(follower); + + //PID configs, slot 0 & 1 + 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); + + Slot0Configs s0c = + new Slot0Configs() + .withKP(ElevatorConstants.S0C_KP) + .withKI(ElevatorConstants.S0C_KI) + .withKD(ElevatorConstants.S0C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + //current limits + motor1.updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + motor2.updateCurrentLimits( + ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + + //apply configs + TalonFXConfigurator m1Config = motor1.getConfigurator(); + TalonFXConfigurator m2Config = motor2.getConfigurator(); + + 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); + + //set master to motor1 + master = motor1; + } + + public void setPosition(double height) { + if (height < Constants.ElevatorConstants.minHeight) height = Constants.ElevatorConstants.minHeight; //min height of elevator to min height in constants + if (height > Constants.ElevatorConstants.maxHeight) height = Constants.ElevatorConstants.maxHeight; //max height of elevator to max height in constants + + + //control request + master.setControl( + controlRequest + .withPosition( + height + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION) + .withSlot(0)); + } + + public void resetElevatorPositionToZero() { //self explanatory + master.setPosition(0); + } + + public double getHeight() { //self explanatory + return master.getRotorPosition().getValueAsDouble()* ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS/ ElevatorConstants.CARRAIGE_UPDUCTION; + } + + public void periodic() { //logs + DogLog.log( + "subsystems/Elevator/currentheight", + getHeight()); + } +} \ No newline at end of file From 812ee5d7fefa5691a582a19c8fff7e0ed6b8032b Mon Sep 17 00:00:00 2001 From: arj2cool Date: Wed, 8 Oct 2025 18:18:11 -0700 Subject: [PATCH 2/5] Added needed methods --- .../pathplanner/ElevatorSubsystem2.java | 138 -------- src/main/java/frc/robot/Robot.java | 4 +- .../ArmToAngleAndSpinFlywheel.java | 1 - .../SwerveCommands/ApplySwerveVoltage.java | 1 - .../SwerveCommands/JamesHardenMovement.java | 22 +- .../frc/robot/subsystems/AnthonyVision.java | 10 +- .../robot/subsystems/ElevatorSubsystem.java | 245 +++---------- .../subsystems/ElevatorSubsystemOld.java | 325 ++++++++++++++++++ .../frc/robot/subsystems/SwerveSubsystem.java | 7 +- 9 files changed, 392 insertions(+), 361 deletions(-) delete mode 100644 src/main/deploy/pathplanner/ElevatorSubsystem2.java create mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java diff --git a/src/main/deploy/pathplanner/ElevatorSubsystem2.java b/src/main/deploy/pathplanner/ElevatorSubsystem2.java deleted file mode 100644 index 5c5194ed..00000000 --- a/src/main/deploy/pathplanner/ElevatorSubsystem2.java +++ /dev/null @@ -1,138 +0,0 @@ -// 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.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; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -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; -import frc.robot.Constants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.ElevatorConstants.ElevatorPositions; -import frc.robot.util.LoggedTalonFX; - -public class ElevatorSubsystem2 extends SubsystemBase { - private LoggedTalonFX motor1; //initializing motors, setting master variable & target height - private LoggedTalonFX motor2; - public LoggedTalonFX master; - public double targetHeight; - - private MotionMagicConfigs mmc; //motion magic & control request - - private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); - - public ElevatorSubsystem() { - motor1 = //init motors - 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); - - Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); //set follower to motor 2 - motor2.setControl(follower); - - //PID configs, slot 0 & 1 - 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); - - Slot0Configs s0c = - new Slot0Configs() - .withKP(ElevatorConstants.S0C_KP) - .withKI(ElevatorConstants.S0C_KI) - .withKD(ElevatorConstants.S0C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - //current limits - motor1.updateCurrentLimits( - ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - motor2.updateCurrentLimits( - ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); - - //apply configs - TalonFXConfigurator m1Config = motor1.getConfigurator(); - TalonFXConfigurator m2Config = motor2.getConfigurator(); - - 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); - - //set master to motor1 - master = motor1; - } - - public void setPosition(double height) { - if (height < Constants.ElevatorConstants.minHeight) height = Constants.ElevatorConstants.minHeight; //min height of elevator to min height in constants - if (height > Constants.ElevatorConstants.maxHeight) height = Constants.ElevatorConstants.maxHeight; //max height of elevator to max height in constants - - - //control request - master.setControl( - controlRequest - .withPosition( - height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / ElevatorConstants.CARRAIGE_UPDUCTION) - .withSlot(0)); - } - - public void resetElevatorPositionToZero() { //self explanatory - master.setPosition(0); - } - - public double getHeight() { //self explanatory - return master.getRotorPosition().getValueAsDouble()* ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS/ ElevatorConstants.CARRAIGE_UPDUCTION; - } - - public void periodic() { //logs - DogLog.log( - "subsystems/Elevator/currentheight", - getHeight()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9ec8beb7..0abcf2c2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -73,7 +73,6 @@ public void robotPeriodic() { visionRight.addFilteredPose(); visionLeft.addFilteredPose(); - DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel()); DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide()); } @@ -173,8 +172,7 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} @Override public void testInit() { diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java index 539c037b..a3401471 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java @@ -1,7 +1,6 @@ // TODO: THIS COMMAND NEED TO BE UPDATED TO WORK PROPERLY package frc.robot.commands.DaleCommands; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.ArmSubsystem; diff --git a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java index 3e0870ce..a11cdcf5 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java @@ -1,7 +1,6 @@ package frc.robot.commands.SwerveCommands; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.SwerveSubsystem; diff --git a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java index 93a12da4..2da6a0dd 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java @@ -102,16 +102,26 @@ public void execute() { DogLog.log("Commands/JamesHarden/TargetPose", targetPose); DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseX(m)", targetPose.getX()); DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseY(m)", targetPose.getY()); - DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseHeading(deg)", targetPose.getRotation().getRadians()); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/TargetPoseHeading(deg)", + targetPose.getRotation().getRadians()); - DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsX(mps)", speeds.vxMetersPerSecond); - DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsY(mps)", speeds.vyMetersPerSecond); - DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsTheta(radps)", speeds.omegaRadiansPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsX(mps)", + speeds.vxMetersPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsY(mps)", + speeds.vyMetersPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsTheta(radps)", + speeds.omegaRadiansPerSecond); DogLog.log( - "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)", swerve.getFieldSpeeds().vxMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)", + swerve.getFieldSpeeds().vxMetersPerSecond); DogLog.log( - "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)", swerve.getFieldSpeeds().vyMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)", + swerve.getFieldSpeeds().vyMetersPerSecond); DogLog.log( "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(radps)", swerve.getFieldSpeeds().omegaRadiansPerSecond); diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 877a8646..77f0105e 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -40,7 +40,7 @@ public class AnthonyVision extends SubsystemBase { // Data type is "Cameras", an enum defined in Constants.java with only two options (left, right) private final Constants.Vision.Cameras cameraId; - private String camTitle; + private String camTitle; // Reef tag IDs for each side of the field private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); @@ -182,7 +182,7 @@ public void addFilteredPose() { DogLog.log("Vision/" + camTitle + "/ValidTags", false); return; } - DogLog.log("Vision/" + camTitle + "/ValidTags", true); + DogLog.log("Vision/" + camTitle + "/ValidTags", true); // Log all tags that haven't been thrown out int tagCount = validTags.size(); @@ -306,7 +306,8 @@ private double computeNoiseXY( double tagFactor = 1.0 / Math.sqrt(effectiveTags); // Distance term (keep as d^2) - double distanceFactor = baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance); + double distanceFactor = + baseNoise + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance); // Speed term (quadratic, saturated) double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; @@ -315,12 +316,11 @@ private double computeNoiseXY( DogLog.log("Vision/tagFactor", tagFactor); DogLog.log("Vision/distanceFactor", distanceFactor); DogLog.log("Vision/speedFactor", speedFactor); - + double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; return computedStdDevs; } - private double computeNoiseHeading( double baseNoise, double distanceCoefficient, diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a9595664..284dbdaa 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -11,51 +11,29 @@ import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; -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; import frc.robot.Constants; import frc.robot.Constants.ElevatorConstants; -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 motor1; // initializing motors, setting master variable & target height private LoggedTalonFX motor2; public LoggedTalonFX master; + public double targetHeight; - private LinearFilter elevatorFilter; - private double currentHeightToF; - private boolean elevatorZeroed; - - private MotionMagicConfigs mmc; - private ElevatorPositions currentLevel; - private CANrange distance; // Time of Flight (ToF) sensor + private MotionMagicConfigs mmc; // motion magic & control request 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); - distance = - new CANrange( - ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + public static ElevatorSubsystem instance; - motor1 = + public ElevatorSubsystem() { + motor1 = // init motors new LoggedTalonFX( "subsystems/Elevator/motor1", ElevatorConstants.MOTOR1_PORT, @@ -66,12 +44,11 @@ private ElevatorSubsystem() { ElevatorConstants.MOTOR2_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - currentLevel = ElevatorPositions.Intake; - - // Set up motor followers and deal with inverted motors - Follower follower = new Follower(ElevatorConstants.MOTOR1_PORT, false); + Follower follower = + new Follower(ElevatorConstants.MOTOR1_PORT, false); // set follower to motor 2 motor2.setControl(follower); + // PID configs, slot 0 & 1 Slot1Configs s1c = new Slot1Configs() .withKP(ElevatorConstants.S1C_KP) @@ -96,11 +73,13 @@ private ElevatorSubsystem() { .withGravityType(GravityTypeValue.Elevator_Static) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // current limits motor1.updateCurrentLimits( ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); motor2.updateCurrentLimits( ElevatorConstants.STATOR_CURRENT_LIMIT, ElevatorConstants.SUPPLY_CURRENT_LIMIT); + // apply configs TalonFXConfigurator m1Config = motor1.getConfigurator(); TalonFXConfigurator m2Config = motor2.getConfigurator(); @@ -122,108 +101,21 @@ private ElevatorSubsystem() { m1Config.apply(moc); m2Config.apply(moc); + // set master to motor1 master = motor1; - currentHeightToF = elevatorFilter.calculate(getToFDistance()); - resetPositionFiltered(); - } - - // instance for elevator subsystem - public static ElevatorSubsystem getInstance() { - if (instance == null) { - instance = new ElevatorSubsystem(); - } - return instance; - } - - public boolean tofIsConnected() { - return distance.isConnected(); - } - - 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 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 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); - } - - // Hardstop Zeroing functions: - public void moveElevatorNegative() { - master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); - } - - public void reduceCurrentLimits() { - master.updateCurrentLimits(30, 10); - } - - public void resetCurrentLimits() { - master.updateCurrentLimits( - Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, - Constants.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 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) { + // if (height < Constants.ElevatorConstants.minHeight) + // height = + // Constants.ElevatorConstants + // .minHeight; // min height of elevator to min height in constants + // if (height > Constants.ElevatorConstants.maxHeight) + // height = + // Constants.ElevatorConstants + // .maxHeight; // max height of elevator to max height in constants + + // control request master.setControl( controlRequest .withPosition( @@ -231,95 +123,38 @@ public void setPosition(double 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)); + public void resetElevatorPositionToZero() { // self explanatory + master.setPosition(0); } - // TODO: ONLY FOR DEBUGGING - public void testElevator(double height) { - this.setPosition(height); + public double getHeight() { // self explanatory + return master.getRotorPosition().getValueAsDouble() + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION; } - public boolean isAtPosition() { - return (Math.abs(getError()) <= ElevatorConstants.SETPOINT_TOLERANCE); + public void periodic() { // logs + DogLog.log("subsystems/Elevator/currentheight", getHeight()); } - public boolean canFunnelTransferCoralToScoring() { - return this.getLevel().equals(Constants.ElevatorConstants.ElevatorPositions.Intake) - && this.getError() < Constants.ElevatorConstants.MAX_POSITIONAL_ERROR; + public boolean isAtPosition() { + return Math.abs(getHeight()-targetHeight) <= 0.05; } - 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 atIntake() { + return (Math.abs(getHeight() - ElevatorConstants.ElevatorPositions.Intake.getHeight()) <= 0.05); } 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()); + return Math.abs(getHeight()) <= 0.05; } - @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 + public static ElevatorSubsystem getInstance() { + if (instance==null) { + instance = new ElevatorSubsystem(); + } + return instance; } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java new file mode 100644 index 00000000..8d294d68 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java @@ -0,0 +1,325 @@ +// 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.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; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +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; +import frc.robot.Constants; +import frc.robot.Constants.ElevatorConstants; +import frc.robot.Constants.ElevatorConstants.ElevatorPositions; +import frc.robot.util.LoggedTalonFX; + +// import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ElevatorSubsystemOld extends SubsystemBase { + private static ElevatorSubsystem instance; + + private LoggedTalonFX motor1; + private LoggedTalonFX motor2; + public LoggedTalonFX master; + + 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(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); + + distance = + new CANrange( + ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + + 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 + 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); + + Slot0Configs s0c = + new Slot0Configs() + .withKP(ElevatorConstants.S0C_KP) + .withKI(ElevatorConstants.S0C_KI) + .withKD(ElevatorConstants.S0C_KD) + .withKS(ElevatorConstants.S0C_KS) + .withKG(ElevatorConstants.S0C_KG) + .withKA(ElevatorConstants.S0C_KA) + .withKV(ElevatorConstants.S0C_KV) + .withGravityType(GravityTypeValue.Elevator_Static) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + + 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(); + + 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(); + } + return instance; + } + + public boolean tofIsConnected() { + return distance.isConnected(); + } + + 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 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 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); + } + + // Hardstop Zeroing functions: + public void moveElevatorNegative() { + master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); + } + + public void reduceCurrentLimits() { + master.updateCurrentLimits(30, 10); + } + + public void resetCurrentLimits() { + master.updateCurrentLimits( + Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, + Constants.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 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 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()); + } + + @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 + } +} diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 51f1f4ff..25f88f6e 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -97,8 +97,11 @@ public SwerveSubsystem( qProfiledPIDController.setIZone(Constants.HardenConstants.QIZONE); headingProfiledPIDController.setIZone(Constants.HardenConstants.HIZONE); - qProfiledPIDController.setIntegratorRange(Constants.HardenConstants.QIRANGE_LOWER, Constants.HardenConstants.QIRANGE_UPPER); - headingProfiledPIDController.setIntegratorRange(Constants.HardenConstants.HIRANGE_LOWER, Constants.HardenConstants.HIRANGE_UPPER); // 0.3 before + qProfiledPIDController.setIntegratorRange( + Constants.HardenConstants.QIRANGE_LOWER, Constants.HardenConstants.QIRANGE_UPPER); + headingProfiledPIDController.setIntegratorRange( + Constants.HardenConstants.HIRANGE_LOWER, + Constants.HardenConstants.HIRANGE_UPPER); // 0.3 before headingProfiledPIDController.enableContinuousInput(-Math.PI, Math.PI); // configureAutoBuilder(); From 0cd65154ddcbe278629a46be89d3730064e6a830 Mon Sep 17 00:00:00 2001 From: arj2cool Date: Wed, 8 Oct 2025 18:31:24 -0700 Subject: [PATCH 3/5] other methods --- .../robot/subsystems/ElevatorSubsystem.java | 49 ++ .../subsystems/ElevatorSubsystemOld.java | 648 +++++++++--------- 2 files changed, 373 insertions(+), 324 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 284dbdaa..0aa4b03e 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -11,6 +11,8 @@ import com.ctre.phoenix6.configs.TalonFXConfigurator; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.TorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; @@ -29,6 +31,10 @@ public class ElevatorSubsystem extends SubsystemBase { private MotionMagicConfigs mmc; // motion magic & control request private final MotionMagicVoltage controlRequest = new MotionMagicVoltage(0); + private final TorqueCurrentFOC torqueRequest = new TorqueCurrentFOC(0); + private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + + private boolean elevatorZeroed = false; public static ElevatorSubsystem instance; @@ -127,6 +133,7 @@ public void setPosition(double height) { public void resetElevatorPositionToZero() { // self explanatory master.setPosition(0); + elevatorZeroed = true; } public double getHeight() { // self explanatory @@ -151,6 +158,48 @@ public boolean isElevatorZeroed() { return Math.abs(getHeight()) <= 0.05; } + public void elevateTo(ElevatorConstants.ElevatorPositions level) { + this.setPosition(level.getHeight()); + } + + public void ElevatorTorqueMode() { + master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); + // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); + } + + public void reduceCurrentLimits() { + master.updateCurrentLimits(30, 10); + } + + public void moveElevatorNegative() { + master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); + } + + public void resetCurrentLimits() { + master.updateCurrentLimits( + Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, + Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); + } + + public boolean elevatorHasBeenZeroed() { + return elevatorZeroed; + } + + public boolean checkCurrent() { + double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); + double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); + + if (Supplycurrent > 1.0 && Statorcurrent > 20) { + return true; + } + return false; + } + + public void resetPositionFiltered() { + // master.setPosition( + // currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + } + public static ElevatorSubsystem getInstance() { if (instance==null) { instance = new ElevatorSubsystem(); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java index 8d294d68..5c7132f3 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java @@ -1,325 +1,325 @@ -// 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.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; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.MotionMagicVoltage; -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; -import frc.robot.Constants; -import frc.robot.Constants.ElevatorConstants; -import frc.robot.Constants.ElevatorConstants.ElevatorPositions; -import frc.robot.util.LoggedTalonFX; - +// // 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.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; +// import com.ctre.phoenix6.controls.Follower; +// import com.ctre.phoenix6.controls.MotionMagicVoltage; +// 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; - -public class ElevatorSubsystemOld extends SubsystemBase { - private static ElevatorSubsystem instance; - - private LoggedTalonFX motor1; - private LoggedTalonFX motor2; - public LoggedTalonFX master; - - 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(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); - - distance = - new CANrange( - ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); - - 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 - 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); - - Slot0Configs s0c = - new Slot0Configs() - .withKP(ElevatorConstants.S0C_KP) - .withKI(ElevatorConstants.S0C_KI) - .withKD(ElevatorConstants.S0C_KD) - .withKS(ElevatorConstants.S0C_KS) - .withKG(ElevatorConstants.S0C_KG) - .withKA(ElevatorConstants.S0C_KA) - .withKV(ElevatorConstants.S0C_KV) - .withGravityType(GravityTypeValue.Elevator_Static) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - - 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(); - - 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(); - } - return instance; - } - - public boolean tofIsConnected() { - return distance.isConnected(); - } - - 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 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 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); - } - - // Hardstop Zeroing functions: - public void moveElevatorNegative() { - master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); - } - - public void reduceCurrentLimits() { - master.updateCurrentLimits(30, 10); - } - - public void resetCurrentLimits() { - master.updateCurrentLimits( - Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, - Constants.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 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 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()); - } - - @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 - } -} +// import frc.robot.Constants; +// import frc.robot.Constants.ElevatorConstants; +// import frc.robot.Constants.ElevatorConstants.ElevatorPositions; +// import frc.robot.util.LoggedTalonFX; + +// // import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// public class ElevatorSubsystemOld extends SubsystemBase { +// private static ElevatorSubsystem instance; + +// private LoggedTalonFX motor1; +// private LoggedTalonFX motor2; +// public LoggedTalonFX master; + +// 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(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); + +// distance = +// new CANrange( +// ElevatorConstants.CANRANGE_PORT, Constants.Swerve.WHICH_SWERVE_ROBOT.CANBUS_NAME); + +// 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 +// 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); + +// Slot0Configs s0c = +// new Slot0Configs() +// .withKP(ElevatorConstants.S0C_KP) +// .withKI(ElevatorConstants.S0C_KI) +// .withKD(ElevatorConstants.S0C_KD) +// .withKS(ElevatorConstants.S0C_KS) +// .withKG(ElevatorConstants.S0C_KG) +// .withKA(ElevatorConstants.S0C_KA) +// .withKV(ElevatorConstants.S0C_KV) +// .withGravityType(GravityTypeValue.Elevator_Static) +// .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + +// 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(); + +// 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(); +// } +// return instance; +// } + +// public boolean tofIsConnected() { +// return distance.isConnected(); +// } + +// 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 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 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); +// } + +// // Hardstop Zeroing functions: +// public void moveElevatorNegative() { +// master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); +// } + +// public void reduceCurrentLimits() { +// master.updateCurrentLimits(30, 10); +// } + +// public void resetCurrentLimits() { +// master.updateCurrentLimits( +// Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, +// Constants.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 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 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()); +// } + +// @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 +// } +// } From 725ed594b5dcb32693c615d494e66579d57a9954 Mon Sep 17 00:00:00 2001 From: arj2cool Date: Wed, 8 Oct 2025 18:35:04 -0700 Subject: [PATCH 4/5] final changes --- .../java/frc/robot/subsystems/ElevatorSubsystem.java | 11 +++++++---- .../frc/robot/subsystems/ElevatorSubsystemOld.java | 12 ++++++++---- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 0aa4b03e..34fe2d93 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -112,6 +112,8 @@ public ElevatorSubsystem() { } public void setPosition(double height) { + targetHeight = height; + // if (height < Constants.ElevatorConstants.minHeight) // height = // Constants.ElevatorConstants @@ -147,7 +149,7 @@ public void periodic() { // logs } public boolean isAtPosition() { - return Math.abs(getHeight()-targetHeight) <= 0.05; + return Math.abs(getHeight() - targetHeight) <= 0.05; } public boolean atIntake() { @@ -162,7 +164,7 @@ public void elevateTo(ElevatorConstants.ElevatorPositions level) { this.setPosition(level.getHeight()); } - public void ElevatorTorqueMode() { + public void ElevatorTorqueMode() { master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); } @@ -197,11 +199,12 @@ public boolean checkCurrent() { public void resetPositionFiltered() { // master.setPosition( - // currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + // currentHeightToF * + // Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } public static ElevatorSubsystem getInstance() { - if (instance==null) { + if (instance == null) { instance = new ElevatorSubsystem(); } return instance; diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java index 5c7132f3..65dcd2d0 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java @@ -141,10 +141,12 @@ // public void resetPositionFiltered() { // master.setPosition( -// currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); +// currentHeightToF * +// Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); // DogLog.log( // "Subsystems/Elevator/resetElevatorPosition", -// currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); +// currentHeightToF * +// Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); // } // public void resetPosition() { @@ -185,7 +187,8 @@ // public void resetElevatorPositionToZero() { // master.setPosition(0); -// // master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0)); +// // +// master.setControl(controlRequest.withPosition(master.getPosition().getValueAsDouble()).withSlot(0)); // // master.setPosition(0); // // master.setControl(controlRequest.withPosition(0).withSlot(0)); // // master.setPosition(0); @@ -308,7 +311,8 @@ // DogLog.log( // "Subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); // DogLog.log( -// "Subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); +// "Subsystems/Elevator/elevatorProfile", +// master.getClosedLoopReference().getValueAsDouble()); // } // @Override From d97823297d9a03c00a3857d96384c28615a153a9 Mon Sep 17 00:00:00 2001 From: arj2cool Date: Wed, 8 Oct 2025 19:12:03 -0700 Subject: [PATCH 5/5] logs --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/subsystems/ElevatorSubsystem.java | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6e8464ca..534d4bf0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -926,7 +926,7 @@ public static class ElevatorConstants { public static final double STATOR_CURRENT_LIMIT = 50.0; // TODO: change for actual match public static final double SUPPLY_CURRENT_LIMIT = 30.0; // TODO: change for actual match - public static double S0C_KP = 1.0; // 1.0 before (okay) + public static double S0C_KP = 1.04; // 1.0 before (okay) public static double S0C_KI = 0.0; public static double S0C_KD = 0.005; diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 34fe2d93..f708dfd0 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -146,6 +146,7 @@ public double getHeight() { // self explanatory public void periodic() { // logs DogLog.log("subsystems/Elevator/currentheight", getHeight()); + DogLog.log("subsystems/Elevator/currentheight", getHeight()); } public boolean isAtPosition() {