From 1d57d6abb84fee27954e56afa7798f99b88202eb Mon Sep 17 00:00:00 2001 From: LandonMorse <155490087+LandonMorse@users.noreply.github.com> Date: Thu, 20 Feb 2025 11:52:46 -0500 Subject: [PATCH 1/5] dont care --- .../robot/subsystems/ElevatorSubsystem.java | 17 +++++- .../robot/subsystems/VisionPoseEstimator.java | 54 +++++++++---------- 2 files changed, 43 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index fb48a55..65a7ce5 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -23,6 +23,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DutyCycle; import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,7 +39,7 @@ public class ElevatorSubsystem extends SubsystemBase { private VelocityVoltage elevatorVelocityVoltage = new VelocityVoltage(0); private TalonFXConfiguration climberConfig = new TalonFXConfiguration(); private CommandXboxController driver; - + private Servo linearActuator; /** Creates a new Elevator. */ public ElevatorSubsystem() { motor1.clearStickyFaults(); @@ -137,6 +138,20 @@ public Command intakeCoral(){ public Command returnHome(){ return this.runOnce(() -> elevatorSetPosition = Constants.ELEVATOR_LOWER_LIMIT); } + + public Command servoOut() { + return new InstantCommand( + () -> linearActuator.set(1), + this + ); +} + +public Command servoIn() { + return new InstantCommand( + () -> linearActuator.set(200d / 370d), // 200d / 270d + this + ); +} @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/VisionPoseEstimator.java b/src/main/java/frc/robot/subsystems/VisionPoseEstimator.java index ce20228..d5b828f 100644 --- a/src/main/java/frc/robot/subsystems/VisionPoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/VisionPoseEstimator.java @@ -47,8 +47,8 @@ public final class VisionPoseEstimator { - private static final Matrix multiTagStdDevs = Constants.PhotonVision.multiTagStdDevs; - private static final Object singleTagStdDevs = Constants.PhotonVision.singleTagStdDevs; + private static final Matrix multiTagStdDevs = null; + private static final Object singleTagStdDevs = null; private final PhotonCamera camera1; private final PhotonCamera camera2; private PhotonCamera currentCamera; @@ -160,7 +160,7 @@ public PhotonPipelineResult getLatestResult() { */ public Optional getEstimatedVisionGlobalPose() { useBestCamera(); - var visionEst = currentPhotonEstimator.update(getLatestResult()); + var visionEst = currentPhotonEstimator.update(null); double latestTimestamp = currentCamera.getLatestResult().getTimestampSeconds(); boolean newResult = Math.abs(latestTimestamp - lastEstTimestamp) > 1e-5; if (newResult) lastEstTimestamp = latestTimestamp; @@ -174,34 +174,34 @@ public Optional getEstimatedVisionGlobalPose() { * * @param estimatedPose The estimated pose to guess standard deviations for. */ - public Matrix getVisionEstimationStdDevs(Pose2d estimatedPose) { + public Matrix getVisionEstimationStdDevs(Pose2d estimatedPose) { if (DriverStation.isDisabled()) { return VecBuilder.fill(0.001d, 0.001d, 0.001d); } - Matrix estStdDevs = (Matrix) singleTagStdDevs; - var targets = getLatestResult().getTargets(); - int numTags = 0; - double avgDist = 0; - for (var tgt : targets) { - var tagPose = currentPhotonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); - if (tagPose.isEmpty()) continue; - numTags++; - avgDist += - tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); - } - if (numTags == 0) return (Matrix) estStdDevs; - avgDist /= numTags; - - // Decrease std devs if multiple targets are visible - if (numTags > 1) estStdDevs = multiTagStdDevs; - // Increase std devs based on (average) distance - if (numTags == 1 && avgDist > 4) - estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); - else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); - - return (Matrix) estStdDevs; - } + var estStdDevs = singleTagStdDevs; + var targets = getLatestResult().getTargets(); + int numTags = 0; + double avgDist = 0; + for (var tgt : targets) { + var tagPose = currentPhotonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); + } + if (numTags == 0) return (Matrix) estStdDevs; + avgDist /= numTags; + + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = multiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.equals(1 + (avgDist * avgDist / 30)); + + return (Matrix) estStdDevs; + } /** * @return The {@link Pose2d} of the robot according to the {@link SwerveDrivePoseEstimator} From 15a01a2386aa7f43842a0bdfb363a621ea0e67c3 Mon Sep 17 00:00:00 2001 From: LandonMorse <155490087+LandonMorse@users.noreply.github.com> Date: Fri, 21 Feb 2025 20:37:14 -0500 Subject: [PATCH 2/5] stupid --- build.gradle | 2 +- vendordeps/Phoenix6-frc2025-latest.json | 86 +++++++++++++++++-------- vendordeps/photonlib.json | 12 ++-- 3 files changed, 65 insertions(+), 35 deletions(-) diff --git a/build.gradle b/build.gradle index 81c0f3b..9b32861 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.1" } java { diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index acc78db..e2ee54c 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.2", + "version": "25.3.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "25.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,21 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.3.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +224,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +240,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +256,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +272,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +288,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +304,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +320,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +336,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +352,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +368,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +384,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +400,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +416,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.3.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +428,22 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.3.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 990ce2f..955f250 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2025.2.1-rc1", + "version": "v2025.2.1-rc2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1-rc1", + "version": "v2025.2.1-rc2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2025.2.1-rc1", + "version": "v2025.2.1-rc2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2025.2.1-rc1", + "version": "v2025.2.1-rc2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2025.2.1-rc1" + "version": "v2025.2.1-rc2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2025.2.1-rc1" + "version": "v2025.2.1-rc2" } ] } \ No newline at end of file From 5fdc259d9738be4c15d4aebc5274fc5c1d065dbe Mon Sep 17 00:00:00 2001 From: ngciviello <25nciviello@mtbluersd.org> Date: Wed, 12 Mar 2025 14:11:58 -0400 Subject: [PATCH 3/5] yes --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/commands/RobotTurn.java | 43 +++++++++++++++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 8 +++- 3 files changed, 51 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/commands/RobotTurn.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a8dd843..d597c5b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -123,7 +123,7 @@ private void configureButtonBindings() { ))); driver.rightTrigger().whileTrue(swerveSubsystem.halveRotationSpeed()); - + elevatorSubsystem.setDefaultCommand(elevatorSubsystem.run(()->elevatorSubsystem.driveByJoystick(auxDriver::getRightY))); wristSubsystem.setDefaultCommand(wristSubsystem.run(()->wristSubsystem.spinByJoystick(auxDriver::getLeftY))); diff --git a/src/main/java/frc/robot/commands/RobotTurn.java b/src/main/java/frc/robot/commands/RobotTurn.java new file mode 100644 index 0000000..641d83c --- /dev/null +++ b/src/main/java/frc/robot/commands/RobotTurn.java @@ -0,0 +1,43 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.SwerveSubsystem; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class RobotTurn extends Command { + + + private SwerveSubsystem swerveSubsystem; + + /** Creates a new RobotTurn. */ + public RobotTurn(SwerveSubsystem swerveSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(swerveSubsystem ); + this.swerveSubsystem = swerveSubsystem; + } + + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + swerveSubsystem.drive(new Translation2d(),1d,false,false ); + } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 930dd84..8bad1f5 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -6,12 +6,13 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; - + import com.ctre.phoenix6.hardware.CANcoder; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; + public class IntakeSubsystem extends SubsystemBase { private final TalonFX intakeMotor = new TalonFX(Constants.INTAKE_MOTOR_ID); private final TalonFXConfiguration intakeConfig= new TalonFXConfiguration(); @@ -38,6 +39,7 @@ public Command intakeAlgae() { ); } + public Command stopIntake() { return new InstantCommand(() -> stopMotor(), this); } @@ -46,6 +48,10 @@ public Command intakeCoral() { return new InstantCommand(()->spinMotor(-.3), this); } + public void HoldAlgae(double setPosition) { + intakeMotor.setPosition(setPosition, .1); + } + @Override public void periodic() { // This method will be called once per scheduler run From 1e8c78f17fc21f76631040870bd28cdd98666d84 Mon Sep 17 00:00:00 2001 From: LandonMorse <155490087+LandonMorse@users.noreply.github.com> Date: Mon, 31 Mar 2025 20:16:13 -0400 Subject: [PATCH 4/5] fixes and path doesn't build but will soon --- .../pathplanner/paths/B Coarl Station R.path | 54 ++++++ src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 21 ++- .../robot/subsystems/ElevatorSubsystem.java | 161 ++++++++++++------ .../frc/robot/subsystems/IntakeSubsystem.java | 13 +- 5 files changed, 198 insertions(+), 53 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/B Coarl Station R.path diff --git a/src/main/deploy/pathplanner/paths/B Coarl Station R.path b/src/main/deploy/pathplanner/paths/B Coarl Station R.path new file mode 100644 index 0000000..1a19bf7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B Coarl Station R.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.7684311224489797, + "y": 2.13345025510204 + }, + "prevControl": null, + "nextControl": { + "x": 1.6863520408163264, + "y": 1.8424426020408156 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4326530612244897, + "y": 1.3201211734693867 + }, + "prevControl": { + "x": 1.5445790816326532, + "y": 1.6932079081632638 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 55.451632943988734 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 65.397205841715 + }, + "useDefaultConstraints": true +} \ 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 65e1d18..1274010 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -208,7 +208,7 @@ public static final class AutoConstants { //TODO: The below constants are used i public static final int ELEVATOR_MOTOR_LEFT_ID = 10; public static final int ELEVATOR_MOTOR_RIGHT_ID = 11; public static final double ELEVATOR_LOWER_LIMIT = 0.335449; //.403809; //TODO: Placeholder - //public static final double ELEVATOR_UPPER_LIMIT = 43; //TODO: Placeholder + public static final double ELEVATOR_UPPER_LIMIT = 43; //TODO: Placeholder public static final double ELEVATOR_MAX_ROTATIONS_PER_SEC = 80; public static final double ELEVATOR_CAN_TO_MOTOR_RATIO = 5.24954/0.916748; public static final int ELEVATOR_CAN_CODER_ID = 4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 583110e..63e79d7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -3,9 +3,12 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; @@ -147,7 +150,7 @@ private void configureButtonBindings() { // ))); driver.rightTrigger().whileTrue(swerveSubsystem.halveRotationSpeed()); - + elevatorSubsystem.setDefaultCommand(elevatorSubsystem.run(()->elevatorSubsystem.driveByJoystick(auxDriver::getRightY))); wristSubsystem.setDefaultCommand(wristSubsystem.run(()->wristSubsystem.spinByJoystick(auxDriver::getLeftY))); @@ -180,7 +183,19 @@ private void configureButtonBindings() { */ public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - return autoChooser.getSelected(); + return autoChooser.getSelected(); + // Load the path we want to pathfind to and follow +PathPlannerPath path = PathPlannerPath.fromPathFile("B Coarl Station R"); + +// Create the constraints to use while pathfinding. The constraints defined in the path will only be used for the path. +PathConstraints constraints = new PathConstraints( + 3.0, 4.0, + Units.degreesToRadians(540), Units.degreesToRadians(720)); + +// Since AutoBuilder is configured, we can use it to build pathfinding commands +Command pathfindingCommand = AutoBuilder.pathfindThenFollowPath( + path, + constraints); //return new PathPlannerAuto("New Auto"); // Command[] autoCommands = new Command[numOfAutoActions.getSelected()*2]; @@ -224,4 +239,4 @@ public void setupAutoChoosers() { hasSetupAutoChoosers = true; } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index fb48a55..4a68856 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -5,24 +5,33 @@ package frc.robot.subsystems; import java.util.ResourceBundle.Control; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import javax.swing.text.Position; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CANcoderConfigurator; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.ControlModeValue; +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.DutyCycle; import edu.wpi.first.wpilibj.RobotState; +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -30,52 +39,63 @@ import frc.robot.Constants; public class ElevatorSubsystem extends SubsystemBase { - private final TalonFX motor1 = new TalonFX(Constants.ELEVATOR_MOTOR_LEFT_ID); - private final TalonFX motor2 = new TalonFX(Constants.ELEVATOR_MOTOR_RIGHT_ID); - private boolean enableSetPosition = true; - private double elevatorSetPosition; - private PositionVoltage elevatorPositionVoltage = new PositionVoltage(0); - private VelocityVoltage elevatorVelocityVoltage = new VelocityVoltage(0); - private TalonFXConfiguration climberConfig = new TalonFXConfiguration(); - private CommandXboxController driver; - - /** Creates a new Elevator. */ - public ElevatorSubsystem() { - motor1.clearStickyFaults(); - motor2.clearStickyFaults(); - - climberConfig.CurrentLimits.SupplyCurrentLimit = 40; - climberConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - - climberConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Constants.ELEVATOR_UPPER_LIMIT; - climberConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Constants.ELEVATOR_LOWER_LIMIT; - climberConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - climberConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - - climberConfig.Slot0.kP = 0.435; - climberConfig.Slot0.kI = 0.02; - climberConfig.Slot0.kD = 0.0; - climberConfig.Slot0.kG = 0.65; - - climberConfig.Slot1.kP = 0.18; - climberConfig.Slot1.kI = 0;//0.005; - climberConfig.Slot1.kD = 0.0; - climberConfig.Slot1.kG = 0.25; - climberConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - climberConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + private final TalonFX motor1 = new TalonFX(Constants.ELEVATOR_MOTOR_LEFT_ID); + private final TalonFX motor2 = new TalonFX(Constants.ELEVATOR_MOTOR_RIGHT_ID); + private boolean enableSetPosition = true; + private double elevatorSetPosition; + private PositionVoltage elevatorPositionVoltage = new PositionVoltage(0); + private VelocityVoltage elevatorVelocityVoltage = new VelocityVoltage(0); + private TalonFXConfiguration climberConfig = new TalonFXConfiguration(); + private CommandXboxController driver; + private Servo elevatorStopper; + private double elevatorStopperPosition; + private final CANcoder elevatorCANcoder; + + /** Creates a new Elevator. */ + public ElevatorSubsystem() { + motor1.clearStickyFaults(); + motor2.clearStickyFaults(); + + climberConfig.CurrentLimits.SupplyCurrentLimit = 40; + climberConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + + // climberConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Constants.ELEVATOR_UPPER_LIMIT; + climberConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Constants.ELEVATOR_LOWER_LIMIT; + climberConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; + climberConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; + // climberConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + // climberConfig.DifferentialSensors.DifferentialRemoteSensorID = 4; + + climberConfig.Slot0.kP = 0.435; + climberConfig.Slot0.kI = 0.02; + climberConfig.Slot0.kD = 0.0; + climberConfig.Slot0.kG = 0.65; + + climberConfig.Slot1.kP = 0.17; + climberConfig.Slot1.kI = 0.006;//0.005; + climberConfig.Slot1.kD = 0.01; + climberConfig.Slot1.kG = 0.65; + + climberConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + climberConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + elevatorCANcoder = new CANcoder(Constants.ELEVATOR_CAN_CODER_ID); motor1.getConfigurator().apply(climberConfig); motor2.getConfigurator().apply(climberConfig); motor2.setControl(new Follower(Constants.ELEVATOR_MOTOR_LEFT_ID, false)); + resetMotorEncoderToAbsolute(); + elevatorStopper = new Servo(0); } - public void spinMotor(double speed) { - motor1.set(speed); + public Command spinMotor(double speed) { + return this.runEnd(()->motor1.set(speed), ()->stopMotor()); } - public void stopMotor() { - motor1.stopMotor(); - enableSetPosition = false; + public Command stopMotor() { + return this.runOnce(()-> {motor1.stopMotor(); + enableSetPosition = false;}); } public void setHoldPosition(double setPosition) { @@ -83,16 +103,29 @@ public void setHoldPosition(double setPosition) { } public double getPosition(){ + // return elevatorCANcoder.getPosition().getValueAsDouble(); return motor1.getPosition().getValueAsDouble(); } public Command stopElevator() { enableSetPosition = true; - return new InstantCommand(()->stopMotor(), this); + return this.runOnce(()->stopMotor()); } public Command resetPosition() { - return this.run(()->motor1.setPosition(Constants.ELEVATOR_LOWER_LIMIT)).andThen(this.run(()->elevatorSetPosition = Constants.ELEVATOR_LOWER_LIMIT)); + return this.run(()->motor1.setPosition(0)); + } + + public Command elevatorToIntakeAuto(){ + return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.CORALSTATION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.CORALSTATION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); + } + + public Command elevatorToL2Auto(){ + return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.L2REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.L2REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); + } + + public Command elevatorToL4Auto(){ + return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.L4REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.L4REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); } //lets the driver control the position, when no input is given the elevator will hold its position @@ -101,10 +134,12 @@ public void driveByJoystick(DoubleSupplier amount) { double amountSpin = MathUtil.applyDeadband(amount.getAsDouble(), .1); elevatorVelocityVoltage.Velocity = -amountSpin*Constants.ELEVATOR_MAX_ROTATIONS_PER_SEC; motor1.setControl(elevatorVelocityVoltage); + resetMotorEncoderToAbsolute(); elevatorSetPosition = getPosition(); } else { - if (motor1.getPosition().getValueAsDouble() <= elevatorSetPosition) { + resetMotorEncoderToAbsolute(); + if (getPosition() <= elevatorSetPosition) { motor1.setControl(elevatorPositionVoltage.withPosition(elevatorSetPosition).withSlot(0)); } else { @@ -115,36 +150,66 @@ public void driveByJoystick(DoubleSupplier amount) { //gives new set positions for the elevator to go to public Command L1Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L1REEFPOSITION); + return this.runOnce(() -> elevatorSetPosition = Constants.L1REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); } public Command L2Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L2REEFPOSITION); + return this.runOnce(() -> elevatorSetPosition = Constants.L2REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); } public Command L3Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L3REEFPOSITION); + return this.runOnce(() -> elevatorSetPosition = Constants.L3REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); } public Command L4Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.ELEVATOR_UPPER_LIMIT); + return this.runOnce(() -> elevatorSetPosition = Constants.L4REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); } public Command intakeCoral(){ - return this.runOnce(() -> elevatorSetPosition = Constants.CORALSTATION); + return this.runOnce(() -> elevatorSetPosition = Constants.CORALSTATION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); } public Command returnHome(){ - return this.runOnce(() -> elevatorSetPosition = Constants.ELEVATOR_LOWER_LIMIT); + return this.runOnce(() -> elevatorSetPosition = Constants.ELEVATOR_LOWER_LIMIT * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); + } + + public Command linearActuatorOut(){ + return this.run( + () -> elevatorStopper.set(1) + ); + } + + public Command linearActuatorIn() { + return this.run( + () -> elevatorStopper.set(200d / 370d) + ); } + + public Angle getElevatorPosition() { + return elevatorCANcoder.getAbsolutePosition().getValue(); +} +public void resetMotorEncoderToAbsolute() { + // Angle newPosition = getElevatorPosition() ; + + motor1.setPosition(elevatorCANcoder.getPosition().getValueAsDouble()*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); +} + @Override public void periodic() { // This method will be called once per scheduler run //makes sure when the robot is disabled the position is being updated so when reenabled the elevator will not //go back to the position it was at when it was disabled (hazard) + if (RobotState.isDisabled()) { + resetMotorEncoderToAbsolute(); elevatorSetPosition = getPosition(); } + + elevatorStopperPosition = elevatorStopper.getAngle(); + SmartDashboard.putNumber("Servo Position", elevatorStopperPosition); + SmartDashboard.putNumber("Elevator Position", getPosition()); + SmartDashboard.putNumber("Set Position", elevatorSetPosition); + SmartDashboard.putNumber("Elevator Motor Temperature", motor1.getDeviceTemp().getValueAsDouble()); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 930dd84..01e44f3 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -4,9 +4,12 @@ package frc.robot.subsystems; +import java.util.function.BooleanSupplier; + import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -24,6 +27,13 @@ public IntakeSubsystem() { intakeMotor.clearStickyFaults(); } + public boolean coralInIntake() { + if (intakeMotor.getSupplyVoltage().getValueAsDouble() > 20) { + return true; + } + else return false; + } + public void spinMotor(double speed) { intakeMotor.set(speed); } @@ -49,5 +59,6 @@ public Command intakeCoral() { @Override public void periodic() { // This method will be called once per scheduler run + SmartDashboard.putNumber("Intake Motor Temperature", intakeMotor.getDeviceTemp().getValueAsDouble()); } -} +} \ No newline at end of file From e3d8a6e4fb22249153998f748c7233de4db3d72a Mon Sep 17 00:00:00 2001 From: LandonMorse <155490087+LandonMorse@users.noreply.github.com> Date: Wed, 18 Mar 2026 17:37:40 -0400 Subject: [PATCH 5/5] dskdfggj --- build.gradle | 2 +- settings.gradle | 2 +- .../commands/CmdElevatorSetPosition.java | 34 --- .../robot/subsystems/ElevatorSubsystem.java | 215 ------------------ .../frc/robot/subsystems/IntakeSubsystem.java | 64 ------ .../frc/robot/subsystems/WristSubsystem.java | 142 ------------ vendordeps/WPILibNewCommands.json | 2 +- 7 files changed, 3 insertions(+), 458 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/CmdElevatorSetPosition.java delete mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/WristSubsystem.java diff --git a/build.gradle b/build.gradle index 9b32861..2f26112 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" } java { diff --git a/settings.gradle b/settings.gradle index 969c7b0..486702a 100644 --- a/settings.gradle +++ b/settings.gradle @@ -4,7 +4,7 @@ pluginManagement { repositories { mavenLocal() gradlePluginPortal() - String frcYear = '2025' + String frcYear = '2026' File frcHome if (OperatingSystem.current().isWindows()) { String publicFolder = System.getenv('PUBLIC') diff --git a/src/main/java/frc/robot/commands/CmdElevatorSetPosition.java b/src/main/java/frc/robot/commands/CmdElevatorSetPosition.java deleted file mode 100644 index 825cb29..0000000 --- a/src/main/java/frc/robot/commands/CmdElevatorSetPosition.java +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class CmdElevatorSetPosition extends Command { - /** Creates a new CmdElevatorSetPosition. */ - public CmdElevatorSetPosition() { - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} -double speed = 0.0; - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java deleted file mode 100644 index 4a68856..0000000 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ /dev/null @@ -1,215 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import java.util.ResourceBundle.Control; -import java.util.function.BooleanSupplier; -import java.util.function.DoubleSupplier; - -import javax.swing.text.Position; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.CANcoderConfigurator; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.ControlModeValue; -import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import com.fasterxml.jackson.databind.ser.std.StdKeySerializers.Default; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.wpilibj.DutyCycle; -import edu.wpi.first.wpilibj.RobotState; -import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Constants; - -public class ElevatorSubsystem extends SubsystemBase { - - private final TalonFX motor1 = new TalonFX(Constants.ELEVATOR_MOTOR_LEFT_ID); - private final TalonFX motor2 = new TalonFX(Constants.ELEVATOR_MOTOR_RIGHT_ID); - private boolean enableSetPosition = true; - private double elevatorSetPosition; - private PositionVoltage elevatorPositionVoltage = new PositionVoltage(0); - private VelocityVoltage elevatorVelocityVoltage = new VelocityVoltage(0); - private TalonFXConfiguration climberConfig = new TalonFXConfiguration(); - private CommandXboxController driver; - private Servo elevatorStopper; - private double elevatorStopperPosition; - private final CANcoder elevatorCANcoder; - - /** Creates a new Elevator. */ - public ElevatorSubsystem() { - motor1.clearStickyFaults(); - motor2.clearStickyFaults(); - - climberConfig.CurrentLimits.SupplyCurrentLimit = 40; - climberConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - - // climberConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Constants.ELEVATOR_UPPER_LIMIT; - climberConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Constants.ELEVATOR_LOWER_LIMIT; - climberConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = false; - climberConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - // climberConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; - // climberConfig.DifferentialSensors.DifferentialRemoteSensorID = 4; - - climberConfig.Slot0.kP = 0.435; - climberConfig.Slot0.kI = 0.02; - climberConfig.Slot0.kD = 0.0; - climberConfig.Slot0.kG = 0.65; - - climberConfig.Slot1.kP = 0.17; - climberConfig.Slot1.kI = 0.006;//0.005; - climberConfig.Slot1.kD = 0.01; - climberConfig.Slot1.kG = 0.65; - - climberConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - climberConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - elevatorCANcoder = new CANcoder(Constants.ELEVATOR_CAN_CODER_ID); - - motor1.getConfigurator().apply(climberConfig); - motor2.getConfigurator().apply(climberConfig); - motor2.setControl(new Follower(Constants.ELEVATOR_MOTOR_LEFT_ID, false)); - resetMotorEncoderToAbsolute(); - elevatorStopper = new Servo(0); - } - - public Command spinMotor(double speed) { - return this.runEnd(()->motor1.set(speed), ()->stopMotor()); - } - - public Command stopMotor() { - return this.runOnce(()-> {motor1.stopMotor(); - enableSetPosition = false;}); - } - - public void setHoldPosition(double setPosition) { - motor1.setPosition(setPosition, .1); - } - - public double getPosition(){ - // return elevatorCANcoder.getPosition().getValueAsDouble(); - return motor1.getPosition().getValueAsDouble(); - } - - public Command stopElevator() { - enableSetPosition = true; - return this.runOnce(()->stopMotor()); - } - - public Command resetPosition() { - return this.run(()->motor1.setPosition(0)); - } - - public Command elevatorToIntakeAuto(){ - return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.CORALSTATION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.CORALSTATION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); - } - - public Command elevatorToL2Auto(){ - return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.L2REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.L2REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); - } - - public Command elevatorToL4Auto(){ - return this.runOnce(() -> {motor1.setControl(elevatorPositionVoltage.withPosition(Constants.L4REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO).withSlot(1)); elevatorSetPosition = Constants.L4REEFPOSITION*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO;}); - } - - //lets the driver control the position, when no input is given the elevator will hold its position - public void driveByJoystick(DoubleSupplier amount) { - if(amount.getAsDouble()>.1 || amount.getAsDouble()<-.1){ - double amountSpin = MathUtil.applyDeadband(amount.getAsDouble(), .1); - elevatorVelocityVoltage.Velocity = -amountSpin*Constants.ELEVATOR_MAX_ROTATIONS_PER_SEC; - motor1.setControl(elevatorVelocityVoltage); - resetMotorEncoderToAbsolute(); - elevatorSetPosition = getPosition(); - } - else { - resetMotorEncoderToAbsolute(); - if (getPosition() <= elevatorSetPosition) { - motor1.setControl(elevatorPositionVoltage.withPosition(elevatorSetPosition).withSlot(0)); - } - else { - motor1.setControl(elevatorPositionVoltage.withPosition(elevatorSetPosition).withSlot(1)); - } - } - } - - //gives new set positions for the elevator to go to - public Command L1Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L1REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command L2Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L2REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command L3Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L3REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command L4Reef() { - return this.runOnce(() -> elevatorSetPosition = Constants.L4REEFPOSITION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command intakeCoral(){ - return this.runOnce(() -> elevatorSetPosition = Constants.CORALSTATION * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command returnHome(){ - return this.runOnce(() -> elevatorSetPosition = Constants.ELEVATOR_LOWER_LIMIT * Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); - } - - public Command linearActuatorOut(){ - return this.run( - () -> elevatorStopper.set(1) - ); - } - - public Command linearActuatorIn() { - return this.run( - () -> elevatorStopper.set(200d / 370d) - ); - } - - - public Angle getElevatorPosition() { - return elevatorCANcoder.getAbsolutePosition().getValue(); -} -public void resetMotorEncoderToAbsolute() { - // Angle newPosition = getElevatorPosition() ; - - motor1.setPosition(elevatorCANcoder.getPosition().getValueAsDouble()*Constants.ELEVATOR_CAN_TO_MOTOR_RATIO); -} - - @Override - public void periodic() { - // This method will be called once per scheduler run - //makes sure when the robot is disabled the position is being updated so when reenabled the elevator will not - //go back to the position it was at when it was disabled (hazard) - - if (RobotState.isDisabled()) { - resetMotorEncoderToAbsolute(); - elevatorSetPosition = getPosition(); - } - - elevatorStopperPosition = elevatorStopper.getAngle(); - SmartDashboard.putNumber("Servo Position", elevatorStopperPosition); - SmartDashboard.putNumber("Elevator Position", getPosition()); - SmartDashboard.putNumber("Set Position", elevatorSetPosition); - SmartDashboard.putNumber("Elevator Motor Temperature", motor1.getDeviceTemp().getValueAsDouble()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java deleted file mode 100644 index 01e44f3..0000000 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import java.util.function.BooleanSupplier; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class IntakeSubsystem extends SubsystemBase { - private final TalonFX intakeMotor = new TalonFX(Constants.INTAKE_MOTOR_ID); - private final TalonFXConfiguration intakeConfig= new TalonFXConfiguration(); - /** Creates a new IntakeModule. */ - public IntakeSubsystem() { - intakeConfig.CurrentLimits.SupplyCurrentLimit = 60; - intakeConfig.CurrentLimits.SupplyCurrentLowerLimit = 35; - intakeConfig.CurrentLimits.SupplyCurrentLowerTime = .1; - intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - intakeMotor.clearStickyFaults(); - } - - public boolean coralInIntake() { - if (intakeMotor.getSupplyVoltage().getValueAsDouble() > 20) { - return true; - } - else return false; - } - - public void spinMotor(double speed) { - intakeMotor.set(speed); - } - - public void stopMotor() { - intakeMotor.stopMotor(); - } - - public Command intakeAlgae() { - return this.run( - () -> spinMotor(.3) - ); - } - - public Command stopIntake() { - return new InstantCommand(() -> stopMotor(), this); - } - - public Command intakeCoral() { - return new InstantCommand(()->spinMotor(-.3), this); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - SmartDashboard.putNumber("Intake Motor Temperature", intakeMotor.getDeviceTemp().getValueAsDouble()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java deleted file mode 100644 index 4b4b850..0000000 --- a/src/main/java/frc/robot/subsystems/WristSubsystem.java +++ /dev/null @@ -1,142 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import java.util.function.DoubleSupplier; - -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.wpilibj.RobotState; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; - -public class WristSubsystem extends SubsystemBase { - private TalonFX wristMotor = new TalonFX(Constants.WRIST_MOTOR_ID); - private TalonFXConfiguration wristConfig = new TalonFXConfiguration(); - private VelocityVoltage wristVelocityVoltage = new VelocityVoltage(0); - private PositionVoltage wristPositionVoltage = new PositionVoltage(0); - private double wristSetPosition; - - /** Creates a new WristModule. */ - public WristSubsystem() { - wristConfig.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Constants.WRIST_UPPER_LIMIT; - wristConfig.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Constants.WRIST_LOWER_LIMIT; - wristConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = true; - wristConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = true; - - wristConfig.CurrentLimits.SupplyCurrentLimit = 60; - wristConfig.CurrentLimits.SupplyCurrentLowerLimit = 35; - wristConfig.CurrentLimits.SupplyCurrentLowerTime = .01; - wristConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - wristConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - wristConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - wristConfig.Slot0.kP = .2;//0.3; - wristConfig.Slot0.kI = 0.001;//.005; - wristConfig.Slot0.kD = 0.0; - wristConfig.Slot0.kG = -.16; - - wristConfig.Slot1.kP =0.0; - wristConfig.Slot1.kI = 0.0; - wristConfig.Slot1.kD = 0.0; - wristConfig.Slot1.kG = .3; - - wristMotor.getConfigurator().apply(wristConfig); - wristMotor.clearStickyFaults(); - } - - public void spinWristMotor(double speed){ - wristMotor.set(speed); - } - - public void stopWristMotor(){ - wristMotor.stopMotor(); - } - - public double getWristPosition() { - return wristMotor.getPosition().getValueAsDouble(); - } - - public void updatePosition(){ - wristPositionVoltage.Position = getWristPosition(); - } - - public void spinByJoystick(DoubleSupplier amount) { - double spinAmount = MathUtil.applyDeadband(amount.getAsDouble(), .1); - if(spinAmount>.1||spinAmount<-.01){ - wristVelocityVoltage.Velocity = spinAmount*Constants.WRIST_MAX_ROTATIONS_PER_SEC; - wristMotor.setControl(wristVelocityVoltage); - wristSetPosition = getWristPosition(); - } - else { - wristMotor.setControl(wristPositionVoltage.withPosition(wristSetPosition).withSlot(0)); - } - } - - public Command wristToIntake(){ - return this.runOnce(() -> wristSetPosition = Constants.WRIST_INTAKE_POSITION); - } - - public Command wristToL1(){ - return this.runOnce(() -> wristSetPosition = Constants.WRIST_L1_POSITION); - } - - public Command wristToL1Auto(){ - return this.runOnce(() -> wristMotor.setControl(wristPositionVoltage.withPosition(Constants.WRIST_L1_POSITION).withSlot(0))); - } - - public Command wristToLMIDAuto(){ - return this.runOnce(() -> wristMotor.setControl(wristPositionVoltage.withPosition(Constants.WRIST_LMID_POSITION).withSlot(0))); - } - - public Command wristToL4Auto(){ - return this.runOnce(() -> wristMotor.setControl(wristPositionVoltage.withPosition(Constants.WRIST_L4_POSITION).withSlot(0))); - } - - public Command wristToLMID(){ - return this.runOnce(() -> wristSetPosition = Constants.WRIST_LMID_POSITION); - } - - public Command wristToL4(){ - return this.runOnce(() -> wristSetPosition = Constants.WRIST_L4_POSITION); - } - - public Command wristToBarge() { - return this.runOnce(() -> wristSetPosition = Constants.WIRST_BARGE_POSITION); - } - - public Command spinWrist(double speed) { - return new InstantCommand(()->spinWrist(speed), this); - } - - public Command stopWrist() { - return new InstantCommand(()-> stopWristMotor(), this); - } - - public Command resetPosition() { - return this.runOnce(() -> wristMotor.setPosition(0)); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - //makes sure when the robot is disabled the position is being updated so when reenabled the wrist will not - //go back to the position it was at when it was disabled (hazard) - if(RobotState.isDisabled()) { - wristSetPosition = getWristPosition(); - } - SmartDashboard.putNumber(" Wrist Set Position", wristSetPosition); - SmartDashboard.putNumber("Real Position", wristMotor.getPosition().getValueAsDouble()); - } -} diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json index 3718e0a..c54ae11 100644 --- a/vendordeps/WPILibNewCommands.json +++ b/vendordeps/WPILibNewCommands.json @@ -3,7 +3,7 @@ "name": "WPILib-New-Commands", "version": "1.0.0", "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266", - "frcYear": "2025", + "frcYear": "2026", "mavenUrls": [], "jsonUrl": "", "javaDependencies": [