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/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..f708dfd0 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -13,49 +13,33 @@ 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); + private boolean elevatorZeroed = false; - 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 +50,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 +79,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,204 +107,107 @@ 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 setPosition(double height) { + targetHeight = 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() { + public void resetElevatorPositionToZero() { // self explanatory 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); + elevatorZeroed = true; } - 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 getHeight() { // self explanatory + return master.getRotorPosition().getValueAsDouble() + * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS + / ElevatorConstants.CARRAIGE_UPDUCTION; } - public double getError() { - return currentLevel.height - * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS - / Constants.ElevatorConstants.CARRAIGE_UPDUCTION - - master.getPosition().getValueAsDouble(); + public void periodic() { // logs + DogLog.log("subsystems/Elevator/currentheight", getHeight()); + DogLog.log("subsystems/Elevator/currentheight", getHeight()); } - public ElevatorPositions getLevel() { - return currentLevel; + public boolean isAtPosition() { + return Math.abs(getHeight() - targetHeight) <= 0.05; } public boolean atIntake() { - return currentLevel.equals(ElevatorPositions.Intake); + return (Math.abs(getHeight() - ElevatorConstants.ElevatorPositions.Intake.getHeight()) <= 0.05); } - public void elevateTo(ElevatorPositions level) { - this.currentLevel = level; - this.setPosition(level.height); + public boolean isElevatorZeroed() { + return Math.abs(getHeight()) <= 0.05; } - 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 elevateTo(ElevatorConstants.ElevatorPositions level) { + this.setPosition(level.getHeight()); } 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 void reduceCurrentLimits() { + master.updateCurrentLimits(30, 10); } - public boolean canFunnelTransferCoralToScoring() { - return this.getLevel().equals(Constants.ElevatorConstants.ElevatorPositions.Intake) - && this.getError() < Constants.ElevatorConstants.MAX_POSITIONAL_ERROR; + public void moveElevatorNegative() { + master.setControl(velocityRequest.withVelocity(-5).withSlot(1)); } - 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 void resetCurrentLimits() { + master.updateCurrentLimits( + Constants.ElevatorConstants.STATOR_CURRENT_LIMIT, + Constants.ElevatorConstants.SUPPLY_CURRENT_LIMIT); } - public boolean isElevatorZeroed() { + public boolean elevatorHasBeenZeroed() { return elevatorZeroed; } - public void elevatorHasBeenZeroed() { - elevatorZeroed = true; - } + public boolean checkCurrent() { + double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); + double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); - @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()); + if (Supplycurrent > 1.0 && Statorcurrent > 20) { + return true; + } + return false; } - @Override - public void simulationPeriodic() { - // Simulate encoder behavior based on motor speed - double simulatedSpeed = master.getVelocity().getValueAsDouble(); - double currentPosition = master.getPosition().getValueAsDouble(); + public void resetPositionFiltered() { + // master.setPosition( + // currentHeightToF * + // Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); + } - // 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..65dcd2d0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystemOld.java @@ -0,0 +1,329 @@ +// // 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();