From 49c0d0284e64aad7ac5785396ba5e52ab8374b57 Mon Sep 17 00:00:00 2001 From: Kyle Waremburg Date: Wed, 19 Mar 2025 21:28:09 -0500 Subject: [PATCH 01/13] Proper LL AKit logging --- gradlew | 0 src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 10 +-- .../elevator/ElevatorIOTalonFX.java | 2 +- .../frc/robot/subsystems/vision/Vision.java | 62 +++++++++++++++++++ .../frc/robot/subsystems/vision/VisionIO.java | 2 + .../subsystems/vision/VisionIOLimelight.java | 13 ++++ 7 files changed, 85 insertions(+), 6 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ea31a54..6f0415e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -24,7 +24,7 @@ * (log replay from a file). */ public final class Constants { - public static final Mode simMode = Mode.SIM; + public static final Mode simMode = Mode.REPLAY; public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; public static final Distance kReefL1 = Meters.of(0.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 264203e..b14f435 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1438,17 +1438,19 @@ public void periodic() { double tipOfCoralToBranch = slide.coralToReefBranch(); - Pose2d llPose2dElevator = elevator.getPoseFromLL(); - Pose2d llPose2dSlide = slide.getPoseFromLL(); + // Pose2d llPose2dElevator = elevator.getPoseFromLL(); + // Pose2d llPose2dSlide = slide.getPoseFromLL(); double canRangeAngle = elevator.getCANrangeAngle(); + Pose2d blendedPose = vision.getBlendedRobotPoseInTarget(); + // elevator.moveElevator(latencyCompensatedTargetElevator); - elevator.moveElevator(llPose2dSlide, tipOfCoralToBranch); + elevator.moveElevator(blendedPose, tipOfCoralToBranch); // Logger.recordOutput("RobotContainer/LatencyTargetElevator", // latencyCompensatedTargetElevator); // Logger.recordOutput("RobotContainer/LatencyTargetSlide", latencyCompensatedTargetSlide); slide.commonSlideMove( - llPose2dSlide, elevator.elevatorPos(), averaged_robot_angle_to_reef_face.getDegrees()); + blendedPose, elevator.elevatorPos(), averaged_robot_angle_to_reef_face.getDegrees()); } catch (Exception exception) { } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index 674e6b3..eedde0a 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -75,7 +75,7 @@ public ElevatorIOTalonFX() { elevatorLeftCfg.Slot0.kA = Constants.kElevatorkA; elevatorLeftCfg.Slot0.kS = Constants.kElevatorkS; elevatorLeftCfg.CurrentLimits.StatorCurrentLimit = 80.0; - elevatorLeftCfg.Feedback.SensorToMechanismRatio = 23.622; + elevatorLeftCfg.Feedback.SensorToMechanismRatio = 31.496; elevatorLeftCfg.MotionMagic.MotionMagicCruiseVelocity = Constants.kElevatorMotionMagicCruiseVelocity; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 7e06d58..d3bfe28 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -13,18 +13,24 @@ package frc.robot.subsystems.vision; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; import static frc.robot.subsystems.vision.VisionConstants.*; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.LinkedList; import java.util.List; @@ -36,6 +42,14 @@ public class Vision extends SubsystemBase { private final VisionIOInputsAutoLogged[] inputs; private final Alert[] disconnectedAlerts; + private final LinearFilter slidingAverageFilterX = LinearFilter.movingAverage(9); + private final LinearFilter slidingAverageFilterY = LinearFilter.movingAverage(9); + private final LinearFilter slidingAverageFilterAngle = LinearFilter.movingAverage(9); + + public double m_averagedRobotAngleToReefFace = 0.0; + public final Trigger angleToLargeTrigger = + new Trigger(() -> Math.abs(m_averagedRobotAngleToReefFace) < 12.5); + public Vision(VisionConsumer consumer, VisionIO... io) { this.consumer = consumer; this.io = io; @@ -64,6 +78,54 @@ public Rotation2d getTargetX(int cameraIndex) { return inputs[cameraIndex].latestTargetObservation.tx(); } + /** + * Returns the best target, which can be used for simple servoing with vision. + * + * @param cameraIndex The index of the camera to use. + */ + public Pose2d getRobotPoseInTarget(int cameraIndex) { + return inputs[cameraIndex].robotPoseTarget; + } + + public Pose2d getBlendedRobotPoseInTarget() { + Pose2d leftCameraPose = getRobotPoseInTarget(0); + Pose2d rightCameraPose = getRobotPoseInTarget(1); + + var leftVisible = (leftCameraPose.getX() != 0.0) && (leftCameraPose.getY() != 0.0); + var rightVisible = (rightCameraPose.getX() != 0.0) && (rightCameraPose.getY() != 0.0); + + Translation2d translation = new Translation2d(); + Rotation2d rotation = new Rotation2d(); + + if (leftVisible && rightVisible) { + translation = leftCameraPose.getTranslation().plus(rightCameraPose.getTranslation()).div(2.0); + rotation = leftCameraPose.getRotation().plus(rightCameraPose.getRotation()).div(2.0); + } else if (leftVisible) { + translation = leftCameraPose.getTranslation(); + rotation = leftCameraPose.getRotation(); + } else if (rightVisible) { + translation = rightCameraPose.getTranslation(); + rotation = rightCameraPose.getRotation(); + } + + Distance averaged_robot_to_tag_x = + Meters.of(slidingAverageFilterX.calculate(translation.getX())); + Distance averaged_robot_to_tag_y = + Meters.of(slidingAverageFilterY.calculate(translation.getY())); + Rotation2d averaged_robot_angle_to_reef_face = + new Rotation2d(Radians.of(slidingAverageFilterAngle.calculate(rotation.getRadians()))); + + Logger.recordOutput("Vision/averagedAngle", averaged_robot_angle_to_reef_face.getDegrees()); + + Pose2d llPose2d = + new Pose2d( + averaged_robot_to_tag_x, averaged_robot_to_tag_y, averaged_robot_angle_to_reef_face); + + Logger.recordOutput("Vision/averagedPose", llPose2d); + + return llPose2d; + } + @Override public void periodic() { for (int i = 0; i < io.length; i++) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 3183358..a9de77c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; @@ -25,6 +26,7 @@ public static class VisionIOInputs { new TargetObservation(new Rotation2d(), new Rotation2d()); public PoseObservation[] poseObservations = new PoseObservation[0]; public int[] tagIds = new int[0]; + public Pose2d robotPoseTarget = new Pose2d(); } /** Represents the angle to a simple target, not used for pose estimation. */ diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index d6e2a77..ccbfbe2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -13,6 +13,7 @@ package frc.robot.subsystems.vision; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; @@ -38,6 +39,7 @@ public class VisionIOLimelight implements VisionIO { private final DoubleSubscriber tySubscriber; private final DoubleArraySubscriber megatag1Subscriber; private final DoubleArraySubscriber megatag2Subscriber; + private final DoubleArraySubscriber robotPoseTargetSubscriber; /** * Creates a new VisionIOLimelight. @@ -55,6 +57,8 @@ public VisionIOLimelight(String name, Supplier rotationSupplier) { megatag1Subscriber = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(new double[] {}); megatag2Subscriber = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(new double[] {}); + robotPoseTargetSubscriber = + table.getDoubleArrayTopic("botpose_targetspace").subscribe(new double[] {}); } @Override @@ -140,6 +144,15 @@ public void updateInputs(VisionIOInputs inputs) { for (int id : tagIds) { inputs.tagIds[i++] = id; } + + for (var robotPoseTarget : robotPoseTargetSubscriber.readQueue()) { + if (robotPoseTarget.value.length == 0) continue; + inputs.robotPoseTarget = + new Pose2d( + robotPoseTarget.value[0], + robotPoseTarget.value[2], + Rotation2d.fromDegrees(robotPoseTarget.value[4])); + } } /** Parses the 3D pose from a Limelight botpose array. */ From 32fe03378c97558ef7064ddbeb8458ae4137fe18 Mon Sep 17 00:00:00 2001 From: Kython89 Date: Fri, 4 Apr 2025 17:59:34 -0500 Subject: [PATCH 02/13] vision working on red and blue --- src/main/java/frc/robot/FieldConstants.java | 64 +++++++++---------- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 29 +++++++-- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../elevator/ElevatorIOTalonFX.java | 2 +- .../frc/robot/subsystems/slide/Slide.java | 5 -- .../frc/robot/subsystems/vision/Vision.java | 4 +- 7 files changed, 62 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java index 5d19b5d..6bbda32 100644 --- a/src/main/java/frc/robot/FieldConstants.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -64,7 +64,7 @@ public static class Reef { Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line public static final Pose2d[] centerFaces = - new Pose2d[6]; // Starting facing the driver station in clockwise order + new Pose2d[12]; // Starting facing the driver station in clockwise order public static final List> branchPositions = new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise @@ -100,39 +100,39 @@ public static class Reef { Units.inchesToMeters(160.375), Units.inchesToMeters(130.144), Rotation2d.fromDegrees(-120)); - // centerFaces[6] = - // new Pose2d( - // Units.inchesToMeters(530.49), - // Units.inchesToMeters(130.17), - // Rotation2d.fromDegrees(300)); - // centerFaces[7] = - // new Pose2d( - // Units.inchesToMeters(546.87), - // Units.inchesToMeters(158.50), - // Rotation2d.fromDegrees(0)); - // centerFaces[8] = - // new Pose2d( - // Units.inchesToMeters(530.49), - // Units.inchesToMeters(186.83), - // Rotation2d.fromDegrees(60)); - // centerFaces[9] = - // new Pose2d( - // Units.inchesToMeters(497.77), - // Units.inchesToMeters(186.83), - // Rotation2d.fromDegrees(120)); - // centerFaces[10] = - // new Pose2d( - // Units.inchesToMeters(481.39), - // Units.inchesToMeters(158.50), - // Rotation2d.fromDegrees(180)); - // centerFaces[11] = - // new Pose2d( - // Units.inchesToMeters(497.77), - // Units.inchesToMeters(130.17), - // Rotation2d.fromDegrees(240)); + centerFaces[6] = + new Pose2d( + Units.inchesToMeters(530.49), + Units.inchesToMeters(130.17), + Rotation2d.fromDegrees(300)); + centerFaces[7] = + new Pose2d( + Units.inchesToMeters(546.87), + Units.inchesToMeters(158.50), + Rotation2d.fromDegrees(0)); + centerFaces[8] = + new Pose2d( + Units.inchesToMeters(530.49), + Units.inchesToMeters(186.83), + Rotation2d.fromDegrees(60)); + centerFaces[9] = + new Pose2d( + Units.inchesToMeters(497.77), + Units.inchesToMeters(186.83), + Rotation2d.fromDegrees(120)); + centerFaces[10] = + new Pose2d( + Units.inchesToMeters(481.39), + Units.inchesToMeters(158.50), + Rotation2d.fromDegrees(180)); + centerFaces[11] = + new Pose2d( + Units.inchesToMeters(497.77), + Units.inchesToMeters(130.17), + Rotation2d.fromDegrees(240)); // Initialize branch positions - for (int face = 0; face < 6; face++) { + for (int face = 0; face < 12; face++) { Map fillRight = new HashMap<>(); Map fillLeft = new HashMap<>(); for (var level : ReefHeight.values()) { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 12c0a70..8b06e92 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -50,7 +50,7 @@ public class Robot extends LoggedRobot { private boolean brakesDisabled = false; public Robot() { - super(0.01); + super(0.0142); // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b14f435..294df6f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; @@ -113,6 +115,7 @@ public class RobotContainer { private static final LoggedNetworkNumber latency = new LoggedNetworkNumber("latency", 0); private CANdle m_CANdle = new CANdle(1); public LinearFilter odometryAverageFilterAngle = LinearFilter.movingAverage(9); + private Alliance m_alliance = Alliance.Blue; // private final Command m_testAutoCommand = new TestAuto(drive); @@ -923,7 +926,7 @@ private void configureTriggerBindings() { .and(seesTarget()) .and(elevator.peggedTrigger.negate()) .and(shooter.isShooterVelocityLowEnoughToScore()) - .and(slide.angleTooLargeTrigger()) + .and(vision.angleTooLargeTrigger) .and(shooter.isShooterZAccelLowEnough()) // .and(drive.isDriveAccelLowEnoughToScore().debounce(0.05)) // was 0.25 .and(robotMovingTrigger()); // was 0.06 @@ -1372,8 +1375,24 @@ public void periodic() { Logger.recordOutput("RobotContainer/Height1", elevatorL2Pos()); Logger.recordOutput("RobotContainer/Height2", elevatorL3Pos()); Logger.recordOutput("RobotContainer/Height3", elevatorL4Pos()); + Logger.recordOutput("RobotContainer/AngleToGreat", vision.angleTooLargeTrigger); + + var alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + if (alliance != m_alliance) { + m_alliance = alliance; + if (m_alliance == Alliance.Blue) { + int[] blueTagIds = {17, 18, 19, 20, 21, 22}; + LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", blueTagIds); + LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", blueTagIds); + } else { + int[] redTagIds = {6, 7, 8, 9, 10, 11}; + LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fl", redTagIds); + LimelightHelpers.SetFiducialIDFiltersOverride("limelight-fr", redTagIds); + } + } // Boolean doRejectUpdate = false; + // LimelightHelpers.PoseEstimate mt1 = // LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight-fl"); @@ -1438,19 +1457,19 @@ public void periodic() { double tipOfCoralToBranch = slide.coralToReefBranch(); - // Pose2d llPose2dElevator = elevator.getPoseFromLL(); - // Pose2d llPose2dSlide = slide.getPoseFromLL(); + // Pose2d llPose2dElevator = elevator.getPoseFromLL(); + // Pose2d llPose2dSlide = slide.getPoseFromLL(); double canRangeAngle = elevator.getCANrangeAngle(); Pose2d blendedPose = vision.getBlendedRobotPoseInTarget(); - + // elevator.moveElevator(latencyCompensatedTargetElevator); elevator.moveElevator(blendedPose, tipOfCoralToBranch); // Logger.recordOutput("RobotContainer/LatencyTargetElevator", // latencyCompensatedTargetElevator); // Logger.recordOutput("RobotContainer/LatencyTargetSlide", latencyCompensatedTargetSlide); slide.commonSlideMove( - blendedPose, elevator.elevatorPos(), averaged_robot_angle_to_reef_face.getDegrees()); + blendedPose, elevator.elevatorPos(), averaged_robot_angle_to_reef_face.getDegrees()); } catch (Exception exception) { } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 2832c61..cb6433c 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -238,7 +238,7 @@ public void periodic() { */ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.01); + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.0142); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index eedde0a..674e6b3 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -75,7 +75,7 @@ public ElevatorIOTalonFX() { elevatorLeftCfg.Slot0.kA = Constants.kElevatorkA; elevatorLeftCfg.Slot0.kS = Constants.kElevatorkS; elevatorLeftCfg.CurrentLimits.StatorCurrentLimit = 80.0; - elevatorLeftCfg.Feedback.SensorToMechanismRatio = 31.496; + elevatorLeftCfg.Feedback.SensorToMechanismRatio = 23.622; elevatorLeftCfg.MotionMagic.MotionMagicCruiseVelocity = Constants.kElevatorMotionMagicCruiseVelocity; diff --git a/src/main/java/frc/robot/subsystems/slide/Slide.java b/src/main/java/frc/robot/subsystems/slide/Slide.java index 6e61ef0..ac6a979 100644 --- a/src/main/java/frc/robot/subsystems/slide/Slide.java +++ b/src/main/java/frc/robot/subsystems/slide/Slide.java @@ -68,7 +68,6 @@ public void periodic() { Logger.recordOutput("Slide/SDisScoreReady", m_slideCloseEnoughToScore); Logger.recordOutput("Slide/SClose", isSlideCloseToTarget()); Logger.recordOutput("Slide/SlideAlign", m_slideAlign); - Logger.recordOutput("Slide/angleToGreatToScore", angleTooLargeTrigger()); } public Command slideVoltageCmd(double volts, boolean overrideLimits) { @@ -167,10 +166,6 @@ else if (fl_visible) { return llPose2d; } - public Trigger angleTooLargeTrigger() { - return new Trigger(() -> Math.abs(m_averagedRobotAngleToReefFace) < 12.5); - } - public void commonSlideMove(Pose2d pose, Double elevatorHeight, double theta) { Pose2d bumperPose2d = robotCenterOfBumper(pose, pose.getRotation()); Logger.recordOutput("Slide/BumperPose", bumperPose2d); diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index d3bfe28..6be2fab 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -47,7 +47,7 @@ public class Vision extends SubsystemBase { private final LinearFilter slidingAverageFilterAngle = LinearFilter.movingAverage(9); public double m_averagedRobotAngleToReefFace = 0.0; - public final Trigger angleToLargeTrigger = + public final Trigger angleTooLargeTrigger = new Trigger(() -> Math.abs(m_averagedRobotAngleToReefFace) < 12.5); public Vision(VisionConsumer consumer, VisionIO... io) { @@ -115,6 +115,8 @@ public Pose2d getBlendedRobotPoseInTarget() { Rotation2d averaged_robot_angle_to_reef_face = new Rotation2d(Radians.of(slidingAverageFilterAngle.calculate(rotation.getRadians()))); + m_averagedRobotAngleToReefFace = averaged_robot_angle_to_reef_face.getDegrees(); + Logger.recordOutput("Vision/averagedAngle", averaged_robot_angle_to_reef_face.getDegrees()); Pose2d llPose2d = From 52b77c56b6e20d2999a7f47fee1e9c61f156cc9f Mon Sep 17 00:00:00 2001 From: Kython89 Date: Fri, 4 Apr 2025 22:42:11 -0500 Subject: [PATCH 03/13] 4 piece working amazingly --- .../deploy/pathplanner/autos/Frontface 4.auto | 61 ++--- src/main/deploy/pathplanner/paths/F4 1.path | 8 +- src/main/deploy/pathplanner/paths/F4 2.path | 35 ++- src/main/deploy/pathplanner/paths/F4 3.path | 12 +- src/main/deploy/pathplanner/paths/F4 4.path | 2 +- src/main/deploy/pathplanner/paths/F4 5.path | 2 +- src/main/deploy/pathplanner/paths/F4 6.path | 19 +- src/main/java/frc/robot/Robot.java | 19 +- src/main/java/frc/robot/RobotContainer.java | 258 +++++++++--------- .../robot/subsystems/elevator/Elevator.java | 20 +- .../robot/subsystems/elevator/ElevatorIO.java | 4 +- .../elevator/ElevatorIOTalonFX.java | 43 ++- 12 files changed, 236 insertions(+), 247 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Frontface 4.auto b/src/main/deploy/pathplanner/autos/Frontface 4.auto index 7d8f1e7..dd1c4cc 100644 --- a/src/main/deploy/pathplanner/autos/Frontface 4.auto +++ b/src/main/deploy/pathplanner/autos/Frontface 4.auto @@ -10,18 +10,6 @@ "pathName": "F4 1" } }, - { - "type": "named", - "data": { - "name": "5PieceScore" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0 - } - }, { "type": "path", "data": { @@ -35,27 +23,28 @@ } }, { - "type": "named", + "type": "parallel", "data": { - "name": "5PieceScore" + "commands": [ + { + "type": "named", + "data": { + "name": "5PieceScore" + } + }, + { + "type": "named", + "data": { + "name": "autoL3DeAlgeafy" + } + } + ] } }, { "type": "wait", "data": { - "waitTime": 0 - } - }, - { - "type": "named", - "data": { - "name": "autoL3DeAlgeafy" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.5 + "waitTime": 0.4 } }, { @@ -70,18 +59,6 @@ "pathName": "F4 5" } }, - { - "type": "named", - "data": { - "name": "5PieceScore" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0 - } - }, { "type": "path", "data": { @@ -99,12 +76,6 @@ "data": { "name": "5PieceScore" } - }, - { - "type": "wait", - "data": { - "waitTime": 0 - } } ] } diff --git a/src/main/deploy/pathplanner/paths/F4 1.path b/src/main/deploy/pathplanner/paths/F4 1.path index 4f6d420..531424c 100644 --- a/src/main/deploy/pathplanner/paths/F4 1.path +++ b/src/main/deploy/pathplanner/paths/F4 1.path @@ -32,11 +32,11 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.7, + "minWaypointRelativePos": 0.6, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.1, + "maxAcceleration": 1.2, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -70,8 +70,8 @@ } ], "globalConstraints": { - "maxVelocity": 6.0, - "maxAcceleration": 5.0, + "maxVelocity": 4.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/F4 2.path b/src/main/deploy/pathplanner/paths/F4 2.path index 3ab75bb..7379048 100644 --- a/src/main/deploy/pathplanner/paths/F4 2.path +++ b/src/main/deploy/pathplanner/paths/F4 2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.4063150104370652, - "y": 5.685463358904612 + "x": 1.3839614129027513, + "y": 5.696731469050455 }, "prevControl": { - "x": 1.7815617209633812, - "y": 5.271729806273033 + "x": 1.8024346366726127, + "y": 5.2833140298496595 }, "nextControl": null, "isLocked": false, @@ -31,16 +31,16 @@ "rotationTargets": [ { "waypointRelativePos": 0.6871069182390003, - "rotationDegrees": 137.13691041664012 + "rotationDegrees": 140.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.6, + "minWaypointRelativePos": 0.65, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.25, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -52,19 +52,19 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "IntakeOn", + "name": "5PieceScore", "waypointRelativePos": 0, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "IntakeOn" + "name": "5PieceScore" } } }, { "name": "ElevatorDown", - "waypointRelativePos": 0, + "waypointRelativePos": 0.06, "endWaypointRelativePos": null, "command": { "type": "named", @@ -72,11 +72,22 @@ "name": "ElevatorDown" } } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } } ], "globalConstraints": { "maxVelocity": 6.0, - "maxAcceleration": 4.5, + "maxAcceleration": 3.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -84,7 +95,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 137.48955292199918 + "rotation": 139.0 }, "reversed": false, "folder": "Front 4", diff --git a/src/main/deploy/pathplanner/paths/F4 3.path b/src/main/deploy/pathplanner/paths/F4 3.path index 75c1f32..9819008 100644 --- a/src/main/deploy/pathplanner/paths/F4 3.path +++ b/src/main/deploy/pathplanner/paths/F4 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4063150104370652, - "y": 5.685463358904612 + "x": 1.3839614129027513, + "y": 5.696731469050455 }, "prevControl": null, "nextControl": { - "x": 1.8998484947882865, - "y": 5.061753805014962 + "x": 1.8774948972539727, + "y": 5.073021915160805 }, "isLocked": false, "linkedName": "F4 2" @@ -65,7 +65,7 @@ ], "globalConstraints": { "maxVelocity": 6.0, - "maxAcceleration": 4.5, + "maxAcceleration": 4.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -79,7 +79,7 @@ "folder": "Front 4", "idealStartingState": { "velocity": 0, - "rotation": 137.48955292199918 + "rotation": 139.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F4 4.path b/src/main/deploy/pathplanner/paths/F4 4.path index 730ba8c..ddaf64f 100644 --- a/src/main/deploy/pathplanner/paths/F4 4.path +++ b/src/main/deploy/pathplanner/paths/F4 4.path @@ -35,7 +35,7 @@ "minWaypointRelativePos": 0.6, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.1, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, diff --git a/src/main/deploy/pathplanner/paths/F4 5.path b/src/main/deploy/pathplanner/paths/F4 5.path index 84602f8..4361246 100644 --- a/src/main/deploy/pathplanner/paths/F4 5.path +++ b/src/main/deploy/pathplanner/paths/F4 5.path @@ -36,7 +36,7 @@ "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 3.0, - "maxAcceleration": 2.52, + "maxAcceleration": 1.75, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/F4 6.path b/src/main/deploy/pathplanner/paths/F4 6.path index 8e2927a..d09b0fd 100644 --- a/src/main/deploy/pathplanner/paths/F4 6.path +++ b/src/main/deploy/pathplanner/paths/F4 6.path @@ -40,7 +40,7 @@ "minWaypointRelativePos": 0.6, "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 1.5, + "maxVelocity": 1.1, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -52,19 +52,19 @@ "pointTowardsZones": [], "eventMarkers": [ { - "name": "IntakeOn", + "name": "5PieceScore", "waypointRelativePos": 0, "endWaypointRelativePos": null, "command": { "type": "named", "data": { - "name": "IntakeOn" + "name": "5PieceScore" } } }, { "name": "ElevatorDown", - "waypointRelativePos": 0, + "waypointRelativePos": 0.06, "endWaypointRelativePos": null, "command": { "type": "named", @@ -72,6 +72,17 @@ "name": "ElevatorDown" } } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } } ], "globalConstraints": { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8b06e92..338effc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,13 +13,14 @@ package frc.robot; +import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.DriveMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; +import com.pathplanner.lib.commands.FollowPathCommand; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.IterativeRobotBase; import edu.wpi.first.wpilibj.Threads; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Watchdog; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -125,6 +126,8 @@ public Robot() { // Start AdvantageKit logger Logger.start(); + SignalLogger.enableAutoLogging(false); + // Check for valid swerve config var modules = new SwerveModuleConstants[] { @@ -144,6 +147,7 @@ public Robot() { // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. robotContainer = new RobotContainer(); + FollowPathCommand.warmupCommand().schedule(); } /** This function is called periodically during all modes. */ @@ -167,11 +171,11 @@ public void robotPeriodic() { /** This function is called once when the robot is disabled. */ @Override public void disabledInit() { - disableTime = Timer.getFPGATimestamp(); + // disableTime = Timer.getFPGATimestamp(); - if (DriverStation.isFMSAttached() == false) { - robotContainer.disableBrakes(); - } + // if (DriverStation.isFMSAttached() == false) { + // robotContainer.disableBrakes(); + // } // robotContainer.disableBrakes(); } @@ -208,8 +212,9 @@ public void teleopInit() { } hasBeenEnabled = true; - - robotContainer.enableBrakes(); + if (DriverStation.isFMSAttached() == false) { + robotContainer.enableBrakes(); + } } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 294df6f..ab30e55 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,12 +22,9 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -115,7 +112,7 @@ public class RobotContainer { private static final LoggedNetworkNumber latency = new LoggedNetworkNumber("latency", 0); private CANdle m_CANdle = new CANdle(1); public LinearFilter odometryAverageFilterAngle = LinearFilter.movingAverage(9); - private Alliance m_alliance = Alliance.Blue; + private Alliance m_alliance = Alliance.Red; // private final Command m_testAutoCommand = new TestAuto(drive); @@ -1175,12 +1172,12 @@ private final Command autoScoreFor3Piece() { private final Command autoScoreFor5Piece() { return new SequentialCommandGroup( - new WaitUntilCommand( - () -> - slide.isSlideReadyToScore() - && elevator.isElevatorClose() - && angulator.readyToScore()) - .withTimeout(0.1), + // new WaitUntilCommand( + // () -> + // slide.isSlideReadyToScore() + // && elevator.isElevatorClose() + // && angulator.readyToScore()) + // .withTimeout(0.1), shooter.setShooterVelocityCmd(65), // new WaitCommand(0.05), // was 0.25 // shooter.positionVoltageNoMovement(), @@ -1208,8 +1205,7 @@ private final Command autoIntakeDown() { private final Command autoL3DeAlgeafy() { return Commands.sequence( angulator.angulatorPosStateCmd(4), - slide.centerSlideCmd(true), - slide.slideAlignCmd(true), + slide.setSlidePositionCmd(Meters.of(0.05)), new WaitUntilCommand(angulator::isAngulatorSafeToMove), elevator.setM_ElevatorAlignCmd(true), elevator.setM_ElevatorHeightCmd(5), @@ -1243,124 +1239,126 @@ private Trigger robotMovingTrigger() { && Math.abs(drive.getChassisSpeeds().omegaRadiansPerSecond) < 0.1); } - public Pose2d getLatencyCompTarget(double latency) { - Pose2d currentPose = drive.getPose(); - - Rotation2d poseRotation = currentPose.getRotation(); - ChassisSpeeds chassisSpeeds = drive.getChassisSpeeds(); - if (RobotBase.isReal() == false) { - chassisSpeeds = new ChassisSpeeds(1, 0, 0); - } - Translation2d globalVelocity = - new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) - .rotateBy(poseRotation); - - double angularSpeed = chassisSpeeds.omegaRadiansPerSecond; + // public Pose2d getLatencyCompTarget(double latency) { + // Pose2d currentPose = drive.getPose(); + + // Rotation2d poseRotation = currentPose.getRotation(); + // ChassisSpeeds chassisSpeeds = drive.getChassisSpeeds(); + // if (RobotBase.isReal() == false) { + // chassisSpeeds = new ChassisSpeeds(1, 0, 0); + // } + // Translation2d globalVelocity = + // new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond) + // .rotateBy(poseRotation); + + // double angularSpeed = chassisSpeeds.omegaRadiansPerSecond; + + // Integer reefFaceIdx = Align.getClosestReefFaceIndex(currentPose); + + // Pose2d reefFace = Align.getClosestReefFace(reefFaceIdx); + + // if (RobotBase.isReal() == false) { + // reefFace = new Pose2d(5, 6, Rotation2d.fromDegrees(60)); + // } + + // Rotation2d robot_angle_to_reef_face = + // poseRotation.rotateBy(reefFace.getRotation().unaryMinus()); + + // Pose2d robotRelativeToTarget = currentPose.relativeTo(reefFace); + // // Pose2d targetToRobot = + // // currentPose.transformBy( + // // new Transform2d(reefFace.getTranslation(), reefFace.getRotation()).inverse()); + + // Logger.recordOutput("RobotContainer/currentPose", currentPose); // works + // Logger.recordOutput("RobotContainer/TargetToRobot", robotRelativeToTarget); + // Logger.recordOutput("RobotContainer/ClosestReefFace", reefFace); // works + // Logger.recordOutput("RobotContainer/ChassisSpeeds", chassisSpeeds); // works + // Logger.recordOutput("RobotContainer/globalVelocitys", globalVelocity); // works + // Logger.recordOutput("RobotContainer/angularSpeed", angularSpeed); // works + + // double currentTime = Timer.getFPGATimestamp(); + // // double dt = currentTime - m_PreviousTime; + // Logger.recordOutput("RobotContainer/currentTime", currentTime); + + // double dtX = latency; // latency.get(); + // double accelX = (0); + // // targetSpaceVelocity.getX() - m_PreviousChassisSpeeds.getX()); // *dt/; + // // if (accelX < 0) { + // // double stopTimeX = Math.abs(targetSpaceVelocity.getX() / accelX); + // // dtX = Math.min(latency, stopTimeX); + // // } + // double futureX = currentPose.getX() + (globalVelocity.getX() * dtX); + // // + (0.5 * accelX * Math.pow(dtX, 2)); + // futureX = Math.max(0, futureX); + + // double dtY = latency; // latency.get(); + // double accelY = (0); + // // targetSpaceVelocity.getY() - m_PreviousChassisSpeeds.getY()); // *dt; + // // if (accelY < 0) { + // // double stopTimeY = Math.abs(targetSpaceVelocity.getY() / accelY); + // // dtY = Math.min(latency, stopTimeY); + // // } + // double futureY = currentPose.getY() + (globalVelocity.getY() * dtY); + // // + (0.5 * accelY * Math.pow(dtY, 2)); + + // double dtOmega = latency; // latency.get(); + // double accelOmega = (0); + // // angularSpeed - m_PreviousAngle); // * dt; + // // if (accelOmega != 0) { + // // double stopTimeOmega = Math.abs(angularSpeed / accelOmega); + // // dtOmega = Math.min(latency, stopTimeOmega); + // // } + // double futureOmega = poseRotation.getRadians() + (angularSpeed * dtOmega); + // // + (0.5 * accelOmega * Math.pow(dtOmega, 2)); + // m_PreviousAngle = accelOmega; + + // m_PreviousChassisSpeeds = globalVelocity; + + // m_PreviousTime = Timer.getFPGATimestamp(); + + // // futureX = targetToRobot.getX(); + // // futureY = targetToRobot.getY(); + // // futureOmega = targetToRobot.getRotation().getRadians(); + // // Logger.recordOutput("RobotContainer/dt", dt); + // // Logger.recordOutput("RobotContainer/accelX", accelX); + // // Logger.recordOutput("RobotContainer/accelY", accelY); + // // Logger.recordOutput("RobotContainer/accelOmega", accelOmega); + + // Logger.recordOutput("RobotContainer/m_PreviousAngle", m_PreviousAngle); + // Logger.recordOutput("RobotContainer/previousTime", m_PreviousTime); + // Logger.recordOutput("RobotContainer/PreviousChassisSpeeds", m_PreviousChassisSpeeds); + + // Logger.recordOutput("RobotContainer/futureX", futureX); + // Logger.recordOutput("RobotContainer/futureY", futureY); + // Logger.recordOutput("RobotContainer/futureOmega", futureOmega); + + // Logger.recordOutput("RobotContainer/DtX", dtX); + // Logger.recordOutput("RobotContainer/DtY", dtY); + // Logger.recordOutput("RobotContainer/DtOmega", dtOmega); + // Logger.recordOutput("RobotContainer/robot_angle_to_reef_face", robot_angle_to_reef_face); + + // Logger.recordOutput( + // "RobotContainer/futurePose", + // new Pose2d(new Translation2d(futureX, futureY), Rotation2d.fromRadians(futureOmega))); + + // Pose2d futurePose2d = + // new Pose2d(new Translation2d(futureX, futureY), Rotation2d.fromRadians(futureOmega)); + + // Pose2d futurePose2dTargetSpace = futurePose2d.relativeTo(reefFace); + // // Logger.recordOutput("RobotContainer/pose", futurePose2d); // .transformBy( + // // new Transform2d(reefFace.getTranslation(), reefFace.getRotation()))); + + // Logger.recordOutput("RobotContainer/TargetSpaceY", futurePose2dTargetSpace.getX()); + // Logger.recordOutput("RobotContainer/TargetSpaceX", futurePose2dTargetSpace.getY()); + // Logger.recordOutput("RobotContainer/TargetSpaceOmega", + // futurePose2dTargetSpace.getRotation()); + + // return new Pose2d( + // futurePose2dTargetSpace.getY(), + // -futurePose2dTargetSpace.getX(), + // futurePose2dTargetSpace.getRotation()); + // } - Integer reefFaceIdx = Align.getClosestReefFaceIndex(currentPose); - - Pose2d reefFace = Align.getClosestReefFace(reefFaceIdx); - - if (RobotBase.isReal() == false) { - reefFace = new Pose2d(5, 6, Rotation2d.fromDegrees(60)); - } - - Rotation2d robot_angle_to_reef_face = - poseRotation.rotateBy(reefFace.getRotation().unaryMinus()); - - Pose2d robotRelativeToTarget = currentPose.relativeTo(reefFace); - // Pose2d targetToRobot = - // currentPose.transformBy( - // new Transform2d(reefFace.getTranslation(), reefFace.getRotation()).inverse()); - - Logger.recordOutput("RobotContainer/currentPose", currentPose); // works - Logger.recordOutput("RobotContainer/TargetToRobot", robotRelativeToTarget); - Logger.recordOutput("RobotContainer/ClosestReefFace", reefFace); // works - Logger.recordOutput("RobotContainer/ChassisSpeeds", chassisSpeeds); // works - Logger.recordOutput("RobotContainer/globalVelocitys", globalVelocity); // works - Logger.recordOutput("RobotContainer/angularSpeed", angularSpeed); // works - - double currentTime = Timer.getFPGATimestamp(); - // double dt = currentTime - m_PreviousTime; - Logger.recordOutput("RobotContainer/currentTime", currentTime); - - double dtX = latency; // latency.get(); - double accelX = (0); - // targetSpaceVelocity.getX() - m_PreviousChassisSpeeds.getX()); // *dt/; - // if (accelX < 0) { - // double stopTimeX = Math.abs(targetSpaceVelocity.getX() / accelX); - // dtX = Math.min(latency, stopTimeX); - // } - double futureX = currentPose.getX() + (globalVelocity.getX() * dtX); - // + (0.5 * accelX * Math.pow(dtX, 2)); - futureX = Math.max(0, futureX); - - double dtY = latency; // latency.get(); - double accelY = (0); - // targetSpaceVelocity.getY() - m_PreviousChassisSpeeds.getY()); // *dt; - // if (accelY < 0) { - // double stopTimeY = Math.abs(targetSpaceVelocity.getY() / accelY); - // dtY = Math.min(latency, stopTimeY); - // } - double futureY = currentPose.getY() + (globalVelocity.getY() * dtY); - // + (0.5 * accelY * Math.pow(dtY, 2)); - - double dtOmega = latency; // latency.get(); - double accelOmega = (0); - // angularSpeed - m_PreviousAngle); // * dt; - // if (accelOmega != 0) { - // double stopTimeOmega = Math.abs(angularSpeed / accelOmega); - // dtOmega = Math.min(latency, stopTimeOmega); - // } - double futureOmega = poseRotation.getRadians() + (angularSpeed * dtOmega); - // + (0.5 * accelOmega * Math.pow(dtOmega, 2)); - m_PreviousAngle = accelOmega; - - m_PreviousChassisSpeeds = globalVelocity; - - m_PreviousTime = Timer.getFPGATimestamp(); - - // futureX = targetToRobot.getX(); - // futureY = targetToRobot.getY(); - // futureOmega = targetToRobot.getRotation().getRadians(); - // Logger.recordOutput("RobotContainer/dt", dt); - // Logger.recordOutput("RobotContainer/accelX", accelX); - // Logger.recordOutput("RobotContainer/accelY", accelY); - // Logger.recordOutput("RobotContainer/accelOmega", accelOmega); - - Logger.recordOutput("RobotContainer/m_PreviousAngle", m_PreviousAngle); - Logger.recordOutput("RobotContainer/previousTime", m_PreviousTime); - Logger.recordOutput("RobotContainer/PreviousChassisSpeeds", m_PreviousChassisSpeeds); - - Logger.recordOutput("RobotContainer/futureX", futureX); - Logger.recordOutput("RobotContainer/futureY", futureY); - Logger.recordOutput("RobotContainer/futureOmega", futureOmega); - - Logger.recordOutput("RobotContainer/DtX", dtX); - Logger.recordOutput("RobotContainer/DtY", dtY); - Logger.recordOutput("RobotContainer/DtOmega", dtOmega); - Logger.recordOutput("RobotContainer/robot_angle_to_reef_face", robot_angle_to_reef_face); - - Logger.recordOutput( - "RobotContainer/futurePose", - new Pose2d(new Translation2d(futureX, futureY), Rotation2d.fromRadians(futureOmega))); - - Pose2d futurePose2d = - new Pose2d(new Translation2d(futureX, futureY), Rotation2d.fromRadians(futureOmega)); - - Pose2d futurePose2dTargetSpace = futurePose2d.relativeTo(reefFace); - // Logger.recordOutput("RobotContainer/pose", futurePose2d); // .transformBy( - // new Transform2d(reefFace.getTranslation(), reefFace.getRotation()))); - - Logger.recordOutput("RobotContainer/TargetSpaceY", futurePose2dTargetSpace.getX()); - Logger.recordOutput("RobotContainer/TargetSpaceX", futurePose2dTargetSpace.getY()); - Logger.recordOutput("RobotContainer/TargetSpaceOmega", futurePose2dTargetSpace.getRotation()); - - return new Pose2d( - futurePose2dTargetSpace.getY(), - -futurePose2dTargetSpace.getX(), - futurePose2dTargetSpace.getRotation()); - } // /** // * Use this to pass the autonomous command to the main {@link Robot} class. // * @@ -1459,7 +1457,7 @@ public void periodic() { // Pose2d llPose2dElevator = elevator.getPoseFromLL(); // Pose2d llPose2dSlide = slide.getPoseFromLL(); - double canRangeAngle = elevator.getCANrangeAngle(); + // double canRangeAngle = elevator.getCANrangeAngle(); Pose2d blendedPose = vision.getBlendedRobotPoseInTarget(); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index a5ad496..ad21494 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -60,12 +60,12 @@ public void periodic() { Logger.recordOutput("Elevator/EAlgaeReady", isElevatorCloseAlgae()); Logger.recordOutput("Elevator/EScoreReady", isElevatorClose()); Logger.recordOutput("RobotContainer/EBranchOverride", m_branchOverride); - Logger.recordOutput( - "Elevator/AngleFromCANrange", - canRangeAverageFilterAngle.calculate( - Math.toDegrees(-Math.atan((inputs.CANrange1Dis - inputs.CANrange2Dis) / 0.256)))); - Logger.recordOutput( - "Elevator/CANRangeDistance", -(inputs.CANrange1Dis + inputs.CANrange2Dis) / 2.0); + // Logger.recordOutput( + // "Elevator/AngleFromCANrange", + // canRangeAverageFilterAngle.calculate( + // Math.toDegrees(-Math.atan((inputs.CANrange1Dis - inputs.CANrange2Dis) / 0.256)))); + // Logger.recordOutput( + // "Elevator/CANRangeDistance", -(inputs.CANrange1Dis + inputs.CANrange2Dis) / 2.0); Logger.recordOutput("Elevator/EMaxOffset", m_pegged == false); } @@ -382,10 +382,10 @@ public Distance getElevatorPosition() { return inputs.masterPosition; } - public double getCANrangeAngle() { - return canRangeAverageFilterAngle.calculate( - Math.toDegrees(-Math.atan((inputs.CANrange1Dis - inputs.CANrange2Dis) / 0.256))); - } + // public double getCANrangeAngle() { + // return canRangeAverageFilterAngle.calculate( + // Math.toDegrees(-Math.atan((inputs.CANrange1Dis - inputs.CANrange2Dis) / 0.256))); + // } public Trigger isElevatorLevel1Coral() { return new Trigger(() -> m_elevatorSetpoint == Constants.kReefL1); diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index ec3640b..6b647ed 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -34,8 +34,8 @@ public static class ElevatorIOInputs { public double slaveAppliedVolts = 0.0; public double slaveCurrentAmps = 0.0; public double slavePosition = 0.0; - public double CANrange1Dis = 0.0; - public double CANrange2Dis = 0.0; + // public double CANrange1Dis = 0.0; + // public double CANrange2Dis = 0.0; } public default void updateInputs(ElevatorIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index 674e6b3..c66e01e 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -17,17 +17,13 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANrangeConfiguration; -import com.ctre.phoenix6.configs.FovParamsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANrange; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import com.ctre.phoenix6.signals.UpdateModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -56,11 +52,11 @@ public class ElevatorIOTalonFX implements ElevatorIO { private final StatusSignal m_SlaveCurrent; private final StatusSignal m_SlavePosition; - private final CANrange m_CANrange = new CANrange(0, "2481"); - private final CANrange m_CaNrange2 = new CANrange(1, "2481"); + // private final CANrange m_CANrange = new CANrange(0, "2481"); + // private final CANrange m_CaNrange2 = new CANrange(1, "2481"); - private final StatusSignal m_CANrange1Dis; - private final StatusSignal m_CANrange2Dis; + // private final StatusSignal m_CANrange1Dis; + // private final StatusSignal m_CANrange2Dis; public ElevatorIOTalonFX() { @@ -102,16 +98,16 @@ public ElevatorIOTalonFX() { // elevatorRightCfg.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; m_elevatorRightTalonFX.getConfigurator().apply(elevatorRightCfg); - CANrangeConfiguration CANrangecfg = new CANrangeConfiguration(); + // CANrangeConfiguration CANrangecfg = new CANrangeConfiguration(); - CANrangecfg.ProximityParams.MinSignalStrengthForValidMeasurement = 2000; - CANrangecfg.ProximityParams.ProximityThreshold = 0.1; - CANrangecfg.withFovParams(new FovParamsConfigs().withFOVRangeX(6.75).withFOVRangeY(6.75)); + // CANrangecfg.ProximityParams.MinSignalStrengthForValidMeasurement = 2000; + // CANrangecfg.ProximityParams.ProximityThreshold = 0.1; + // CANrangecfg.withFovParams(new FovParamsConfigs().withFOVRangeX(6.75).withFOVRangeY(6.75)); - CANrangecfg.ToFParams.UpdateMode = UpdateModeValue.ShortRange100Hz; + // CANrangecfg.ToFParams.UpdateMode = UpdateModeValue.ShortRange100Hz; - m_CANrange.getConfigurator().apply(CANrangecfg); - m_CaNrange2.getConfigurator().apply(CANrangecfg); + // m_CANrange.getConfigurator().apply(CANrangecfg); + // m_CaNrange2.getConfigurator().apply(CANrangecfg); m_MasterVelocity = m_elevatorLeftTalonFX.getVelocity(); m_MasterAppliedVolts = m_elevatorLeftTalonFX.getMotorVoltage(); @@ -123,8 +119,8 @@ public ElevatorIOTalonFX() { m_SlaveCurrent = m_elevatorRightTalonFX.getStatorCurrent(); m_SlavePosition = m_elevatorRightTalonFX.getRotorPosition(); - m_CANrange1Dis = m_CANrange.getDistance(); - m_CANrange2Dis = m_CaNrange2.getDistance(); + // m_CANrange1Dis = m_CANrange.getDistance(); + // m_CANrange2Dis = m_CaNrange2.getDistance(); m_elevatorRightTalonFX.setControl(new Follower(m_elevatorLeftTalonFX.getDeviceID(), true)); // m_elevatorLeftTalonFX.setPosition(0); @@ -145,12 +141,9 @@ public void updateInputs(ElevatorIOInputs inputs) { var elevatorSlaveStatus = BaseStatusSignal.refreshAll( - m_SlaveVelocity, - m_SlaveAppliedVolts, - m_SlaveCurrent, - m_SlavePosition, - m_CANrange1Dis, - m_CANrange2Dis); + m_SlaveVelocity, m_SlaveAppliedVolts, m_SlaveCurrent, m_SlavePosition); + // m_CANrange1Dis, + // m_CANrange2Dis); inputs.slaveConnected = elevatorSlaveStatus.isOK(); inputs.slaveVelocityRotPerSec = m_SlaveVelocity.getValueAsDouble(); @@ -158,8 +151,8 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.slaveCurrentAmps = m_SlaveCurrent.getValueAsDouble(); inputs.slavePosition = m_SlavePosition.getValueAsDouble(); - inputs.CANrange1Dis = m_CANrange1Dis.getValueAsDouble(); - inputs.CANrange2Dis = m_CANrange2Dis.getValueAsDouble(); + // inputs.CANrange1Dis = m_CANrange1Dis.getValueAsDouble(); + // inputs.CANrange2Dis = m_CANrange2Dis.getValueAsDouble(); } @Override From 8145f88c8fd41c2c558080f0aedfe764100dc4c7 Mon Sep 17 00:00:00 2001 From: Kython89 Date: Fri, 4 Apr 2025 23:31:46 -0500 Subject: [PATCH 04/13] 4 piece working better --- src/main/deploy/pathplanner/paths/F4 1.path | 2 +- src/main/deploy/pathplanner/paths/F4 2.path | 18 +++++++++--------- src/main/deploy/pathplanner/paths/F4 3.path | 10 +++++----- src/main/deploy/pathplanner/paths/F4 5.path | 2 +- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 2 +- 6 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/F4 1.path b/src/main/deploy/pathplanner/paths/F4 1.path index 531424c..74cf451 100644 --- a/src/main/deploy/pathplanner/paths/F4 1.path +++ b/src/main/deploy/pathplanner/paths/F4 1.path @@ -36,7 +36,7 @@ "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 2.0, - "maxAcceleration": 1.2, + "maxAcceleration": 1.15, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/F4 2.path b/src/main/deploy/pathplanner/paths/F4 2.path index 7379048..43150cd 100644 --- a/src/main/deploy/pathplanner/paths/F4 2.path +++ b/src/main/deploy/pathplanner/paths/F4 2.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.548703082604006, - "y": 4.839716329654968 + "x": 2.3958059210526317, + "y": 4.611924342105263 }, "isLocked": false, "linkedName": "F4 1" }, { "anchor": { - "x": 1.3839614129027513, - "y": 5.696731469050455 + "x": 1.4063150104370652, + "y": 5.66 }, "prevControl": { - "x": 1.8024346366726127, - "y": 5.2833140298496595 + "x": 1.8281250000012157, + "y": 5.179605263154684 }, "nextControl": null, "isLocked": false, @@ -31,13 +31,13 @@ "rotationTargets": [ { "waypointRelativePos": 0.6871069182390003, - "rotationDegrees": 140.0 + "rotationDegrees": 136.0 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.65, + "minWaypointRelativePos": 0.7, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 1.25, @@ -95,7 +95,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 139.0 + "rotation": 133.35422167699903 }, "reversed": false, "folder": "Front 4", diff --git a/src/main/deploy/pathplanner/paths/F4 3.path b/src/main/deploy/pathplanner/paths/F4 3.path index 9819008..8529de0 100644 --- a/src/main/deploy/pathplanner/paths/F4 3.path +++ b/src/main/deploy/pathplanner/paths/F4 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.3839614129027513, - "y": 5.696731469050455 + "x": 1.4063150104370652, + "y": 5.66 }, "prevControl": null, "nextControl": { - "x": 1.8774948972539727, - "y": 5.073021915160805 + "x": 1.8998484947882865, + "y": 5.03629044611035 }, "isLocked": false, "linkedName": "F4 2" @@ -79,7 +79,7 @@ "folder": "Front 4", "idealStartingState": { "velocity": 0, - "rotation": 139.0 + "rotation": 133.35422167699903 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F4 5.path b/src/main/deploy/pathplanner/paths/F4 5.path index 4361246..580ba53 100644 --- a/src/main/deploy/pathplanner/paths/F4 5.path +++ b/src/main/deploy/pathplanner/paths/F4 5.path @@ -60,7 +60,7 @@ ], "globalConstraints": { "maxVelocity": 6.0, - "maxAcceleration": 5.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6f0415e..83a29c4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -32,7 +32,7 @@ public final class Constants { public static final Distance kReefL3 = Meters.of(0.92); // 0.9706 public static final Distance kReefL4 = Meters.of(1.61); // 1.5975 public static final Distance kDeAlgaeL2 = Meters.of(0.5); - public static final Distance kDeAlgaeL3 = Meters.of(.9); + public static final Distance kDeAlgaeL3 = Meters.of(.93); public static final Distance kElevatorTroughScorePos = Meters.of(0.08); public static final Distance kElevatorTrough2ScorePos = Meters.of(0.13); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab30e55..14cdd4d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1205,7 +1205,7 @@ private final Command autoIntakeDown() { private final Command autoL3DeAlgeafy() { return Commands.sequence( angulator.angulatorPosStateCmd(4), - slide.setSlidePositionCmd(Meters.of(0.05)), + slide.setSlidePositionCmd(Meters.of(0.06)), new WaitUntilCommand(angulator::isAngulatorSafeToMove), elevator.setM_ElevatorAlignCmd(true), elevator.setM_ElevatorHeightCmd(5), From b73b69098341d69f5927bbdfbff3e9c31e63ccdd Mon Sep 17 00:00:00 2001 From: Kython89 Date: Sat, 5 Apr 2025 23:56:22 -0500 Subject: [PATCH 05/13] small changes --- src/main/java/frc/robot/Constants.java | 9 +- src/main/java/frc/robot/RobotContainer.java | 196 ++++++++++++------ .../robot/subsystems/elevator/Elevator.java | 62 ++++-- .../elevator/ElevatorIOTalonFX.java | 7 +- .../subsystems/shooter/ShooterIOTalonFX.java | 10 +- .../frc/robot/subsystems/slide/Slide.java | 4 +- 6 files changed, 202 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 83a29c4..be130cf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -34,8 +34,7 @@ public final class Constants { public static final Distance kDeAlgaeL2 = Meters.of(0.5); public static final Distance kDeAlgaeL3 = Meters.of(.93); public static final Distance kElevatorTroughScorePos = Meters.of(0.08); - public static final Distance kElevatorTrough2ScorePos = Meters.of(0.13); - + public static final Distance kElevatorTrough2ScorePos = Meters.of(0.12); // 0.10 public static final Double kElevatorL4FudgeFactor = 1.4; public static final Double kElevatorL23FudgeFactor = 1.5; @@ -149,7 +148,7 @@ public final class Constants { public static final Double kShooterMotionMagicJerk = 0.0; public static final Double kShooterScoreSpeed = 8.0; public static final Double kShooterLength = 8.5; - public static final Double kL1ShooterVelo = 31.0; // 15 + public static final Double kL1ShooterVelo = 31.0; // 31 public static final Double kL4ShooterVelo = 60.0; // Only gets to 90 when at 110 public static final Double kL2andL3ShooterVelo = 60.0; // Goes to around 52 @@ -187,8 +186,8 @@ public final class Constants { public static final Double kAngulatorL4 = 47.0; public static final Double kAngulatorDeAlgaeL2 = 45.0; public static final Double kAngulatorTravelPos = 4.5; - public static final Double kAngulatorTroughScorePos = -5.0; - public static final Double kAngulatorTrough2ScorePos = -8.0; + public static final Double kAngulatorTroughScorePos = 0.0; + public static final Double kAngulatorTrough2ScorePos = -8.0; // 8 public static final Double kAngulatorBargeScorePos = 0.0; public static final Double kAngulatorGroundPickUpPos = 61.5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 14cdd4d..82fd8c8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -31,7 +31,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; @@ -399,10 +401,23 @@ private void configureButtonBindings() { new WaitUntilCommand(slide::isElevatorSafeToMoveDown), elevator.setM_ElevatorHeightCmd(0), elevator.setM_ElevatorAlignCmd(false), - elevator.HomeCmd(), - new WaitUntilCommand(slide::isAlignedWithFeedersmallerTolerance), - slide.voltageOutCommand(0.0), - angulator.angulatorPosStateCmd(0)) + new ParallelCommandGroup( + elevator.HomeCmd(), + new SequentialCommandGroup( + new WaitUntilCommand(slide::isElevatorSafeToMoveDown), + new WaitUntilCommand( + () -> elevator.getElevatorPosition().magnitude() < 0.35), + angulator.angulatorPosStateCmd(0))), + + // elevator.HomeCmd() + // .alongWith( + // new WaitUntilCommand(slide::isAlignedWithFeedersmallerTolerance) + // .andThen( + // new WaitUntilCommand( + // () -> elevator.getElevatorPosition().magnitude() < + // 0.2))), + new WaitUntilCommand(slide::isAlignedWithFeederSmallerTolerance), + slide.voltageOutCommand(0.0)) .withName("ElevatorDown")); // Commands Elevator While in Ball Mode, Cancels Slide and Elevator Adjust, but Keeps Shooter @@ -422,7 +437,7 @@ private void configureButtonBindings() { elevator.setM_ElevatorHeightCmd(0), elevator.setM_ElevatorAlignCmd(false), elevator.HomeCmd(), - new WaitUntilCommand(slide::isAlignedWithFeedersmallerTolerance), + new WaitUntilCommand(slide::isAlignedWithFeederSmallerTolerance), slide.voltageOutCommand(0.0), angulator.angulatorPosStateCmd(0)) .withName("ElevatorDownBallMode")); @@ -467,6 +482,7 @@ private void configureButtonBindings() { .onTrue( Commands.sequence( humanCmd(), + elevator.setHeightAlign(false), slide.slideAlignCmd(true), angulator.angulatorPosStateCmd(4), new WaitUntilCommand(angulator::isAngulatorSafeToMove), @@ -524,6 +540,7 @@ private void configureButtonBindings() { .onTrue( Commands.sequence( humanCmd(), + elevator.setHeightAlign(false), slide.slideAlignCmd(true), angulator.angulatorPosStateCmd(4), new WaitUntilCommand(angulator::isAngulatorSafeToMove), @@ -578,13 +595,19 @@ private void configureButtonBindings() { .and(feeder.hasCoral().negate()) .onTrue( Commands.sequence( - humanCmd(), - slide.slideAlignCmd(true), - angulator.angulatorPosStateCmd(4), + humanCmd() + .alongWith( + elevator + .setHeightAlign(false) + .alongWith( + slide + .slideAlignCmd(true) + .alongWith(angulator.angulatorPosStateCmd(4)))), new WaitUntilCommand(angulator::isAngulatorSafeToMove), - elevator.setM_ElevatorHeightCmd(3), - elevator.setM_ElevatorAlignCmd(true), - new WaitUntilCommand(elevator::isElevatorClose), + elevator + .setM_ElevatorHeightCmd(3) + .alongWith(elevator.setM_ElevatorAlignCmd(true)), + new WaitUntilCommand(elevator::isElevatorCloseEnoughToAngle), angulator.angulatorPosStateCmd(2)) .withName("ElevatorL4")); @@ -609,16 +632,37 @@ private void configureButtonBindings() { // Commands Elevator to High Trough Score Height, No Slide or Elevator Auto Adjust operatorController .back() - // .and(operatorController.x()) + .and(operatorController.leftBumper().negate()) .and(feeder.hasCoral().negate()) - .onTrue( - Commands.sequence( - humanCmd(), - elevator.setM_ElevatorHeightCmd(8), - elevator.setM_ElevatorAlignCmd(true), - angulator.angulatorPosStateCmd(8), - slide.slideAlignCmd(false)) - .withName("ElevatorTroughScoreHeight2")); + .onTrue(troughModeCmd().withName("ElevatorTroughModeCmd")); + + operatorController.back().and(operatorController.leftBumper()).onTrue(troughMode1Cmd()); + + // operatorController + // .back() + // .and(elevatorTroughPosTrigger().negate()) + // .and(elevatorTrough2PosTrigger().negate()) + // .onTrue( + // Commands.sequence( + // humanCmd(), + // elevator.setM_ElevatorHeightCmd(7), + // elevator.setM_ElevatorAlignCmd(true), + // angulator.angulatorPosStateCmd(5), + // slide.slideAlignCmd(false)) + // .withName("ElevatorTroughScoreHeight")); + + // operatorController + // .back() + // .and(elevatorTroughPosTrigger().debounce(0.25)) + // .and(feeder.hasCoral().negate()) + // .onTrue( + // Commands.sequence( + // humanCmd(), + // elevator.setM_ElevatorHeightCmd(8), + // elevator.setM_ElevatorAlignCmd(true), + // angulator.angulatorPosStateCmd(8), + // slide.slideAlignCmd(false)) + // .withName("ElevatorTrough2ScoreHeight")); // Commands Elevator to Low Trough Score Height, No Slide or Elevator Auto Adjust // operatorController @@ -676,14 +720,35 @@ private void configureButtonBindings() { // Trough Score operatorController .rightBumper() - .and( - elevatorHomePosTrigger().or(elevatorTroughPosTrigger()).or(elevatorTrough2PosTrigger())) + .and(elevatorTroughPosTrigger()) + .and(operatorController.leftBumper().negate()) + .onTrue( + Commands.sequence( + shooter.setShooterVelocityCmd(Constants.kL1ShooterVelo), + new WaitCommand(0.1), + slide + .setSlidePositionCmd(Meters.of(0.04)) + .alongWith(shooter.setHasCoralCmd(false))) + .withName("TroughScore")); + + operatorController + .rightBumper() + .and(elevatorTrough2PosTrigger()) .and(operatorController.leftBumper().negate()) + .onTrue( + Commands.sequence( + shooter + .setShooterVelocityCmd(Constants.kL1ShooterVelo) + .alongWith(shooter.setHasCoralCmd(false))) + .withName("Trough2Score")); + + operatorController + .rightBumper() + .and(elevatorHomePosTrigger()) .onTrue( shooter .setShooterVelocityCmd(Constants.kL1ShooterVelo) - .alongWith(shooter.setHasCoralCmd(false)) - .withName("TroughScore")); + .alongWith(shooter.setHasCoralCmd(false))); // L2 and L3 Score operatorController @@ -851,6 +916,8 @@ private void configureTriggerBindings() { .and(elevator.isAlignedWithFeederTrigger()) .and(feeder.hasCoral()) .and(angulator.isAlignedWithFeederTrigger()) + .and(elevatorHomePosTrigger()) // added to make sure elevator setpoint AND position are at + // zero, when feeding past beambreak .onTrue( Commands.sequence(feeder.setFeederRollerVoltsCmd(4), shooter.setShooterVelocityCmd(10)) .withName("startMovingCoralToShooter")); @@ -864,7 +931,8 @@ private void configureTriggerBindings() { .isAlignedWithFeederTrigger() .negate() .or(elevator.isAlignedWithFeederTrigger().negate()) - .or(angulator.isAlignedWithFeederTrigger().negate())) + .or(angulator.isAlignedWithFeederTrigger().negate()) + .or(elevatorHomePosTrigger().negate())) .onTrue(feeder.setFeederPosZero().withName("stopFeederIfAnythingIsntAligned")); feeder @@ -897,6 +965,15 @@ private void configureTriggerBindings() { // .slideAlignCmd(false) // .andThen(slide.setSlidePositionCmd(Constants.kAlgaeSlidePosition))); + coralBetweenCoralAndReefTrigger() + .and(robotNotMovingTrigger()) + .debounce(0.05) + .onTrue( + elevator + .setHeightAlign(true) + .alongWith(new WaitCommand(0.1)) + .withName("EnableElevatorHeightTracking")); + // // Feed position when down. elevator .isAngulatorSafeToMoveBackTrigger() @@ -916,7 +993,7 @@ private void configureTriggerBindings() { // slide.setSlidePositionCmd(Constants.kSlideFeederPosition))); Trigger autoFireTriggerL4 = elevator - .isElevatorCloseTrigger() + .isElevatorReadyToScore() .and(elevatorL4PosTrigger()) .and(slide.isSlideReadyToScoreTrigger()) .and(angulator.readyToScoreTrigger()) @@ -926,15 +1003,15 @@ private void configureTriggerBindings() { .and(vision.angleTooLargeTrigger) .and(shooter.isShooterZAccelLowEnough()) // .and(drive.isDriveAccelLowEnoughToScore().debounce(0.05)) // was 0.25 - .and(robotMovingTrigger()); // was 0.06 + .and(robotNotMovingTrigger()); // was 0.06 Trigger autoFireTriggerL2and3 = elevator - .isElevatorCloseTrigger() + .isElevatorReadyToScore() .and(slide.isSlideReadyToScoreTrigger()) .and(angulator.readyToScoreTrigger()) .and(seesTarget()) - .and(robotMovingTrigger()) + .and(robotNotMovingTrigger()) .and(elevatorL2PosTrigger().or(elevatorL3PosTrigger())); operatorController @@ -1003,6 +1080,11 @@ private final Command driveSetFieldRelativeCommand(Boolean fieldRelative) { }); } + private final Trigger coralBetweenCoralAndReefTrigger() { + return new Trigger( + () -> slide.coralToReefBranch() < Constants.kElevatorHeightMaxTrackingDistance.magnitude()); + } + private final Boolean elevatorHomePos() { return elevator.m_elevatorHeight == 0; } @@ -1069,6 +1151,28 @@ public final Trigger seesTarget() { () -> LimelightHelpers.getTV("limelight-fl") || LimelightHelpers.getTV("limelight-fr")); } + public Command troughModeCmd() { + return new ConditionalCommand(troughMode2Cmd(), troughMode1Cmd(), () -> elevatorTroughPos()); + } + + public final Command troughMode1Cmd() { + return Commands.sequence( + humanCmd(), + elevator.setM_ElevatorHeightCmd(7), + elevator.setM_ElevatorAlignCmd(true), + angulator.angulatorPosStateCmd(5), + slide.slideAlignCmd(false)); + } + + public final Command troughMode2Cmd() { + return Commands.sequence( + humanCmd(), + elevator.setM_ElevatorHeightCmd(8), + elevator.setM_ElevatorAlignCmd(true), + angulator.angulatorPosStateCmd(8), + slide.slideAlignCmd(false)); + } + private final Command autoGamePieceHold() { return shooter.positionVoltageNoMovement(); } @@ -1231,7 +1335,7 @@ private final Command humanCmd() { return new InstantCommand(() -> {}, dummy); } - private Trigger robotMovingTrigger() { + private Trigger robotNotMovingTrigger() { return new Trigger( () -> (Math.abs(drive.getChassisSpeeds().vxMetersPerSecond) < 0.1) @@ -1476,36 +1580,6 @@ public void periodic() { // slide.alignWithReefLatencyCompensated( // drive.getPose(), latencyCompensatedTargetSlide, elevator.elevatorPos()); angulator.setAngulatorPosState(); - // if ((elevator - // .isElevatorCloseTrigger() - // .and(slide.isSlideReadyToScoreTrigger()) - // .and(angulator.readyToScoreTrigger()) - // .and(seesTarget()) - // .and(shooter.isShooterVelocityLowEnoughToScore()) - // .and(elevator.peggedTrigger.negate()) - // .and(robotMovingTrigger()) - // .debounce(0.05)) - // .getAsBoolean()) { - // m_CANdle.setLEDs(255, 255, 255); - // } else if ((elevator - // .isElevatorCloseTrigger() - // .and(slide.isSlideReadyToScoreTrigger()) - // .and(angulator.readyToScoreTrigger()) - // .and(seesTarget()) - // .and(elevator.peggedTrigger.negate()) - // .and(shooter.isShooterVelocityLowEnoughToScore())) - // .and(robotMovingTrigger()) - // .getAsBoolean()) { - // m_CANdle.setLEDs(255, 165, 0); - // } else { - // m_CANdle.setLEDs(255, 0, 0); - // } - - // if (slide.isSlideReadyToScore() && elevator.isElevatorClose() && angulator.readyToScore()) { - // m_CANdle.setLEDs(255, 255, 255); - // } else { - // m_CANdle.setLEDs(255, 0, 0); - // } - Logger.recordOutput("RobotContainer/RobotMoving", robotMovingTrigger()); + Logger.recordOutput("RobotContainer/RobotMoving", robotNotMovingTrigger()); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index ad21494..7ec204d 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -35,6 +35,7 @@ public class Elevator extends SubsystemBase { public ElevatorOverride m_branchOverride = ElevatorOverride.AUTO; public Boolean m_pegged = false; public final Trigger peggedTrigger = new Trigger(() -> m_pegged); + public Boolean m_setHeightAlign = true; public LinearFilter canRangeAverageFilterAngle = LinearFilter.movingAverage(9); @@ -208,6 +209,10 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { m_heightDistanceOffset = m_heightDistanceOffset.times(Constants.kElevatorL23FudgeFactor); } + // if (m_setHeightAlign == false) { + // m_heightDistanceOffset = Meters.of(0.0); + // } + if (DriverStation.isAutonomousEnabled()) { m_heightDistanceOffset = Meters.of(0.0063); } @@ -216,25 +221,39 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { // Meters.of(m_heightOffsetFilter.calculate(m_heightDistanceOffset.magnitude())); if (m_elevatorHeight == 1) { + if (m_setHeightAlign == false) { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2); + } else { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2.plus(m_heightDistanceOffset)); + } m_pegged = false; - m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2.plus(m_heightDistanceOffset)); } if (m_elevatorHeight == 2) { + if (m_setHeightAlign == false) { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3); + } else { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3.plus(m_heightDistanceOffset)); + } m_pegged = false; - m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3.plus(m_heightDistanceOffset)); } if (m_elevatorHeight == 3) { - if ((Constants.kReefL4.plus(m_heightDistanceOffset)).magnitude() - > (Constants.kElevatorForwardSoftLimitThreshold)) { - m_pegged = true; - m_ElevatorIO.setElevatorPositionMeters( - Meters.of(Constants.kElevatorForwardSoftLimitThreshold)); - m_elevatorSetpoint = - Meters.of(Constants.kElevatorForwardSoftLimitThreshold).minus(m_heightDistanceOffset); - } else { - m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4.plus(m_heightDistanceOffset)); - m_elevatorSetpoint = Constants.kReefL4; + if (m_setHeightAlign == false) { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4); m_pegged = false; + } else { + if ((Constants.kReefL4.plus(m_heightDistanceOffset)).magnitude() + > (Constants.kElevatorForwardSoftLimitThreshold)) { + m_pegged = true; + m_ElevatorIO.setElevatorPositionMeters( + Meters.of(Constants.kElevatorForwardSoftLimitThreshold)); + m_elevatorSetpoint = + Meters.of(Constants.kElevatorForwardSoftLimitThreshold) + .minus(m_heightDistanceOffset); + } else { + m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4.plus(m_heightDistanceOffset)); + m_elevatorSetpoint = Constants.kReefL4; + m_pegged = false; + } } // m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4.plus(m_heightDistanceOffset)); @@ -291,6 +310,10 @@ public void setM_ElevatorHeight(int num) { } } + public Command setHeightAlign(boolean yes) { + return Commands.runOnce(() -> m_setHeightAlign = yes); + } + public Command setM_ElevatorHeightCmd(int num) { return Commands.runOnce(() -> setM_ElevatorHeight(num)); } @@ -346,11 +369,24 @@ public Boolean isAlignedWithFeeder() { public Boolean isElevatorClose() { return MathUtil.isNear( - m_elevatorSetpoint.plus(m_heightDistanceOffset).magnitude(), + m_elevatorSetpoint.magnitude(), inputs.masterPosition.magnitude(), Constants.kElevatorClosetoTarget); } + public Boolean isElevatorCloseEnoughToAngle() { + return MathUtil.isNear(m_elevatorSetpoint.magnitude(), inputs.masterPosition.magnitude(), 0.75); + } + + public Trigger isElevatorReadyToScore() { + return new Trigger( + () -> + MathUtil.isNear( + m_elevatorSetpoint.plus(m_heightDistanceOffset).magnitude(), + inputs.masterPosition.magnitude(), + Constants.kElevatorClosetoTarget)); + } + public Boolean isElevatorCloseAlgae() { return MathUtil.isNear( m_elevatorSetpoint.magnitude(), diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index c66e01e..e2990e8 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -159,13 +159,14 @@ public void updateInputs(ElevatorIOInputs inputs) { public Command setElevatorHomeCmd() { return new FunctionalCommand( () -> { - m_elevatorLeftTalonFX.setControl(m_MotionMagicVoltage.withPosition(0)); + m_elevatorLeftTalonFX.setControl(m_MotionMagicVoltage.withPosition(0.04)); }, () -> {}, interrupted -> { - m_elevatorLeftTalonFX.setControl(m_Voltage.withOutput(0)); + // m_elevatorLeftTalonFX.setControl(m_Voltage.withOutput(0)); + m_elevatorLeftTalonFX.setControl(m_MotionMagicVoltage.withPosition(0.0)); }, - () -> (m_MasterPosition.getValueAsDouble()) < 0.01); + () -> (m_MasterPosition.getValueAsDouble()) < 0.04); } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 064616c..5538e96 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -74,7 +74,7 @@ public ShooterIOTalonFX() { shooterCfg.MotionMagic.MotionMagicAcceleration = Constants.kShooterMotionMagicAcceleration; shooterCfg.MotionMagic.MotionMagicCruiseVelocity = Constants.kShooterMotionMagicCruiseVelocity; shooterCfg.MotionMagic.MotionMagicJerk = Constants.kShooterMotionMagicJerk; - shooterCfg.CurrentLimits.StatorCurrentLimit = 50.0; + shooterCfg.CurrentLimits.StatorCurrentLimit = 60.0; m_shooterTalonFX.getConfigurator().apply(shooterCfg); m_Velocity = m_shooterTalonFX.getVelocity(); @@ -89,7 +89,13 @@ public ShooterIOTalonFX() { shooterAccelY = pigeon.getAccelerationY(); shooterAccelZ = pigeon.getAccelerationZ(); BaseStatusSignal.setUpdateFrequencyForAll( - 30, shooterXVelocity, shooterYVelocity, shooterZVelocity); + 70, + shooterXVelocity, + shooterYVelocity, + shooterZVelocity, + shooterAccelX, + shooterAccelY, + shooterAccelZ); } @Override diff --git a/src/main/java/frc/robot/subsystems/slide/Slide.java b/src/main/java/frc/robot/subsystems/slide/Slide.java index ac6a979..767dc1f 100644 --- a/src/main/java/frc/robot/subsystems/slide/Slide.java +++ b/src/main/java/frc/robot/subsystems/slide/Slide.java @@ -255,7 +255,7 @@ public void commonSlideMove(Pose2d pose, Double elevatorHeight, double theta) { m_SlideIO.setSlideAlignPosition(m_slidePos); // if distance to the tip of reef branch is less than 16 inches do nothing - } else if (slideLegA > .406) { + } else if (Math.abs(m_coralToReefBranch) > .35) { // checks to make sure setpoint is greater than a tenth of an inch different from last // setpoint befor emovement @@ -360,7 +360,7 @@ public Boolean isAlignedWithFeeder() { Constants.kSlideFeederPositionTolerance); } - public Boolean isAlignedWithFeedersmallerTolerance() { + public Boolean isAlignedWithFeederSmallerTolerance() { return MathUtil.isNear( Constants.kSlideFeederPosition.magnitude(), inputs.position.magnitude(), From 523cc2c8bc0487fbc14f9335b6dd89a06d8dc217 Mon Sep 17 00:00:00 2001 From: Kython89 Date: Mon, 7 Apr 2025 22:59:28 -0500 Subject: [PATCH 06/13] auto changes --- .../deploy/pathplanner/autos/Processor 4.auto | 61 ++++++++++ src/main/deploy/pathplanner/paths/LCS 1.path | 86 ++++++++++++++ src/main/deploy/pathplanner/paths/LCS 2.path | 107 ++++++++++++++++++ src/main/deploy/pathplanner/paths/LCS 3.path | 80 +++++++++++++ src/main/deploy/pathplanner/paths/LCS 4.path | 102 +++++++++++++++++ src/main/deploy/pathplanner/paths/LCS 5.path | 80 +++++++++++++ src/main/deploy/pathplanner/paths/LCS 6.path | 107 ++++++++++++++++++ src/main/deploy/pathplanner/paths/LCS 7.path | 85 ++++++++++++++ src/main/deploy/pathplanner/settings.json | 5 +- src/main/java/frc/robot/Constants.java | 18 +-- src/main/java/frc/robot/RobotContainer.java | 58 +++++++++- .../subsystems/shooter/ShooterIOTalonFX.java | 2 +- 12 files changed, 778 insertions(+), 13 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Processor 4.auto create mode 100644 src/main/deploy/pathplanner/paths/LCS 1.path create mode 100644 src/main/deploy/pathplanner/paths/LCS 2.path create mode 100644 src/main/deploy/pathplanner/paths/LCS 3.path create mode 100644 src/main/deploy/pathplanner/paths/LCS 4.path create mode 100644 src/main/deploy/pathplanner/paths/LCS 5.path create mode 100644 src/main/deploy/pathplanner/paths/LCS 6.path create mode 100644 src/main/deploy/pathplanner/paths/LCS 7.path diff --git a/src/main/deploy/pathplanner/autos/Processor 4.auto b/src/main/deploy/pathplanner/autos/Processor 4.auto new file mode 100644 index 0000000..728c16b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Processor 4.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LCS 1" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 2" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 3" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 4" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 5" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 6" + } + }, + { + "type": "path", + "data": { + "pathName": "LCS 7" + } + }, + { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 1.path b/src/main/deploy/pathplanner/paths/LCS 1.path new file mode 100644 index 0000000..b807fe1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 1.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.946471406470677, + "y": 2.464 + }, + "prevControl": null, + "nextControl": { + "x": 6.564270068533928, + "y": 2.5387007677177387 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.136, + "y": 2.8714275049357614 + }, + "prevControl": { + "x": 5.431882955072022, + "y": 2.8088519902918905 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 1" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "autoIntakeDown", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, + { + "name": "Processor4Piece1", + "waypointRelativePos": 0.4021543985637337, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor4Piece1" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -60.70863782901576 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 2.path b/src/main/deploy/pathplanner/paths/LCS 2.path new file mode 100644 index 0000000..fc9b955 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 2.path @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.13642948781766, + "y": 2.8714275049357614 + }, + "prevControl": null, + "nextControl": { + "x": 4.031496710526316, + "y": 2.177631578947369 + }, + "isLocked": false, + "linkedName": "LCS 1" + }, + { + "anchor": { + "x": 2.1484847537019114, + "y": 0.8010986125110935 + }, + "prevControl": { + "x": 3.178007780017701, + "y": 1.5708354546163574 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6273584905660404, + "rotationDegrees": -132.7479460740572 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "5PieceScore", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + }, + { + "name": "ElevatorDown", + "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "ElevatorDown" + } + } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.1, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -132.7688253919688 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -60.70863782901576 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 3.path b/src/main/deploy/pathplanner/paths/LCS 3.path new file mode 100644 index 0000000..04c2ffd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 3.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.1484847537019114, + "y": 0.8010986125110935 + }, + "prevControl": null, + "nextControl": { + "x": 2.77186065723731, + "y": 1.5607772434122547 + }, + "isLocked": false, + "linkedName": "LCS 2" + }, + { + "anchor": { + "x": 3.691938256400978, + "y": 2.9554844604667143 + }, + "prevControl": { + "x": 3.4920278347004228, + "y": 2.704133780054894 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 3" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.65, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.1, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Processor4Piece2", + "waypointRelativePos": 0.5026929982046682, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor4Piece2" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -132.7688253919688 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 4.path b/src/main/deploy/pathplanner/paths/LCS 4.path new file mode 100644 index 0000000..220fddb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 4.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.691938256400978, + "y": 2.9554844604667143 + }, + "prevControl": null, + "nextControl": { + "x": 3.303650313508808, + "y": 2.4371241112668884 + }, + "isLocked": false, + "linkedName": "LCS 3" + }, + { + "anchor": { + "x": 2.1484847537019114, + "y": 0.8010986125110935 + }, + "prevControl": { + "x": 1.9348617578898692, + "y": 0.5468967666219559 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "5PieceScore", + "waypointRelativePos": 0.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + }, + { + "name": "ElevatorDown", + "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "ElevatorDown" + } + } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.2692998204667869, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -132.7688253919688 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 5.path b/src/main/deploy/pathplanner/paths/LCS 5.path new file mode 100644 index 0000000..d0def1f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 5.path @@ -0,0 +1,80 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.1484847537019114, + "y": 0.8010986125110935 + }, + "prevControl": null, + "nextControl": { + "x": 2.504234805043925, + "y": 1.1204025396119586 + }, + "isLocked": false, + "linkedName": "LCS 2" + }, + { + "anchor": { + "x": 3.9330626764352288, + "y": 2.812394568574623 + }, + "prevControl": { + "x": 3.048423720292707, + "y": 1.9589872143388218 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 4" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.75, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.1, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Processor4Piece3", + "waypointRelativePos": 0.6535008976660683, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor4Piece3" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -132.7688253919688 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 6.path b/src/main/deploy/pathplanner/paths/LCS 6.path new file mode 100644 index 0000000..02b18b5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 6.path @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9330626764352288, + "y": 2.812394568574623 + }, + "prevControl": null, + "nextControl": { + "x": 2.951116162831399, + "y": 1.8827786337248869 + }, + "isLocked": false, + "linkedName": "LCS 4" + }, + { + "anchor": { + "x": 2.1484847537019114, + "y": 0.8010986125110935 + }, + "prevControl": { + "x": 2.5648973533077983, + "y": 1.187595871584342 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LCS 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6053459119496889, + "rotationDegrees": -135.0824807086794 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "5PieceScore", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + }, + { + "name": "ElevatorDown", + "waypointRelativePos": 0.05, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "ElevatorDown" + } + } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.33572710951526064, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -132.7688253919688 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 7.path b/src/main/deploy/pathplanner/paths/LCS 7.path new file mode 100644 index 0000000..a827343 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LCS 7.path @@ -0,0 +1,85 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.1484847537019114, + "y": 0.8010986125110935 + }, + "prevControl": null, + "nextControl": { + "x": 2.9374650168578578, + "y": 1.3495361125158465 + }, + "isLocked": false, + "linkedName": "LCS 2" + }, + { + "anchor": { + "x": 4.92157171856577, + "y": 2.7284019117633864 + }, + "prevControl": { + "x": 4.288106502758831, + "y": 2.355304020390542 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6729559748427736, + "rotationDegrees": -61.81181756103639 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 1.1, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Processor4Piece4", + "waypointRelativePos": 0.6535008976660687, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Processor4Piece4" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 4.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -61.04900479253321 + }, + "reversed": false, + "folder": "4 Piece Processor side", + "idealStartingState": { + "velocity": 0, + "rotation": -132.7688253919688 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 0e9a996..7e19274 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -5,10 +5,11 @@ "pathFolders": [ "2 L4 Auto", "3 L4 Auto", + "4 Piece Processor side", "5 Piece Cs", + "Front 4", "Human Load NonProcessor", - "Lollipop 4", - "Front 4" + "Lollipop 4" ], "autoFolders": [], "defaultMaxVel": 3.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index be130cf..57cfcc4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,8 +31,8 @@ public final class Constants { public static final Distance kReefL2 = Meters.of(0.5192); // 0.5492 public static final Distance kReefL3 = Meters.of(0.92); // 0.9706 public static final Distance kReefL4 = Meters.of(1.61); // 1.5975 - public static final Distance kDeAlgaeL2 = Meters.of(0.5); - public static final Distance kDeAlgaeL3 = Meters.of(.93); + public static final Distance kDeAlgaeL2 = Meters.of(0.55); + public static final Distance kDeAlgaeL3 = Meters.of(.98); public static final Distance kElevatorTroughScorePos = Meters.of(0.08); public static final Distance kElevatorTrough2ScorePos = Meters.of(0.12); // 0.10 public static final Double kElevatorL4FudgeFactor = 1.4; @@ -51,15 +51,15 @@ public final class Constants { public static final Integer kShooterPigeon2 = 2; // Intake Pivot configurations - public static final Double kIntakePivotkP = 0.0; + public static final Double kIntakePivotkP = 10.0; public static final Double kIntakePivotkI = 0.0; public static final Double kIntakePivotkD = 0.0; - public static final Double kIntakePivotkV = 0.127; + public static final Double kIntakePivotkV = 0.105; public static final Double kIntakePivotkA = 0.0; public static final Double kIntakePivotkS = 0.0; public static final Double kIntakePivotMotionMagicCruiseVelocity = 94.0; // Max speed at 12V - public static final Double kIntakePivotMotionMagicAcceleration = 200.0; - public static final Double kIntakePivotMotionMagicJerk = 0.0; + public static final Double kIntakePivotMotionMagicAcceleration = 400.0; + public static final Double kIntakePivotMotionMagicJerk = 2000.0; public static final Double kIntakePivotForwardSoftLimitThreshold = 30.9; public static final Double kIntakePivotReverseSoftLimitThreshold = 0.0; public static final Boolean kIntakePivotForwardSoftLimitEnable = true; @@ -148,9 +148,9 @@ public final class Constants { public static final Double kShooterMotionMagicJerk = 0.0; public static final Double kShooterScoreSpeed = 8.0; public static final Double kShooterLength = 8.5; - public static final Double kL1ShooterVelo = 31.0; // 31 + public static final Double kL1ShooterVelo = 40.0; // 31 public static final Double kL4ShooterVelo = 60.0; // Only gets to 90 when at 110 - public static final Double kL2andL3ShooterVelo = 60.0; // Goes to around 52 + public static final Double kL2andL3ShooterVelo = 60.0; // Goes to around 52 at 60 // climber configurations public static final Double kClimberkP = 100.0; @@ -188,7 +188,7 @@ public final class Constants { public static final Double kAngulatorTravelPos = 4.5; public static final Double kAngulatorTroughScorePos = 0.0; public static final Double kAngulatorTrough2ScorePos = -8.0; // 8 - public static final Double kAngulatorBargeScorePos = 0.0; + public static final Double kAngulatorBargeScorePos = -3.0; public static final Double kAngulatorGroundPickUpPos = 61.5; public static final Double kAngulatorFeederPosition = -20.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 82fd8c8..c3dd25c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -218,6 +218,10 @@ public RobotContainer() { NamedCommands.registerCommand("autoL3DeAlgeafy", autoL3DeAlgeafy()); NamedCommands.registerCommand("ReefL3", autoL3()); NamedCommands.registerCommand("autoBallDrop", autoBallDrop()); + NamedCommands.registerCommand("Processor4Piece1", processorAutoPiece1()); + NamedCommands.registerCommand("Processor4Piece2", processorAutoPiece2()); + NamedCommands.registerCommand("Processor4Piece3", processorAutoPiece3()); + NamedCommands.registerCommand("Processor4Piece4", processorAutoPiece4()); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -471,6 +475,19 @@ private void configureButtonBindings() { new WaitUntilCommand(slide::isSlideCloseToTarget), angulator.angulatorPosStateCmd(7)) .withName("GroundAlgaePickUp")); + // operatorController + // .leftStick() + // .onTrue( + // Commands.sequence( + // humanCmd(), + // angulator.angulatorPosStateCmd(4), + // new WaitUntilCommand(angulator::isAngulatorSafeToMove), + // slide.setSlidePositionCmd(Constants.kCenterPos), + // new WaitUntilCommand(slide::isSlideCloseToTarget), + // elevator.setM_ElevatorAlignCmd(8), + + // ) + // ) // Commands Elevator to L2 Coral Height, Slide and Elevator Auto Adjust operatorController @@ -808,7 +825,7 @@ private void configureButtonBindings() { operatorController .leftBumper() - .onTrue(shooter.setShooterVelocityCmd(-70).withName("AlgaePickup")); + .onTrue(shooter.setShooterVelocityCmd(-100).withName("AlgaePickup")); // Score In Barge operatorController @@ -1259,6 +1276,45 @@ private final Command autoL1() { angulator.angulatorPosStateCmd(0)); } + private final Command processorAutoPiece1() { + return Commands.sequence( + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.))); + } + + private final Command processorAutoPiece2() { + return Commands.sequence( + new WaitUntilCommand(() -> shooter.hasCoral().getAsBoolean()), + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.17))); + } + + private final Command processorAutoPiece3() { + return Commands.sequence( + new WaitUntilCommand(() -> shooter.hasCoral().getAsBoolean()), + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.20))); + } + + private final Command processorAutoPiece4() { + return Commands.sequence( + new WaitUntilCommand(() -> shooter.hasCoral().getAsBoolean()), + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.25))); + } + private final Command autoScoreFor3Piece() { return new SequentialCommandGroup( new WaitUntilCommand( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 5538e96..46fd1c8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -74,7 +74,7 @@ public ShooterIOTalonFX() { shooterCfg.MotionMagic.MotionMagicAcceleration = Constants.kShooterMotionMagicAcceleration; shooterCfg.MotionMagic.MotionMagicCruiseVelocity = Constants.kShooterMotionMagicCruiseVelocity; shooterCfg.MotionMagic.MotionMagicJerk = Constants.kShooterMotionMagicJerk; - shooterCfg.CurrentLimits.StatorCurrentLimit = 60.0; + shooterCfg.CurrentLimits.StatorCurrentLimit = 50.0; m_shooterTalonFX.getConfigurator().apply(shooterCfg); m_Velocity = m_shooterTalonFX.getVelocity(); From c60f0e33003205133d077085d4be2c695b58f376 Mon Sep 17 00:00:00 2001 From: Kython89 Date: Thu, 10 Apr 2025 16:54:38 -0500 Subject: [PATCH 07/13] 4/8 --- .../deploy/pathplanner/autos/Processor 4.auto | 12 +++++++ src/main/deploy/pathplanner/paths/F4 1.path | 4 +-- src/main/deploy/pathplanner/paths/F4 2.path | 4 +-- src/main/deploy/pathplanner/paths/LCS 1.path | 8 ++--- src/main/deploy/pathplanner/paths/LCS 2.path | 22 ++++++------- src/main/deploy/pathplanner/paths/LCS 3.path | 33 +++++++++++-------- src/main/deploy/pathplanner/paths/LCS 4.path | 29 +++++----------- src/main/deploy/pathplanner/paths/LCS 5.path | 33 +++++++++++-------- src/main/deploy/pathplanner/paths/LCS 6.path | 31 ++++++----------- src/main/deploy/pathplanner/paths/LCS 7.path | 28 ++++++++-------- src/main/java/frc/robot/Constants.java | 7 ++-- src/main/java/frc/robot/RobotContainer.java | 18 ++++++---- 12 files changed, 119 insertions(+), 110 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/Processor 4.auto b/src/main/deploy/pathplanner/autos/Processor 4.auto index 728c16b..0125733 100644 --- a/src/main/deploy/pathplanner/autos/Processor 4.auto +++ b/src/main/deploy/pathplanner/autos/Processor 4.auto @@ -22,6 +22,12 @@ "pathName": "LCS 3" } }, + { + "type": "named", + "data": { + "name": "5PieceScore" + } + }, { "type": "path", "data": { @@ -34,6 +40,12 @@ "pathName": "LCS 5" } }, + { + "type": "named", + "data": { + "name": "5PieceScore" + } + }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/paths/F4 1.path b/src/main/deploy/pathplanner/paths/F4 1.path index 74cf451..87a07e2 100644 --- a/src/main/deploy/pathplanner/paths/F4 1.path +++ b/src/main/deploy/pathplanner/paths/F4 1.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 3.1548708457619004, + "x": 3.175, "y": 4.358630803339179 }, "prevControl": { - "x": 3.183735977340848, + "x": 3.2038651315789473, "y": 5.3015584349181255 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/F4 2.path b/src/main/deploy/pathplanner/paths/F4 2.path index 43150cd..a962256 100644 --- a/src/main/deploy/pathplanner/paths/F4 2.path +++ b/src/main/deploy/pathplanner/paths/F4 2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 3.1548708457619004, + "x": 3.175, "y": 4.358630803339179 }, "prevControl": null, "nextControl": { - "x": 2.3958059210526317, + "x": 2.415935075290731, "y": 4.611924342105263 }, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LCS 1.path b/src/main/deploy/pathplanner/paths/LCS 1.path index b807fe1..1f304f9 100644 --- a/src/main/deploy/pathplanner/paths/LCS 1.path +++ b/src/main/deploy/pathplanner/paths/LCS 1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.136, - "y": 2.8714275049357614 + "x": 5.179216737944418, + "y": 2.8955872013570754 }, "prevControl": { - "x": 5.431882955072022, - "y": 2.8088519902918905 + "x": 5.47509969301644, + "y": 2.8330116867132045 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LCS 2.path b/src/main/deploy/pathplanner/paths/LCS 2.path index fc9b955..90b889e 100644 --- a/src/main/deploy/pathplanner/paths/LCS 2.path +++ b/src/main/deploy/pathplanner/paths/LCS 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.13642948781766, - "y": 2.8714275049357614 + "x": 5.179216737944418, + "y": 2.8955872013570754 }, "prevControl": null, "nextControl": { - "x": 4.031496710526316, - "y": 2.177631578947369 + "x": 3.8294407894736837, + "y": 2.2546052631578943 }, "isLocked": false, "linkedName": "LCS 1" }, { "anchor": { - "x": 2.1484847537019114, - "y": 0.8010986125110935 + "x": 2.021, + "y": 0.7994156169734083 }, "prevControl": { - "x": 3.178007780017701, - "y": 1.5708354546163574 + "x": 3.0216578947368418, + "y": 1.790451801186588 }, "nextControl": null, "isLocked": false, @@ -30,7 +30,7 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.6273584905660404, + "waypointRelativePos": 0.6776729559748446, "rotationDegrees": -132.7479460740572 } ], @@ -87,7 +87,7 @@ ], "globalConstraints": { "maxVelocity": 3.5, - "maxAcceleration": 3.0, + "maxAcceleration": 2.8, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -95,7 +95,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -132.7688253919688 + "rotation": -128.6116599476519 }, "reversed": false, "folder": "4 Piece Processor side", diff --git a/src/main/deploy/pathplanner/paths/LCS 3.path b/src/main/deploy/pathplanner/paths/LCS 3.path index 04c2ffd..7ed10eb 100644 --- a/src/main/deploy/pathplanner/paths/LCS 3.path +++ b/src/main/deploy/pathplanner/paths/LCS 3.path @@ -3,40 +3,45 @@ "waypoints": [ { "anchor": { - "x": 2.1484847537019114, - "y": 0.8010986125110935 + "x": 2.021, + "y": 0.7994156169734083 }, "prevControl": null, "nextControl": { - "x": 2.77186065723731, - "y": 1.5607772434122547 + "x": 2.6443759035353986, + "y": 1.5590942478745697 }, "isLocked": false, "linkedName": "LCS 2" }, { "anchor": { - "x": 3.691938256400978, - "y": 2.9554844604667143 + "x": 3.8146166810247983, + "y": 2.8987628231340046 }, "prevControl": { - "x": 3.4920278347004228, - "y": 2.704133780054894 + "x": 3.614706259324243, + "y": 2.6474121427221844 }, "nextControl": null, "isLocked": false, "linkedName": "LCS 3" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.8317610062893104, + "rotationDegrees": -119.99999999999999 + } + ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.65, + "minWaypointRelativePos": 0.75, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.1, + "maxAcceleration": 1.35, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -48,7 +53,7 @@ "eventMarkers": [ { "name": "Processor4Piece2", - "waypointRelativePos": 0.5026929982046682, + "waypointRelativePos": 0.33, "endWaypointRelativePos": null, "command": { "type": "named", @@ -60,7 +65,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -74,7 +79,7 @@ "folder": "4 Piece Processor side", "idealStartingState": { "velocity": 0, - "rotation": -132.7688253919688 + "rotation": -128.6116599476519 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 4.path b/src/main/deploy/pathplanner/paths/LCS 4.path index 220fddb..63d0d9b 100644 --- a/src/main/deploy/pathplanner/paths/LCS 4.path +++ b/src/main/deploy/pathplanner/paths/LCS 4.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.691938256400978, - "y": 2.9554844604667143 + "x": 3.8213518783932194, + "y": 2.9054980205024266 }, "prevControl": null, "nextControl": { - "x": 3.303650313508808, - "y": 2.4371241112668884 + "x": 3.1078125, + "y": 2.3123355263157896 }, "isLocked": false, "linkedName": "LCS 3" }, { "anchor": { - "x": 2.1484847537019114, - "y": 0.8010986125110935 + "x": 2.021, + "y": 0.7994156169734083 }, "prevControl": { - "x": 1.9348617578898692, - "y": 0.5468967666219559 + "x": 2.372169441998694, + "y": 1.2536507379032424 }, "nextControl": null, "isLocked": false, @@ -46,17 +46,6 @@ ], "pointTowardsZones": [], "eventMarkers": [ - { - "name": "5PieceScore", - "waypointRelativePos": 0.0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "5PieceScore" - } - } - }, { "name": "ElevatorDown", "waypointRelativePos": 0.05, @@ -90,7 +79,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -132.7688253919688 + "rotation": -128.6116599476519 }, "reversed": false, "folder": "4 Piece Processor side", diff --git a/src/main/deploy/pathplanner/paths/LCS 5.path b/src/main/deploy/pathplanner/paths/LCS 5.path index d0def1f..ee2357b 100644 --- a/src/main/deploy/pathplanner/paths/LCS 5.path +++ b/src/main/deploy/pathplanner/paths/LCS 5.path @@ -3,40 +3,45 @@ "waypoints": [ { "anchor": { - "x": 2.1484847537019114, - "y": 0.8010986125110935 + "x": 2.021, + "y": 0.7994156169734083 }, "prevControl": null, "nextControl": { - "x": 2.504234805043925, - "y": 1.1204025396119586 + "x": 2.5983026315789473, + "y": 1.2708794327655348 }, "isLocked": false, "linkedName": "LCS 2" }, { "anchor": { - "x": 3.9330626764352288, - "y": 2.812394568574623 + "x": 3.9347059868001075, + "y": 2.849571035298859 }, "prevControl": { - "x": 3.048423720292707, - "y": 1.9589872143388218 + "x": 3.140956255762699, + "y": 1.9517313826234841 }, "nextControl": null, "isLocked": false, "linkedName": "LCS 4" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.8726415094339707, + "rotationDegrees": -119.99999999999999 + } + ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.75, + "minWaypointRelativePos": 0.87, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 3.0, - "maxAcceleration": 1.1, + "maxAcceleration": 1.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -48,7 +53,7 @@ "eventMarkers": [ { "name": "Processor4Piece3", - "waypointRelativePos": 0.6535008976660683, + "waypointRelativePos": 0.6, "endWaypointRelativePos": null, "command": { "type": "named", @@ -60,7 +65,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -74,7 +79,7 @@ "folder": "4 Piece Processor side", "idealStartingState": { "velocity": 0, - "rotation": -132.7688253919688 + "rotation": -128.6116599476519 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LCS 6.path b/src/main/deploy/pathplanner/paths/LCS 6.path index 02b18b5..f236735 100644 --- a/src/main/deploy/pathplanner/paths/LCS 6.path +++ b/src/main/deploy/pathplanner/paths/LCS 6.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.9330626764352288, - "y": 2.812394568574623 + "x": 3.9347059868001075, + "y": 2.849571035298859 }, "prevControl": null, "nextControl": { - "x": 2.951116162831399, - "y": 1.8827786337248869 + "x": 2.9426705017481174, + "y": 1.9359439480223979 }, "isLocked": false, "linkedName": "LCS 4" }, { "anchor": { - "x": 2.1484847537019114, - "y": 0.8010986125110935 + "x": 2.021, + "y": 0.7994156169734083 }, "prevControl": { - "x": 2.5648973533077983, - "y": 1.187595871584342 + "x": 2.7345774818798887, + "y": 1.655268742410823 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.6053459119496889, - "rotationDegrees": -135.0824807086794 + "rotationDegrees": -128.08554313418483 } ], "constraintZones": [ @@ -51,17 +51,6 @@ ], "pointTowardsZones": [], "eventMarkers": [ - { - "name": "5PieceScore", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": { - "type": "named", - "data": { - "name": "5PieceScore" - } - } - }, { "name": "ElevatorDown", "waypointRelativePos": 0.05, @@ -95,7 +84,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -132.7688253919688 + "rotation": -128.6116599476519 }, "reversed": false, "folder": "4 Piece Processor side", diff --git a/src/main/deploy/pathplanner/paths/LCS 7.path b/src/main/deploy/pathplanner/paths/LCS 7.path index a827343..621ab0a 100644 --- a/src/main/deploy/pathplanner/paths/LCS 7.path +++ b/src/main/deploy/pathplanner/paths/LCS 7.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.1484847537019114, - "y": 0.8010986125110935 + "x": 2.021, + "y": 0.7994156169734083 }, "prevControl": null, "nextControl": { - "x": 2.9374650168578578, - "y": 1.3495361125158465 + "x": 3.0573014305086144, + "y": 1.5024288465675784 }, "isLocked": false, "linkedName": "LCS 2" }, { "anchor": { - "x": 4.92157171856577, - "y": 2.7284019117633864 + "x": 4.893712143783675, + "y": 2.732663794013762 }, "prevControl": { - "x": 4.288106502758831, - "y": 2.355304020390542 + "x": 4.252796052631578, + "y": 2.225740131578947 }, "nextControl": null, "isLocked": false, @@ -31,17 +31,17 @@ "rotationTargets": [ { "waypointRelativePos": 0.6729559748427736, - "rotationDegrees": -61.81181756103639 + "rotationDegrees": -59.99999999999999 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.7, + "minWaypointRelativePos": 0.75, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 4.0, - "maxAcceleration": 1.1, + "maxAcceleration": 1.25, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -65,7 +65,7 @@ ], "globalConstraints": { "maxVelocity": 4.0, - "maxAcceleration": 4.0, + "maxAcceleration": 4.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -73,13 +73,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -61.04900479253321 + "rotation": -59.99999999999999 }, "reversed": false, "folder": "4 Piece Processor side", "idealStartingState": { "velocity": 0, - "rotation": -132.7688253919688 + "rotation": -128.6116599476519 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 57cfcc4..2ae293d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -148,7 +148,10 @@ public final class Constants { public static final Double kShooterMotionMagicJerk = 0.0; public static final Double kShooterScoreSpeed = 8.0; public static final Double kShooterLength = 8.5; - public static final Double kL1ShooterVelo = 40.0; // 31 + public static final Double kStowShooterVelo = 100.0; + public static final Double kL1ShooterVelo = 50.0; // 38 + public static final Double kL1PostSlideShooterVelo = 30.0; + public static final Double kL1_5ShooterVelo = 70.0; public static final Double kL4ShooterVelo = 60.0; // Only gets to 90 when at 110 public static final Double kL2andL3ShooterVelo = 60.0; // Goes to around 52 at 60 @@ -187,7 +190,7 @@ public final class Constants { public static final Double kAngulatorDeAlgaeL2 = 45.0; public static final Double kAngulatorTravelPos = 4.5; public static final Double kAngulatorTroughScorePos = 0.0; - public static final Double kAngulatorTrough2ScorePos = -8.0; // 8 + public static final Double kAngulatorTrough2ScorePos = -12.0; // 8 public static final Double kAngulatorBargeScorePos = -3.0; public static final Double kAngulatorGroundPickUpPos = 61.5; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c3dd25c..40bf682 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -742,7 +742,8 @@ private void configureButtonBindings() { .onTrue( Commands.sequence( shooter.setShooterVelocityCmd(Constants.kL1ShooterVelo), - new WaitCommand(0.1), + new WaitCommand(0.10), + shooter.setShooterVelocityCmd(Constants.kL1PostSlideShooterVelo), slide .setSlidePositionCmd(Meters.of(0.04)) .alongWith(shooter.setHasCoralCmd(false))) @@ -755,7 +756,7 @@ private void configureButtonBindings() { .onTrue( Commands.sequence( shooter - .setShooterVelocityCmd(Constants.kL1ShooterVelo) + .setShooterVelocityCmd(Constants.kL1_5ShooterVelo) .alongWith(shooter.setHasCoralCmd(false))) .withName("Trough2Score")); @@ -764,7 +765,7 @@ private void configureButtonBindings() { .and(elevatorHomePosTrigger()) .onTrue( shooter - .setShooterVelocityCmd(Constants.kL1ShooterVelo) + .setShooterVelocityCmd(Constants.kStowShooterVelo) .alongWith(shooter.setHasCoralCmd(false))); // L2 and L3 Score @@ -964,6 +965,11 @@ private void configureTriggerBindings() { feeder.setFeederRollerVoltsCmd(0), shooter.positionVoltageNoMovement(), // setShooterVoltageCmd(0) shooter.setHasCoralCmd(true)) + // new WaitCommand(0.1), + // elevator.setM_ElevatorHeightCmd(7), + // elevator.setM_ElevatorAlignCmd(true), + // angulator.angulatorPosStateCmd(5), + // slide.slideAlignCmd(false)) .withName("finishedMovingCoralToShooter")); // Align for Coral scoring @@ -1282,7 +1288,7 @@ private final Command processorAutoPiece1() { elevator.setM_ElevatorHeightCmd(3), elevator.setM_ElevatorAlignCmd(true), new WaitUntilCommand(elevator::isSlideSafeToMove), - slide.setSlidePositionCmd(Meters.of(0.))); + slide.setSlidePositionCmd(Meters.of(0.27))); } private final Command processorAutoPiece2() { @@ -1292,7 +1298,7 @@ private final Command processorAutoPiece2() { elevator.setM_ElevatorHeightCmd(3), elevator.setM_ElevatorAlignCmd(true), new WaitUntilCommand(elevator::isSlideSafeToMove), - slide.setSlidePositionCmd(Meters.of(0.17))); + slide.setSlidePositionCmd(Meters.of(0.03))); } private final Command processorAutoPiece3() { @@ -1302,7 +1308,7 @@ private final Command processorAutoPiece3() { elevator.setM_ElevatorHeightCmd(3), elevator.setM_ElevatorAlignCmd(true), new WaitUntilCommand(elevator::isSlideSafeToMove), - slide.setSlidePositionCmd(Meters.of(0.20))); + slide.setSlidePositionCmd(Meters.of(0.21))); } private final Command processorAutoPiece4() { From 77af105253d118398c61074c31e59713f604efa7 Mon Sep 17 00:00:00 2001 From: Kython89 Date: Thu, 10 Apr 2025 23:01:03 -0500 Subject: [PATCH 08/13] 4/10 --- src/main/java/frc/robot/Constants.java | 29 ++++--- src/main/java/frc/robot/RobotContainer.java | 80 ++++++++++++++----- .../frc/robot/subsystems/feeder/Feeder.java | 12 +++ .../frc/robot/subsystems/feeder/FeederIO.java | 13 +++ .../subsystems/feeder/FeederIOTalonFX.java | 45 +++++++++-- .../subsystems/shooter/ShooterIOTalonFX.java | 2 +- 6 files changed, 139 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2ae293d..fdadd2d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,8 +33,8 @@ public final class Constants { public static final Distance kReefL4 = Meters.of(1.61); // 1.5975 public static final Distance kDeAlgaeL2 = Meters.of(0.55); public static final Distance kDeAlgaeL3 = Meters.of(.98); - public static final Distance kElevatorTroughScorePos = Meters.of(0.08); - public static final Distance kElevatorTrough2ScorePos = Meters.of(0.12); // 0.10 + public static final Distance kElevatorTroughScorePos = Meters.of(0.18); // 0.08 old // 0.20 + public static final Distance kElevatorTrough2ScorePos = Meters.of(0.12); // 0.12 old // public static final Double kElevatorL4FudgeFactor = 1.4; public static final Double kElevatorL23FudgeFactor = 1.5; @@ -151,7 +151,7 @@ public final class Constants { public static final Double kStowShooterVelo = 100.0; public static final Double kL1ShooterVelo = 50.0; // 38 public static final Double kL1PostSlideShooterVelo = 30.0; - public static final Double kL1_5ShooterVelo = 70.0; + public static final Double kL1_5ShooterVelo = 50.0; public static final Double kL4ShooterVelo = 60.0; // Only gets to 90 when at 110 public static final Double kL2andL3ShooterVelo = 60.0; // Goes to around 52 at 60 @@ -189,8 +189,8 @@ public final class Constants { public static final Double kAngulatorL4 = 47.0; public static final Double kAngulatorDeAlgaeL2 = 45.0; public static final Double kAngulatorTravelPos = 4.5; - public static final Double kAngulatorTroughScorePos = 0.0; - public static final Double kAngulatorTrough2ScorePos = -12.0; // 8 + public static final Double kAngulatorTroughScorePos = 10.0; // 0.0 old L1 + public static final Double kAngulatorTrough2ScorePos = -16.0; // -12 old toss up // 10 public static final Double kAngulatorBargeScorePos = -3.0; public static final Double kAngulatorGroundPickUpPos = 61.5; @@ -198,12 +198,19 @@ public final class Constants { public static final Double kAngulatorFeederPositionTolerance = 2.0; public static final Double kSlideFeederPositionToleranceStopping = 0.005; - public static final Double kFeederkP = 50.0; - public static final Double kFeederkI = 0.0; - public static final Double kFeederkD = 0.0; - public static final Double kFeederkV = 0.0909; - public static final Double kFeederkA = 0.0; - public static final Double kFeederkS = 0.5; + public static final Double kVeloFeederkP = 0.5; + public static final Double kVeloFeederkI = 0.0; + public static final Double kVeloFeederkD = 0.0; + public static final Double kVeloFeederkV = 0.0928; + public static final Double kVeloFeederkA = 0.0; + public static final Double kVeloFeederkS = 0.3; + + public static final Double kPosFeederkP = 50.0; + public static final Double kPosFeederkI = 0.0; + public static final Double kPosFeederkD = 0.0; + public static final Double kPosFeederkV = 0.0909; + public static final Double kPosFeederkA = 0.0; + public static final Double kPosFeederkS = 0.5; // drive configurations public static final Integer DriveMaxSpeed = 1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 40bf682..1867953 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import static edu.wpi.first.units.Units.Meters; -import com.ctre.phoenix.led.CANdle; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.filter.LinearFilter; @@ -77,7 +76,6 @@ import frc.robot.subsystems.vision.VisionIOLimelight; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -108,13 +106,14 @@ public class RobotContainer { private final SendableChooser autoChooser; private Boolean m_FieldRelative = true; - private Translation2d m_PreviousChassisSpeeds = new Translation2d(); - private Double m_PreviousTime = 0.0; - private Double m_PreviousAngle = 0.0; - private static final LoggedNetworkNumber latency = new LoggedNetworkNumber("latency", 0); - private CANdle m_CANdle = new CANdle(1); + // private Translation2d m_PreviousChassisSpeeds = new Translation2d(); + // private Double m_PreviousTime = 0.0; + // private Double m_PreviousAngle = 0.0; + // private static final LoggedNetworkNumber latency = new LoggedNetworkNumber("latency", 0); + // private CANdle m_CANdle = new CANdle(1); public LinearFilter odometryAverageFilterAngle = LinearFilter.movingAverage(9); private Alliance m_alliance = Alliance.Red; + // private Boolean m_intaking = false; // private final Command m_testAutoCommand = new TestAuto(drive); @@ -314,7 +313,7 @@ private void configureButtonBindings() { driveController .rightBumper() - .and(feeder.hasCoral().negate()) + .and(feeder.beambreakTrigger().negate()) .onTrue(intakeUpAndOff().withName("IntakeUp")); driveController.rightTrigger().onTrue(intakeDownAndOn().withName("IntakeDown")); @@ -337,6 +336,7 @@ private void configureButtonBindings() { intake .setIntakeRollerVoltsCmd(0) .alongWith(feeder.setFeederRollerVoltsCmd(0)) + .alongWith(feeder.feederZeroCmd()) .withName("IntakeStop")); // Zero Gyro Heading @@ -496,6 +496,7 @@ private void configureButtonBindings() { .and(operatorController.back().negate()) .and(operatorController.start().negate()) .and(feeder.hasCoral().negate()) + .and(feeder.beambreakTrigger().negate()) .onTrue( Commands.sequence( humanCmd(), @@ -554,6 +555,7 @@ private void configureButtonBindings() { .and(operatorController.back().negate()) .and(operatorController.start().negate()) .and(feeder.hasCoral().negate()) + .and(feeder.beambreakTrigger().negate()) .onTrue( Commands.sequence( humanCmd(), @@ -610,6 +612,7 @@ private void configureButtonBindings() { .y() .and(operatorController.leftBumper().negate()) .and(feeder.hasCoral().negate()) + .and(feeder.beambreakTrigger().negate()) .onTrue( Commands.sequence( humanCmd() @@ -651,6 +654,7 @@ private void configureButtonBindings() { .back() .and(operatorController.leftBumper().negate()) .and(feeder.hasCoral().negate()) + .and(feeder.beambreakTrigger().negate()) .onTrue(troughModeCmd().withName("ElevatorTroughModeCmd")); operatorController.back().and(operatorController.leftBumper()).onTrue(troughMode1Cmd()); @@ -932,18 +936,19 @@ private void configureTriggerBindings() { slide .isAlignedWithFeederTrigger() .and(elevator.isAlignedWithFeederTrigger()) - .and(feeder.hasCoral()) + .and(feeder.beambreakTrigger()) // .hasCoral()) .and(angulator.isAlignedWithFeederTrigger()) .and(elevatorHomePosTrigger()) // added to make sure elevator setpoint AND position are at // zero, when feeding past beambreak .onTrue( - Commands.sequence(feeder.setFeederRollerVoltsCmd(4), shooter.setShooterVelocityCmd(10)) + Commands.sequence(feeder.setFeederVelocityCmd(20), shooter.setShooterVelocityCmd(10)) .withName("startMovingCoralToShooter")); // shooter.setShooterVoltageCmd(1.5))); - // Turn feeder off and intake off + // Turn feeder off and intake off if something isn't aligned properly feeder - .hasCoral() + // .hasCoral() + .beambreakTrigger() .and( slide .isAlignedWithFeederTrigger() @@ -954,23 +959,37 @@ private void configureTriggerBindings() { .onTrue(feeder.setFeederPosZero().withName("stopFeederIfAnythingIsntAligned")); feeder - .hasCoral() + // .hasCoral() + .beambreakTrigger() .onTrue(intake.setIntakeRollerVoltsCmd(0).withName("finishedIntakingGamePiece")); // Turn off feeder and shooter once coral moves from feeder to shooter feeder .hasCoral() + .and(autoEnabled().negate()) + .onFalse( + Commands.sequence( + feeder.setFeederRollerVoltsCmd(0), + feeder.feederZeroCmd(), + shooter.positionVoltageNoMovement(), // setShooterVoltageCmd(0) + shooter.setHasCoralCmd(true), + humanCmd(), + elevator.setM_ElevatorHeightCmd(7), + elevator.setM_ElevatorAlignCmd(true), + angulator.angulatorPosStateCmd(5), + slide.slideAlignCmd(false)) + // intaking(false)) + .withName("finishedMovingCoralToShooterTele")); + feeder + .hasCoral() + .and(autoEnabled()) .onFalse( Commands.sequence( feeder.setFeederRollerVoltsCmd(0), + feeder.feederZeroCmd(), shooter.positionVoltageNoMovement(), // setShooterVoltageCmd(0) shooter.setHasCoralCmd(true)) - // new WaitCommand(0.1), - // elevator.setM_ElevatorHeightCmd(7), - // elevator.setM_ElevatorAlignCmd(true), - // angulator.angulatorPosStateCmd(5), - // slide.slideAlignCmd(false)) - .withName("finishedMovingCoralToShooter")); + .withName("finishedMovingCoralToShooterAuto")); // Align for Coral scoring // elevator @@ -1082,18 +1101,19 @@ public double scaleAxis(double input) { } private final Command intakeDownAndOn() { - return intake.intakeOnCmd().andThen(feeder.setFeederRollerVoltsCmd(1.9)); + return intake.intakeOnCmd().andThen(feeder.setFeederVelocityCmd(18)); } private final Command intakeUpAndOff() { return intake .setIntakePivotPositionCmd(0) .alongWith(intake.setIntakeRollerVoltsCmd(0)) - .alongWith(feeder.setFeederRollerVoltsCmd(0)); + .alongWith(feeder.setFeederRollerVoltsCmd(0)) + .alongWith(feeder.feederZeroCmd()); } private final Command intakeBarfCmd() { - return intake.intakeBarfCommand().andThen(feeder.setFeederRollerVoltsCmd(-2)); + return intake.intakeBarfCommand().andThen(feeder.setFeederVelocityCmd(-18)); } private final Command driveSetFieldRelativeCommand(Boolean fieldRelative) { @@ -1196,6 +1216,19 @@ public final Command troughMode2Cmd() { slide.slideAlignCmd(false)); } + public final Trigger autoEnabled() { + return new Trigger(() -> DriverStation.isAutonomousEnabled() == true); + } + + // public final Command intaking(Boolean yes) { + // return Commands.runOnce(() -> m_intaking = yes); + // } + + // public final Trigger hasCoral() { + // return new Trigger( + // () -> feeder.beambreakTrigger().getAsBoolean() == true); // m_intaking == true && ); + // } + private final Command autoGamePieceHold() { return shooter.positionVoltageNoMovement(); } @@ -1541,6 +1574,9 @@ public void periodic() { Logger.recordOutput("RobotContainer/Height3", elevatorL4Pos()); Logger.recordOutput("RobotContainer/AngleToGreat", vision.angleTooLargeTrigger); + // Logger.recordOutput("RobotContainer/intakeRunning", m_intaking); + // Logger.recordOutput("RobotContainer/hasCoral", hasCoral()); + var alliance = DriverStation.getAlliance().orElse(Alliance.Blue); if (alliance != m_alliance) { m_alliance = alliance; diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 07db402..3f4dc28 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -25,10 +25,22 @@ public Command setFeederRollerVoltsCmd(double roller) { return m_FeederIO.setFeederRollerVoltsCmd(roller); } + public Command setFeederVelocityCmd(double velo) { + return m_FeederIO.setFeederVelocityCmd(velo); + } + + public Command feederZeroCmd() { + return m_FeederIO.feederZeroCmd(); + } + public Trigger hasCoral() { return m_FeederIO.hasCoral(); } + public Trigger beambreakTrigger() { + return m_FeederIO.beambreakTrigger(); + } + public Command setFeederPosZero() { return m_FeederIO.setFeederPosZero(); } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index f856d81..f94f8c4 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -27,6 +27,7 @@ public static class FeederIOInputs { public double currentAmps = 0.0; public boolean hasCoral = false; public double position = 0.0; + public boolean beambreakTrigger; } public default void updateInputs(FeederIOInputs inputs) {} @@ -39,7 +40,19 @@ public default Trigger hasCoral() { return new Trigger(() -> true); } + public default Trigger beambreakTrigger() { + return new Trigger(() -> true); + } + public default Command setFeederPosZero() { return new InstantCommand(); } + + public default Command setFeederVelocityCmd(double velo) { + return new InstantCommand(); + } + + public default Command feederZeroCmd() { + return new InstantCommand(); + } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index 76d48e9..b9a3799 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; @@ -36,9 +37,11 @@ public class FeederIOTalonFX implements FeederIO { private final TalonFX m_feederTalonFX = new TalonFX(Constants.kFeederCANID); private final VoltageOut m_VoltageOut = new VoltageOut(0); private final DigitalInput m_beambreak = new DigitalInput(0); - private final Trigger m_beambreakTrigger = new Trigger(m_beambreak::get); + private final DigitalInput m_beambreak2 = new DigitalInput(2); + // private final Trigger m_beambreakTrigger = new Trigger(m_beambreak::get); // TODO check if right private final PositionVoltage m_PositionVoltage = new PositionVoltage(0); + private final VelocityVoltage m_VelocityVoltage = new VelocityVoltage(0); private final StatusSignal m_Velocity; private final StatusSignal m_AppliedVolts; @@ -50,12 +53,19 @@ public FeederIOTalonFX() { feederCfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; feederCfg.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; feederCfg.CurrentLimits.StatorCurrentLimit = 50.0; - feederCfg.Slot0.kP = Constants.kFeederkP; - feederCfg.Slot0.kI = Constants.kFeederkI; - feederCfg.Slot0.kD = Constants.kFeederkD; - feederCfg.Slot0.kV = Constants.kFeederkV; - feederCfg.Slot0.kA = Constants.kFeederkA; - feederCfg.Slot0.kS = Constants.kFeederkS; + feederCfg.Slot0.kP = Constants.kVeloFeederkP; + feederCfg.Slot0.kI = Constants.kVeloFeederkI; + feederCfg.Slot0.kD = Constants.kVeloFeederkD; + feederCfg.Slot0.kV = Constants.kVeloFeederkV; + feederCfg.Slot0.kA = Constants.kVeloFeederkA; + feederCfg.Slot0.kS = Constants.kVeloFeederkS; + + feederCfg.Slot1.kP = Constants.kPosFeederkP; + feederCfg.Slot1.kI = Constants.kPosFeederkI; + feederCfg.Slot1.kD = Constants.kPosFeederkD; + feederCfg.Slot1.kV = Constants.kPosFeederkV; + feederCfg.Slot1.kA = Constants.kPosFeederkA; + feederCfg.Slot1.kS = Constants.kPosFeederkS; m_feederTalonFX.getConfigurator().apply(feederCfg); @@ -77,6 +87,7 @@ public void updateInputs(FeederIOInputs inputs) { inputs.currentAmps = m_Current.getValueAsDouble(); inputs.position = m_Position.getValueAsDouble(); inputs.hasCoral = !m_beambreak.get(); + inputs.beambreakTrigger = !m_beambreak2.get(); } @Override @@ -84,6 +95,11 @@ public Trigger hasCoral() { return new Trigger(() -> !m_beambreak.get()); } + @Override + public Trigger beambreakTrigger() { + return new Trigger(() -> !m_beambreak2.get()); + } + @Override public Command setFeederRollerVoltsCmd(double roller) { return Commands.runOnce( @@ -97,7 +113,20 @@ public Command setFeederPosZero() { return Commands.runOnce( () -> { m_feederTalonFX.setControl( - m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 0.25)); + m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 0.5).withSlot(1)); }); } + + @Override + public Command setFeederVelocityCmd(double velo) { + return Commands.runOnce( + () -> { + m_feederTalonFX.setControl(m_VelocityVoltage.withVelocity(velo).withSlot(0)); + }); + } + + @Override + public Command feederZeroCmd() { + return Commands.runOnce(() -> m_feederTalonFX.setPosition(0)); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 46fd1c8..d474cff 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -133,7 +133,7 @@ public Command positionVoltageNoMovement() { return Commands.runOnce( () -> { m_shooterTalonFX.setControl( - m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 0.5).withSlot(0)); + m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 1).withSlot(0)); }); } From dbc5d992fac0da22f7a715ea9851ffdf31051e0d Mon Sep 17 00:00:00 2001 From: Kython89 Date: Sat, 12 Apr 2025 00:00:08 -0500 Subject: [PATCH 09/13] 4/12 --- src/main/java/frc/robot/Constants.java | 8 +- src/main/java/frc/robot/Robot.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 88 +++++++++++++++---- .../robot/subsystems/elevator/Elevator.java | 21 +++-- 4 files changed, 91 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fdadd2d..7ce0aa5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -33,8 +33,8 @@ public final class Constants { public static final Distance kReefL4 = Meters.of(1.61); // 1.5975 public static final Distance kDeAlgaeL2 = Meters.of(0.55); public static final Distance kDeAlgaeL3 = Meters.of(.98); - public static final Distance kElevatorTroughScorePos = Meters.of(0.18); // 0.08 old // 0.20 - public static final Distance kElevatorTrough2ScorePos = Meters.of(0.12); // 0.12 old // + public static final Distance kElevatorTroughScorePos = Meters.of(0.08); // 0.08 old // 0.20 + public static final Distance kElevatorTrough2ScorePos = Meters.of(0.20); // 0.12 old // public static final Double kElevatorL4FudgeFactor = 1.4; public static final Double kElevatorL23FudgeFactor = 1.5; @@ -189,8 +189,8 @@ public final class Constants { public static final Double kAngulatorL4 = 47.0; public static final Double kAngulatorDeAlgaeL2 = 45.0; public static final Double kAngulatorTravelPos = 4.5; - public static final Double kAngulatorTroughScorePos = 10.0; // 0.0 old L1 - public static final Double kAngulatorTrough2ScorePos = -16.0; // -12 old toss up // 10 + public static final Double kAngulatorTroughScorePos = 0.0; // 0.0 old L1 + public static final Double kAngulatorTrough2ScorePos = 10.0; // -12 old toss up // 10 public static final Double kAngulatorBargeScorePos = -3.0; public static final Double kAngulatorGroundPickUpPos = 61.5; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 338effc..d2256dd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -211,10 +211,10 @@ public void teleopInit() { autonomousCommand.cancel(); } - hasBeenEnabled = true; - if (DriverStation.isFMSAttached() == false) { - robotContainer.enableBrakes(); - } + // hasBeenEnabled = true; + // if (DriverStation.isFMSAttached() == false) { + // robotContainer.enableBrakes(); + // } } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1867953..1784c1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -644,7 +645,7 @@ private void configureButtonBindings() { slide.setSlidePositionCmd(Meters.of(0.0)), elevator.setM_ElevatorHeightCmd(6), elevator.setM_ElevatorAlignCmd(true), - shooter.setShooterVelocityCmd(-55), + // shooter.setShooterVelocityCmd(-55), new WaitUntilCommand(elevator::isElevatorCloseAlgae), angulator.angulatorPosStateCmd(6)) .withName("ElevatorBarge")); @@ -759,8 +760,11 @@ private void configureButtonBindings() { .and(operatorController.leftBumper().negate()) .onTrue( Commands.sequence( - shooter - .setShooterVelocityCmd(Constants.kL1_5ShooterVelo) + shooter.setShooterVelocityCmd(Constants.kL1ShooterVelo), + new WaitCommand(0.10), + shooter.setShooterVelocityCmd(Constants.kL1PostSlideShooterVelo), + slide + .setSlidePositionCmd(Meters.of(0.04)) .alongWith(shooter.setHasCoralCmd(false))) .withName("Trough2Score")); @@ -830,12 +834,24 @@ private void configureButtonBindings() { operatorController .leftBumper() - .onTrue(shooter.setShooterVelocityCmd(-100).withName("AlgaePickup")); + .onTrue( + shooter + .setShooterVelocityCmd(-100) + .withName("AlgaePickup") + .alongWith( + Commands.runOnce( + () -> operatorController.setRumble(RumbleType.kBothRumble, 0.3)))); // Score In Barge operatorController .leftBumper() - .onFalse(shooter.setShooterVelocityCmd(100).withName("BargeScore")); + .onFalse( + shooter + .setShooterVelocityCmd(100) + .withName("BargeScore") + .alongWith( + Commands.runOnce( + () -> operatorController.setRumble(RumbleType.kBothRumble, 0.0)))); // Stop Shooter When Right Bumper is Released operatorController @@ -966,18 +982,23 @@ private void configureTriggerBindings() { // Turn off feeder and shooter once coral moves from feeder to shooter feeder .hasCoral() + .and(feeder.beambreakTrigger().negate()) .and(autoEnabled().negate()) .onFalse( Commands.sequence( - feeder.setFeederRollerVoltsCmd(0), - feeder.feederZeroCmd(), - shooter.positionVoltageNoMovement(), // setShooterVoltageCmd(0) - shooter.setHasCoralCmd(true), - humanCmd(), - elevator.setM_ElevatorHeightCmd(7), - elevator.setM_ElevatorAlignCmd(true), - angulator.angulatorPosStateCmd(5), - slide.slideAlignCmd(false)) + feeder + .setFeederRollerVoltsCmd(0) + .alongWith( + feeder + .feederZeroCmd() + .alongWith(shooter.positionVoltageNoMovement()) + .alongWith( + shooter.setHasCoralCmd(true)))) // setShooterVoltageCmd(0) + // humanCmd(), + // elevator.setM_ElevatorHeightCmd(7), + // elevator.setM_ElevatorAlignCmd(true), + // angulator.angulatorPosStateCmd(5), + // slide.slideAlignCmd(false)) // intaking(false)) .withName("finishedMovingCoralToShooterTele")); feeder @@ -985,10 +1006,15 @@ private void configureTriggerBindings() { .and(autoEnabled()) .onFalse( Commands.sequence( - feeder.setFeederRollerVoltsCmd(0), - feeder.feederZeroCmd(), - shooter.positionVoltageNoMovement(), // setShooterVoltageCmd(0) - shooter.setHasCoralCmd(true)) + feeder + .setFeederRollerVoltsCmd(0) + .alongWith( + feeder + .feederZeroCmd() + .alongWith( + shooter + .positionVoltageNoMovement() + .alongWith(shooter.setHasCoralCmd(true))))) .withName("finishedMovingCoralToShooterAuto")); // Align for Coral scoring @@ -1075,6 +1101,17 @@ private void configureTriggerBindings() { .setShooterVelocityCmd(Constants.kL2andL3ShooterVelo) .andThen(shooter.setHasCoralCmd(false)) .andThen(new WaitCommand(0.1).withName("autoScoreL2and3"))); + + // operatorController + // .leftTrigger() + // .onTrue(Commands.runOnce(() -> driveController.setRumble(RumbleType.kBothRumble, 0.3))) + // .onFalse(Commands.runOnce(() -> driveController.setRumble(RumbleType.kBothRumble, 0))); + + // operatorController + // .rightTrigger() + // .onTrue(Commands.runOnce(() -> driveController.setRumble(RumbleType.kBothRumble, 0.3))) + // .onFalse(Commands.runOnce(() -> driveController.setRumble(RumbleType.kBothRumble, 0))); + } public double scaleAxis(double input) { @@ -1573,6 +1610,7 @@ public void periodic() { Logger.recordOutput("RobotContainer/Height2", elevatorL3Pos()); Logger.recordOutput("RobotContainer/Height3", elevatorL4Pos()); Logger.recordOutput("RobotContainer/AngleToGreat", vision.angleTooLargeTrigger); + Logger.recordOutput("RobotContainer/SeesTarget", seesTarget()); // Logger.recordOutput("RobotContainer/intakeRunning", m_intaking); // Logger.recordOutput("RobotContainer/hasCoral", hasCoral()); @@ -1591,6 +1629,20 @@ public void periodic() { } } + if (slide.isSlideReadyToScore() && vision.angleTooLargeTrigger.getAsBoolean() + || (!operatorController.leftTrigger().getAsBoolean() + || !operatorController.rightTrigger().getAsBoolean())) { + driveController.setRumble(RumbleType.kBothRumble, 0.0); + } + + if ((!slide.isSlideReadyToScore() + || !vision.angleTooLargeTrigger.getAsBoolean() + || elevator.m_pegged == true) + && (operatorController.leftTrigger().getAsBoolean() + || operatorController.rightTrigger().getAsBoolean())) { + driveController.setRumble(RumbleType.kBothRumble, 0.3); + } + // Boolean doRejectUpdate = false; // LimelightHelpers.PoseEstimate mt1 = diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 7ec204d..0a1b3ff 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -61,6 +61,7 @@ public void periodic() { Logger.recordOutput("Elevator/EAlgaeReady", isElevatorCloseAlgae()); Logger.recordOutput("Elevator/EScoreReady", isElevatorClose()); Logger.recordOutput("RobotContainer/EBranchOverride", m_branchOverride); + Logger.recordOutput("Elevator/elevatorHeight", m_elevatorHeight); // Logger.recordOutput( // "Elevator/AngleFromCANrange", // canRangeAverageFilterAngle.calculate( @@ -223,22 +224,27 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { if (m_elevatorHeight == 1) { if (m_setHeightAlign == false) { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2); + m_elevatorSetpoint = Constants.kReefL2; } else { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2.plus(m_heightDistanceOffset)); + m_elevatorSetpoint = Constants.kReefL2.plus(m_heightDistanceOffset); } m_pegged = false; } if (m_elevatorHeight == 2) { if (m_setHeightAlign == false) { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3); + m_elevatorSetpoint = Constants.kReefL3; } else { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3.plus(m_heightDistanceOffset)); + m_elevatorSetpoint = Constants.kReefL3.plus(m_heightDistanceOffset); } m_pegged = false; } if (m_elevatorHeight == 3) { if (m_setHeightAlign == false) { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4); + m_elevatorSetpoint = Constants.kReefL4; m_pegged = false; } else { if ((Constants.kReefL4.plus(m_heightDistanceOffset)).magnitude() @@ -246,12 +252,11 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { m_pegged = true; m_ElevatorIO.setElevatorPositionMeters( Meters.of(Constants.kElevatorForwardSoftLimitThreshold)); - m_elevatorSetpoint = - Meters.of(Constants.kElevatorForwardSoftLimitThreshold) - .minus(m_heightDistanceOffset); + m_elevatorSetpoint = Meters.of(Constants.kElevatorForwardSoftLimitThreshold); + // .minus(m_heightDistanceOffset); } else { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4.plus(m_heightDistanceOffset)); - m_elevatorSetpoint = Constants.kReefL4; + m_elevatorSetpoint = Constants.kReefL4.plus(m_heightDistanceOffset); m_pegged = false; } } @@ -285,13 +290,13 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { public void setM_ElevatorHeight(int num) { m_elevatorHeight = num; if (m_elevatorHeight == 1) { - m_elevatorSetpoint = Constants.kReefL2; + // m_elevatorSetpoint = Constants.kReefL2; } if (m_elevatorHeight == 2) { - m_elevatorSetpoint = Constants.kReefL3; + // m_elevatorSetpoint = Constants.kReefL3; } if (m_elevatorHeight == 3) { - m_elevatorSetpoint = Constants.kReefL4; + // m_elevatorSetpoint = Constants.kReefL4; } if (m_elevatorHeight == 4) { m_elevatorSetpoint = Constants.kDeAlgaeL2; @@ -382,7 +387,7 @@ public Trigger isElevatorReadyToScore() { return new Trigger( () -> MathUtil.isNear( - m_elevatorSetpoint.plus(m_heightDistanceOffset).magnitude(), + m_elevatorSetpoint.magnitude(), // plus(m_heightDistanceOffset).magnitude(), inputs.masterPosition.magnitude(), Constants.kElevatorClosetoTarget)); } From 1f15c8c83f06f0abb152d78018d0d4b57682ec85 Mon Sep 17 00:00:00 2001 From: Kython89 Date: Sun, 13 Apr 2025 00:02:01 -0500 Subject: [PATCH 10/13] large amounts of bug fixes, queen bee not found yet --- src/main/java/frc/robot/Constants.java | 10 +- src/main/java/frc/robot/RobotContainer.java | 92 +++++++++++-------- .../robot/subsystems/angulator/Angulator.java | 5 + .../frc/robot/subsystems/drive/Drive.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 15 ++- .../frc/robot/subsystems/feeder/Feeder.java | 7 +- .../frc/robot/subsystems/feeder/FeederIO.java | 15 ++- .../subsystems/feeder/FeederIOTalonFX.java | 19 ++-- .../frc/robot/subsystems/shooter/Shooter.java | 4 +- .../subsystems/shooter/ShooterIOTalonFX.java | 2 +- 10 files changed, 99 insertions(+), 72 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ce0aa5..bf7c30f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -31,8 +31,8 @@ public final class Constants { public static final Distance kReefL2 = Meters.of(0.5192); // 0.5492 public static final Distance kReefL3 = Meters.of(0.92); // 0.9706 public static final Distance kReefL4 = Meters.of(1.61); // 1.5975 - public static final Distance kDeAlgaeL2 = Meters.of(0.55); - public static final Distance kDeAlgaeL3 = Meters.of(.98); + public static final Distance kDeAlgaeL2 = Meters.of(0.52); + public static final Distance kDeAlgaeL3 = Meters.of(.95); public static final Distance kElevatorTroughScorePos = Meters.of(0.08); // 0.08 old // 0.20 public static final Distance kElevatorTrough2ScorePos = Meters.of(0.20); // 0.12 old // public static final Double kElevatorL4FudgeFactor = 1.4; @@ -136,12 +136,12 @@ public final class Constants { public static final Double kShooterVelocitykA = 0.0; public static final Double kShooterVelocitykS = 0.6; - public static final Double kShooterPositionkP = 50.0; + public static final Double kShooterPositionkP = 5.0; // 50.0; public static final Double kShooterPositionkI = 0.0; public static final Double kShooterPositionkD = 0.0; - public static final Double kShooterPositionkV = 0.0959; + public static final Double kShooterPositionkV = 0.1238; // 0.0959; public static final Double kShooterPositionkA = 0.0; - public static final Double kShooterPositionkS = 0.6; + public static final Double kShooterPositionkS = 0.3; // 0.6 public static final Double kShooterMotionMagicAcceleration = 0.0; public static final Double kShooterMotionMagicCruiseVelocity = 0.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1784c1b..71976ad 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -496,8 +496,8 @@ private void configureButtonBindings() { .and(operatorController.leftBumper().negate()) .and(operatorController.back().negate()) .and(operatorController.start().negate()) - .and(feeder.hasCoral().negate()) - .and(feeder.beambreakTrigger().negate()) + .and(feeder.hasCoral().negate().debounce(0.01)) + .and(feeder.beambreakTrigger().negate().debounce(0.01)) .onTrue( Commands.sequence( humanCmd(), @@ -507,7 +507,8 @@ private void configureButtonBindings() { new WaitUntilCommand(angulator::isAngulatorSafeToMove), elevator.setM_ElevatorHeightCmd(1), elevator.setM_ElevatorAlignCmd(true), - new WaitUntilCommand(elevator::isElevatorClose), + // new WaitUntilCommand(elevator::isElevatorClose), + new WaitUntilCommand(elevator::canAngulatorAngle), angulator.angulatorPosStateCmd(1)) .withName("ElevatorL2")); @@ -555,8 +556,8 @@ private void configureButtonBindings() { .and(operatorController.leftBumper().negate()) .and(operatorController.back().negate()) .and(operatorController.start().negate()) - .and(feeder.hasCoral().negate()) - .and(feeder.beambreakTrigger().negate()) + .and(feeder.hasCoral().negate().debounce(0.01)) + .and(feeder.beambreakTrigger().negate().debounce(0.01)) .onTrue( Commands.sequence( humanCmd(), @@ -566,7 +567,8 @@ private void configureButtonBindings() { new WaitUntilCommand(angulator::isAngulatorSafeToMove), elevator.setM_ElevatorHeightCmd(2), elevator.setM_ElevatorAlignCmd(true), - new WaitUntilCommand(elevator::isElevatorClose), + // new WaitUntilCommand(elevator::isElevatorClose), + new WaitUntilCommand(elevator::canAngulatorAngle), angulator.angulatorPosStateCmd(1)) .withName("ElevatorL3")); @@ -612,8 +614,8 @@ private void configureButtonBindings() { operatorController .y() .and(operatorController.leftBumper().negate()) - .and(feeder.hasCoral().negate()) - .and(feeder.beambreakTrigger().negate()) + .and(feeder.hasCoral().negate().debounce(0.01)) + .and(feeder.beambreakTrigger().negate().debounce(0.01)) .onTrue( Commands.sequence( humanCmd() @@ -954,10 +956,14 @@ private void configureTriggerBindings() { .and(elevator.isAlignedWithFeederTrigger()) .and(feeder.beambreakTrigger()) // .hasCoral()) .and(angulator.isAlignedWithFeederTrigger()) - .and(elevatorHomePosTrigger()) // added to make sure elevator setpoint AND position are at + .and(elevatorHomePosTrigger()) + .and(angulator.angulatorHomePosStateTrigger()) // added to make sure elevator setpoint AND + // position are at // zero, when feeding past beambreak .onTrue( - Commands.sequence(feeder.setFeederVelocityCmd(20), shooter.setShooterVelocityCmd(10)) + feeder + .setFeederVelocityCmd(40) + .alongWith(shooter.setShooterVelocityCmd(50)) .withName("startMovingCoralToShooter")); // shooter.setShooterVoltageCmd(1.5))); @@ -971,8 +977,15 @@ private void configureTriggerBindings() { .negate() .or(elevator.isAlignedWithFeederTrigger().negate()) .or(angulator.isAlignedWithFeederTrigger().negate()) - .or(elevatorHomePosTrigger().negate())) - .onTrue(feeder.setFeederPosZero().withName("stopFeederIfAnythingIsntAligned")); + .or(elevatorHomePosTrigger().negate()) + .or(angulator.angulatorHomePosStateTrigger().negate())) + .onTrue( + Commands.sequence( + // feeder.setFeederPosZero(), + feeder.setFeederVelocityCmd(-15), + new WaitUntilCommand(feeder.hasCoral().negate()), + feeder.setFeederRollerVoltsCmd(0)) + .withName("stopFeederIfAnythingIsntAligned")); feeder // .hasCoral() @@ -982,18 +995,17 @@ private void configureTriggerBindings() { // Turn off feeder and shooter once coral moves from feeder to shooter feeder .hasCoral() - .and(feeder.beambreakTrigger().negate()) - .and(autoEnabled().negate()) + .and(feeder.beambreakTrigger()) + .and(operatorController.leftBumper().negate()) .onFalse( - Commands.sequence( + // Commands.sequence( + feeder + .setFeederRollerVoltsCmd(0) + .alongWith( feeder - .setFeederRollerVoltsCmd(0) - .alongWith( - feeder - .feederZeroCmd() - .alongWith(shooter.positionVoltageNoMovement()) - .alongWith( - shooter.setHasCoralCmd(true)))) // setShooterVoltageCmd(0) + .feederZeroCmd() + .alongWith(shooter.positionVoltageNoMovement()) + .alongWith(shooter.setHasCoralCmd(true))) // setShooterVoltageCmd(0) // humanCmd(), // elevator.setM_ElevatorHeightCmd(7), // elevator.setM_ElevatorAlignCmd(true), @@ -1001,21 +1013,21 @@ private void configureTriggerBindings() { // slide.slideAlignCmd(false)) // intaking(false)) .withName("finishedMovingCoralToShooterTele")); - feeder - .hasCoral() - .and(autoEnabled()) - .onFalse( - Commands.sequence( - feeder - .setFeederRollerVoltsCmd(0) - .alongWith( - feeder - .feederZeroCmd() - .alongWith( - shooter - .positionVoltageNoMovement() - .alongWith(shooter.setHasCoralCmd(true))))) - .withName("finishedMovingCoralToShooterAuto")); + // feeder + // .hasCoral() + // .and(autoEnabled().negate()) + // .onFalse( + // Commands.sequence( + // feeder + // .setFeederRollerVoltsCmd(0) + // .alongWith( + // feeder + // .feederZeroCmd() + // .alongWith( + // shooter + // .positionVoltageNoMovement() + // .alongWith(shooter.setHasCoralCmd(true))))) + // .withName("finishedMovingCoralToShooterAuto")); // Align for Coral scoring // elevator @@ -1070,7 +1082,7 @@ private void configureTriggerBindings() { .and(shooter.isShooterVelocityLowEnoughToScore()) .and(vision.angleTooLargeTrigger) .and(shooter.isShooterZAccelLowEnough()) - // .and(drive.isDriveAccelLowEnoughToScore().debounce(0.05)) // was 0.25 + .and(drive.isDriveAccelLowEnoughToScore()) // .debounce(0.05)) // was 0.25 .and(robotNotMovingTrigger()); // was 0.06 Trigger autoFireTriggerL2and3 = @@ -1138,7 +1150,7 @@ public double scaleAxis(double input) { } private final Command intakeDownAndOn() { - return intake.intakeOnCmd().andThen(feeder.setFeederVelocityCmd(18)); + return intake.intakeOnCmd().andThen(feeder.setFeederVelocityCmd(40)); } private final Command intakeUpAndOff() { @@ -1471,7 +1483,7 @@ private Trigger robotNotMovingTrigger() { return new Trigger( () -> (Math.abs(drive.getChassisSpeeds().vxMetersPerSecond) < 0.1) - && Math.abs(drive.getChassisSpeeds().vyMetersPerSecond) < 0.2 + && Math.abs(drive.getChassisSpeeds().vyMetersPerSecond) < 0.1 && Math.abs(drive.getChassisSpeeds().omegaRadiansPerSecond) < 0.1); } diff --git a/src/main/java/frc/robot/subsystems/angulator/Angulator.java b/src/main/java/frc/robot/subsystems/angulator/Angulator.java index 17954a3..d242108 100644 --- a/src/main/java/frc/robot/subsystems/angulator/Angulator.java +++ b/src/main/java/frc/robot/subsystems/angulator/Angulator.java @@ -29,6 +29,7 @@ public void periodic() { Logger.recordOutput("Angulator/APosState", m_AngulatorPosState); Logger.recordOutput("Angulator/AScoreReady", readyToScore()); Logger.recordOutput("Angulator/AFeederReady", isAlignedWithFeeder()); + Logger.recordOutput("Angulator/AHomeState", angulatorHomePosStateTrigger()); } public Command setAngulatorPositionCmd(Double angle) { @@ -122,4 +123,8 @@ public Command angulatorVoltageCmd(double volts, boolean overrideLimits) { public Command angulatorZeroCmd() { return m_AngulatorIO.angulatorZeroCmd(); } + + public Trigger angulatorHomePosStateTrigger() { + return new Trigger(() -> m_AngulatorPosState == 0); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index cb6433c..2c570d3 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -372,7 +372,7 @@ public double getMaxAngularSpeedRadPerSec() { public Trigger isDriveAccelLowEnoughToScore() { return new Trigger( - () -> Math.abs(gyroInputs.accelX) < 0.4 && Math.abs(gyroInputs.accelY) < 0.4); + () -> Math.abs(gyroInputs.accelX) < 0.3 && Math.abs(gyroInputs.accelY) < 0.3); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 0a1b3ff..df489cd 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -36,6 +36,7 @@ public class Elevator extends SubsystemBase { public Boolean m_pegged = false; public final Trigger peggedTrigger = new Trigger(() -> m_pegged); public Boolean m_setHeightAlign = true; + public double m_angleSetpoint = 0.0; public LinearFilter canRangeAverageFilterAngle = LinearFilter.movingAverage(9); @@ -59,7 +60,7 @@ public void periodic() { Logger.recordOutput("Elevator/RobotAngleToReefFace", m_robotAngleToReefFace); Logger.recordOutput("Elevator/EHeightOffset", m_heightDistanceOffset); Logger.recordOutput("Elevator/EAlgaeReady", isElevatorCloseAlgae()); - Logger.recordOutput("Elevator/EScoreReady", isElevatorClose()); + Logger.recordOutput("Elevator/EScoreReady", isElevatorReadyToScore()); Logger.recordOutput("RobotContainer/EBranchOverride", m_branchOverride); Logger.recordOutput("Elevator/elevatorHeight", m_elevatorHeight); // Logger.recordOutput( @@ -224,7 +225,7 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { if (m_elevatorHeight == 1) { if (m_setHeightAlign == false) { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2); - m_elevatorSetpoint = Constants.kReefL2; + m_elevatorSetpoint = Constants.kReefL2.plus(m_heightDistanceOffset); } else { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL2.plus(m_heightDistanceOffset)); m_elevatorSetpoint = Constants.kReefL2.plus(m_heightDistanceOffset); @@ -234,7 +235,7 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { if (m_elevatorHeight == 2) { if (m_setHeightAlign == false) { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3); - m_elevatorSetpoint = Constants.kReefL3; + m_elevatorSetpoint = Constants.kReefL3.plus(m_heightDistanceOffset); } else { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3.plus(m_heightDistanceOffset)); m_elevatorSetpoint = Constants.kReefL3.plus(m_heightDistanceOffset); @@ -290,12 +291,15 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { public void setM_ElevatorHeight(int num) { m_elevatorHeight = num; if (m_elevatorHeight == 1) { + m_angleSetpoint = Constants.kReefL2.magnitude(); // m_elevatorSetpoint = Constants.kReefL2; } if (m_elevatorHeight == 2) { + m_angleSetpoint = Constants.kReefL3.magnitude(); // m_elevatorSetpoint = Constants.kReefL3; } if (m_elevatorHeight == 3) { + m_angleSetpoint = Constants.kReefL4.magnitude(); // m_elevatorSetpoint = Constants.kReefL4; } if (m_elevatorHeight == 4) { @@ -379,6 +383,11 @@ public Boolean isElevatorClose() { Constants.kElevatorClosetoTarget); } + public Boolean canAngulatorAngle() { + return MathUtil.isNear( + m_angleSetpoint, inputs.masterPosition.magnitude(), Constants.kElevatorClosetoTarget); + } + public Boolean isElevatorCloseEnoughToAngle() { return MathUtil.isNear(m_elevatorSetpoint.magnitude(), inputs.masterPosition.magnitude(), 0.75); } diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 3f4dc28..b562c7d 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -9,6 +9,9 @@ public class Feeder extends SubsystemBase { private FeederIO m_FeederIO; + // private final DigitalInput m_beambreak = new DigitalInput(0); + // private final DigitalInput m_beambreak2 = new DigitalInput(2); + private final FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged(); public Feeder(FeederIO feederIO) { @@ -34,11 +37,11 @@ public Command feederZeroCmd() { } public Trigger hasCoral() { - return m_FeederIO.hasCoral(); + return new Trigger(() -> inputs.hasCoral); // m_FeederIO.hasCoral(); } public Trigger beambreakTrigger() { - return m_FeederIO.beambreakTrigger(); + return new Trigger(() -> inputs.beambreakTrigger); // m_FeederIO.beambreakTrigger(); } public Command setFeederPosZero() { diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java index f94f8c4..57af99f 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIO.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.button.Trigger; import org.littletonrobotics.junction.AutoLog; public interface FeederIO { @@ -27,7 +26,7 @@ public static class FeederIOInputs { public double currentAmps = 0.0; public boolean hasCoral = false; public double position = 0.0; - public boolean beambreakTrigger; + public boolean beambreakTrigger = false; } public default void updateInputs(FeederIOInputs inputs) {} @@ -36,13 +35,13 @@ public default Command setFeederRollerVoltsCmd(double roller) { return new InstantCommand(); } - public default Trigger hasCoral() { - return new Trigger(() -> true); - } + // public default Trigger hasCoral() { + // return new Trigger(() -> true); + // } - public default Trigger beambreakTrigger() { - return new Trigger(() -> true); - } + // public default Trigger beambreakTrigger() { + // return new Trigger(() -> true); + // } public default Command setFeederPosZero() { return new InstantCommand(); diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index b9a3799..de6b689 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -30,7 +30,6 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; public class FeederIOTalonFX implements FeederIO { @@ -90,15 +89,15 @@ public void updateInputs(FeederIOInputs inputs) { inputs.beambreakTrigger = !m_beambreak2.get(); } - @Override - public Trigger hasCoral() { - return new Trigger(() -> !m_beambreak.get()); - } + // @Override + // public Trigger hasCoral() { + // return new Trigger(() -> !m_beambreak.get()); + // } - @Override - public Trigger beambreakTrigger() { - return new Trigger(() -> !m_beambreak2.get()); - } + // @Override + // public Trigger beambreakTrigger() { + // return new Trigger(() -> !m_beambreak2.get()); + // } @Override public Command setFeederRollerVoltsCmd(double roller) { @@ -113,7 +112,7 @@ public Command setFeederPosZero() { return Commands.runOnce( () -> { m_feederTalonFX.setControl( - m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 0.5).withSlot(1)); + m_PositionVoltage.withPosition(m_Position.getValueAsDouble() - 0.0).withSlot(1)); }); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 67cd995..7567cbb 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -66,12 +66,12 @@ public Trigger isShooterVelocityLowEnoughToScore() { // Left and right rock public Trigger isShooterXVelocityLowEnough() { - return new Trigger(() -> Math.abs(inputs.shooterXVelocity) < 20.0); + return new Trigger(() -> Math.abs(inputs.shooterXVelocity) < 30.0); // 20 } // Forward and backwards rock; angulating motion public Trigger isShooterYVelocityLowEnough() { - return new Trigger(() -> Math.abs(inputs.shooterYVelocity) < 75.0); + return new Trigger(() -> Math.abs(inputs.shooterYVelocity) < 100.0); // 75 } public Trigger isShooterZAccelLowEnough() { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index d474cff..4e9d997 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -133,7 +133,7 @@ public Command positionVoltageNoMovement() { return Commands.runOnce( () -> { m_shooterTalonFX.setControl( - m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 1).withSlot(0)); + m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 3.0).withSlot(0)); }); } From f09fa8bdeba58aa640280ef3fa40bad50778761d Mon Sep 17 00:00:00 2001 From: Kython89 Date: Mon, 14 Apr 2025 16:41:14 -0500 Subject: [PATCH 11/13] commit before updating WPILib --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d2256dd..505b31d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -51,7 +51,7 @@ public class Robot extends LoggedRobot { private boolean brakesDisabled = false; public Robot() { - super(0.0142); + super(0.02); // Record metadata Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); Logger.recordMetadata("BuildDate", BuildConstants.BUILD_DATE); From fe6f3567fd1e16bc1ace28901aa745f8ef91226b Mon Sep 17 00:00:00 2001 From: Kython89 Date: Mon, 14 Apr 2025 20:26:09 -0500 Subject: [PATCH 12/13] 4/14 (Before worlds) --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 22 ++++++++++--------- .../subsystems/shooter/ShooterIOTalonFX.java | 2 +- 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/build.gradle b/build.gradle index 1611678..14a4943 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bf7c30f..08601b6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -149,7 +149,7 @@ public final class Constants { public static final Double kShooterScoreSpeed = 8.0; public static final Double kShooterLength = 8.5; public static final Double kStowShooterVelo = 100.0; - public static final Double kL1ShooterVelo = 50.0; // 38 + public static final Double kL1ShooterVelo = 40.0; // 38 public static final Double kL1PostSlideShooterVelo = 30.0; public static final Double kL1_5ShooterVelo = 50.0; public static final Double kL4ShooterVelo = 60.0; // Only gets to 90 when at 110 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 71976ad..1346fb9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -289,8 +289,8 @@ private void configureButtonBindings() { driveController.povRight().whileTrue(Limelight.limelightDrive(drive)); driveController - .leftBumper() - .and(driveController.a()) + .leftStick() + // .and(driveController.a()) .onTrue( Commands.sequence( humanCmd(), @@ -301,15 +301,15 @@ private void configureButtonBindings() { // driveController.leftBumper().and(driveController.a()).onFalse(climber.climbCmd(0)); driveController - .leftBumper() - .and(driveController.b()) + .rightStick() + // .and(driveController.b()) .onTrue( Commands.sequence(humanCmd(), intake.intakeDownCmd(), climber.climbCmd(5)) .withName("ClimberUp")); driveController - .leftBumper() - .and(driveController.b()) + .rightStick() + // .and(driveController.b()) .onFalse(climber.climbCmd(0).withName("ClimberStop")); driveController @@ -321,12 +321,12 @@ private void configureButtonBindings() { driveController .a() - .and(driveController.leftBumper().negate()) + .and(driveController.leftStick().negate()) .onTrue(driveSetFieldRelativeCommand(true).ignoringDisable(true).withName("FieldRelative")); driveController .b() - .and(driveController.leftBumper().negate()) + .and(driveController.leftStick().negate()) .onTrue( driveSetFieldRelativeCommand(false).ignoringDisable(true).withName("RobotRelative")); @@ -340,6 +340,8 @@ private void configureButtonBindings() { .alongWith(feeder.feederZeroCmd()) .withName("IntakeStop")); + driveController.leftBumper().onTrue(shooter.setShooterVelocityCmd(100)); + // Zero Gyro Heading driveController .x() @@ -377,7 +379,7 @@ private void configureButtonBindings() { // Zerointake driveController - .back() + .start() .onTrue(intake.zeroIntakePivot().ignoringDisable(true).withName("ZeroIntake")); // Drives Normally But Angles Robot To Closest Reef Face @@ -849,7 +851,7 @@ private void configureButtonBindings() { .leftBumper() .onFalse( shooter - .setShooterVelocityCmd(100) + .setShooterVelocityCmd(0) .withName("BargeScore") .alongWith( Commands.runOnce( diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 4e9d997..63ef38b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -133,7 +133,7 @@ public Command positionVoltageNoMovement() { return Commands.runOnce( () -> { m_shooterTalonFX.setControl( - m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 3.0).withSlot(0)); + m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 3.1).withSlot(0)); }); } From 5ac3d5521e6f44912a4bddc780a117ec409cf9e7 Mon Sep 17 00:00:00 2001 From: Kython89 Date: Tue, 21 Oct 2025 18:09:39 -0500 Subject: [PATCH 13/13] all motors on one CANBUS --- .../autos/Copy of Frontface 4.auto | 86 ++++++++++++++ .../pathplanner/paths/Copy of F4 1.path | 91 +++++++++++++++ .../pathplanner/paths/Copy of F4 2.path | 107 ++++++++++++++++++ src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 10 ++ .../angulator/AngulatorIOTalonFX.java | 2 +- .../robot/subsystems/elevator/Elevator.java | 7 +- .../elevator/ElevatorIOTalonFX.java | 5 +- .../subsystems/feeder/FeederIOTalonFX.java | 2 +- .../subsystems/shooter/ShooterIOTalonFX.java | 6 +- .../subsystems/slide/SlideIOTalonFX.java | 2 +- 11 files changed, 311 insertions(+), 9 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Copy of Frontface 4.auto create mode 100644 src/main/deploy/pathplanner/paths/Copy of F4 1.path create mode 100644 src/main/deploy/pathplanner/paths/Copy of F4 2.path diff --git a/src/main/deploy/pathplanner/autos/Copy of Frontface 4.auto b/src/main/deploy/pathplanner/autos/Copy of Frontface 4.auto new file mode 100644 index 0000000..2265a08 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of Frontface 4.auto @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Copy of F4 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Copy of F4 2" + } + }, + { + "type": "path", + "data": { + "pathName": "F4 3" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "5PieceScore" + } + }, + { + "type": "named", + "data": { + "name": "autoL3DeAlgeafy" + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.4 + } + }, + { + "type": "path", + "data": { + "pathName": "F4 4" + } + }, + { + "type": "path", + "data": { + "pathName": "F4 5" + } + }, + { + "type": "path", + "data": { + "pathName": "F4 6" + } + }, + { + "type": "path", + "data": { + "pathName": "F4 7" + } + }, + { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of F4 1.path b/src/main/deploy/pathplanner/paths/Copy of F4 1.path new file mode 100644 index 0000000..c4ce656 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of F4 1.path @@ -0,0 +1,91 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.100822368421052, + "y": 0.5515625 + }, + "prevControl": null, + "nextControl": { + "x": 2.4439144736842096, + "y": 1.9563322368421043 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.175, + "y": 4.0 + }, + "prevControl": { + "x": 3.098190789469281, + "y": 3.0724506578918773 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CL41" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.25, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "autoIntakeDown", + "waypointRelativePos": 0.22800718132854628, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "autoIntakeDown" + } + } + }, + { + "name": "1stPieceUpFrontCopy", + "waypointRelativePos": 0.55, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "1stPieceUpFrontCopy" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 4.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": "Front 4", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Copy of F4 2.path b/src/main/deploy/pathplanner/paths/Copy of F4 2.path new file mode 100644 index 0000000..855bf99 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of F4 2.path @@ -0,0 +1,107 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.175, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.415935075290731, + "y": 4.253293538766084 + }, + "isLocked": false, + "linkedName": "CL41" + }, + { + "anchor": { + "x": 1.4063150104370652, + "y": 5.66 + }, + "prevControl": { + "x": 1.8281250000012157, + "y": 5.179605263154684 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F4 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6871069182390003, + "rotationDegrees": 136.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.25, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "5PieceScore", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "5PieceScore" + } + } + }, + { + "name": "ElevatorDown", + "waypointRelativePos": 0.06, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "ElevatorDown" + } + } + }, + { + "name": "IntakeOn", + "waypointRelativePos": 0.3, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOn" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 3.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 133.35422167699903 + }, + "reversed": false, + "folder": "Front 4", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 08601b6..581bd64 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -30,7 +30,7 @@ public final class Constants { public static final Distance kReefL1 = Meters.of(0.0); public static final Distance kReefL2 = Meters.of(0.5192); // 0.5492 public static final Distance kReefL3 = Meters.of(0.92); // 0.9706 - public static final Distance kReefL4 = Meters.of(1.61); // 1.5975 + public static final Distance kReefL4 = Meters.of(1.59115); // 1.61 // 1.5975 public static final Distance kDeAlgaeL2 = Meters.of(0.52); public static final Distance kDeAlgaeL3 = Meters.of(.95); public static final Distance kElevatorTroughScorePos = Meters.of(0.08); // 0.08 old // 0.20 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1346fb9..76a8da2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -211,6 +211,7 @@ public RobotContainer() { NamedCommands.registerCommand("ElevatorDown", autoL1()); NamedCommands.registerCommand("1stPieceUp", autoL41stPiece()); NamedCommands.registerCommand("1stPieceUpFront", autoL4Piece1FrontFace()); + NamedCommands.registerCommand("1stPieceUpFrontCopy", autoL4Piece1FrontFaceCopy()); NamedCommands.registerCommand("ScoreNoDelay", autoScoreNoDelay()); NamedCommands.registerCommand("5PieceScore", autoScoreFor5Piece()); NamedCommands.registerCommand("HoldPiece", autoGamePieceHold()); @@ -1311,6 +1312,15 @@ private final Command autoL4Piece1FrontFace() { slide.setSlidePositionCmd(Meters.of(0.31))); } + private final Command autoL4Piece1FrontFaceCopy() { + return Commands.sequence( + angulator.angulatorPosStateCmd(2), + elevator.setM_ElevatorHeightCmd(3), + elevator.setM_ElevatorAlignCmd(true), + new WaitUntilCommand(elevator::isSlideSafeToMove), + slide.setSlidePositionCmd(Meters.of(0.0))); + } + private final Command autoL4Piece2() { return Commands.sequence( new WaitUntilCommand(() -> shooter.hasCoral().getAsBoolean()), diff --git a/src/main/java/frc/robot/subsystems/angulator/AngulatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/angulator/AngulatorIOTalonFX.java index 67d7110..b5042a8 100644 --- a/src/main/java/frc/robot/subsystems/angulator/AngulatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/angulator/AngulatorIOTalonFX.java @@ -34,7 +34,7 @@ import org.littletonrobotics.junction.Logger; public class AngulatorIOTalonFX implements AngulatorIO { - private final TalonFX m_angulatorTalonFX = new TalonFX(Constants.kAngulatorCANID); + private final TalonFX m_angulatorTalonFX = new TalonFX(Constants.kAngulatorCANID, "2481"); private final MotionMagicVoltage m_MotionMagicVoltage = new MotionMagicVoltage(0); private final VoltageOut m_VoltageOut = new VoltageOut(0); diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index df489cd..194747b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -233,6 +233,9 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { m_pegged = false; } if (m_elevatorHeight == 2) { + // if(DriverStation.isAutonomousEnabled()) { + // m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3.plus(m_heightDistanceOffset)) + // } if (m_setHeightAlign == false) { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL3); m_elevatorSetpoint = Constants.kReefL3.plus(m_heightDistanceOffset); @@ -243,7 +246,9 @@ public void moveElevator(Pose2d pose, double coralToReefBranch) { m_pegged = false; } if (m_elevatorHeight == 3) { - if (m_setHeightAlign == false) { + if (DriverStation.isAutonomousEnabled()) { + m_ElevatorIO.setElevatorPositionMeters(Meters.of(1.6102)); // ReefL4 + 0.0127 + } else if (m_setHeightAlign == false) { m_ElevatorIO.setElevatorPositionMeters(Constants.kReefL4); m_elevatorSetpoint = Constants.kReefL4; m_pegged = false; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java index e2990e8..eb8a3f0 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -95,6 +95,7 @@ public ElevatorIOTalonFX() { TalonFXConfiguration elevatorRightCfg = new TalonFXConfiguration(); elevatorRightCfg.MotorOutput.NeutralMode = NeutralModeValue.Brake; elevatorRightCfg.CurrentLimits.StatorCurrentLimit = 80.0; + elevatorRightCfg.Feedback.SensorToMechanismRatio = 23.622; // elevatorRightCfg.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; m_elevatorRightTalonFX.getConfigurator().apply(elevatorRightCfg); @@ -123,7 +124,9 @@ public ElevatorIOTalonFX() { // m_CANrange2Dis = m_CaNrange2.getDistance(); m_elevatorRightTalonFX.setControl(new Follower(m_elevatorLeftTalonFX.getDeviceID(), true)); - // m_elevatorLeftTalonFX.setPosition(0); + + m_elevatorLeftTalonFX.setPosition(0); + m_elevatorRightTalonFX.setPosition(0); } @Override diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index de6b689..1934a97 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -33,7 +33,7 @@ import frc.robot.Constants; public class FeederIOTalonFX implements FeederIO { - private final TalonFX m_feederTalonFX = new TalonFX(Constants.kFeederCANID); + private final TalonFX m_feederTalonFX = new TalonFX(Constants.kFeederCANID, "2481"); private final VoltageOut m_VoltageOut = new VoltageOut(0); private final DigitalInput m_beambreak = new DigitalInput(0); private final DigitalInput m_beambreak2 = new DigitalInput(2); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java index 63ef38b..0022610 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFX.java @@ -34,7 +34,7 @@ import frc.robot.Constants; public class ShooterIOTalonFX implements ShooterIO { - private final TalonFX m_shooterTalonFX = new TalonFX(Constants.kShooterCANID); + private final TalonFX m_shooterTalonFX = new TalonFX(Constants.kShooterCANID, "2481"); private final VelocityVoltage m_VelocityVoltage = new VelocityVoltage(0); private final VoltageOut m_VoltageOut = new VoltageOut(0); private final PositionVoltage m_PositionVoltage = new PositionVoltage(0); @@ -43,7 +43,7 @@ public class ShooterIOTalonFX implements ShooterIO { private final StatusSignal m_Current; private final StatusSignal m_Position; - private final Pigeon2 pigeon = new Pigeon2(Constants.kShooterPigeon2); + private final Pigeon2 pigeon = new Pigeon2(Constants.kShooterPigeon2, "2481"); private final StatusSignal shooterXVelocity; private final StatusSignal shooterYVelocity; private final StatusSignal shooterZVelocity; @@ -133,7 +133,7 @@ public Command positionVoltageNoMovement() { return Commands.runOnce( () -> { m_shooterTalonFX.setControl( - m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 3.1).withSlot(0)); + m_PositionVoltage.withPosition(m_Position.getValueAsDouble() + 3.0).withSlot(0)); }); } diff --git a/src/main/java/frc/robot/subsystems/slide/SlideIOTalonFX.java b/src/main/java/frc/robot/subsystems/slide/SlideIOTalonFX.java index de4ac2a..fe3903f 100644 --- a/src/main/java/frc/robot/subsystems/slide/SlideIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/slide/SlideIOTalonFX.java @@ -23,7 +23,7 @@ import org.littletonrobotics.junction.Logger; public class SlideIOTalonFX implements SlideIO { - private final TalonFX m_slideTalonFX = new TalonFX(Constants.kSlideCANID); + private final TalonFX m_slideTalonFX = new TalonFX(Constants.kSlideCANID, "2481"); private final MotionMagicVoltage m_MotionMagicVoltage = new MotionMagicVoltage(0); private final VoltageOut m_VoltageOut = new VoltageOut(0); private final StatusSignal m_Velocity;