From 416c1ca392e024f3d1ca4b6cf279e06c6ab014c4 Mon Sep 17 00:00:00 2001 From: rohan Date: Sat, 17 Feb 2024 11:57:00 -0800 Subject: [PATCH 01/32] fix yagsl.json --- vendordeps/yagsl.json | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/vendordeps/yagsl.json b/vendordeps/yagsl.json index 2b19885..4edaad3 100644 --- a/vendordeps/yagsl.json +++ b/vendordeps/yagsl.json @@ -1,11 +1,8 @@ { "fileName": "yagsl.json", "name": "YAGSL", -<<<<<<< HEAD - "version": "2024.4.8.2", -======= + "version": "2024.4.8.3", ->>>>>>> pathplanning "frcYear": "2024", "uuid": "1ccce5a4-acd2-4d18-bca3-4b8047188400", "mavenUrls": [ @@ -16,11 +13,7 @@ { "groupId": "swervelib", "artifactId": "YAGSL-java", -<<<<<<< HEAD - "version": "2024.4.8.2" -======= "version": "2024.4.8.3" ->>>>>>> pathplanning } ], "jniDependencies": [], From 23c7dc3aa6ba2f1a2fb3196f7045d1a1da32d7b0 Mon Sep 17 00:00:00 2001 From: rohan Date: Sat, 17 Feb 2024 11:59:38 -0800 Subject: [PATCH 02/32] add github action for building --- .github/workflows/main.yml | 46 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) create mode 100644 .github/workflows/main.yml diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 0000000..53643c4 --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,46 @@ +name: CI + +# Controls when the action will run. Triggers the workflow on push or pull request +# events but only for the main branch. +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + # This workflow contains a single job called "build" + build: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + # This grabs the WPILib docker container + container: wpilib/roborio-cross-ubuntu:2024-18.04 + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v2 + + - name: Set up JDK 17 + uses: actions/setup-java@v3 + with: + distribution: temurin + java-version: 17 + cache: gradle + # Grant execute permission for gradlew + - name: Grant execute permission for gradlew + run: chmod +x gradle + + - name: Run spotless code formatter + run: ./gradlew :spotlessApply + + # Runs a single command using the runners shell + - name: Compile and run tests on robot code + run: ./gradlew build + + - uses: "actions/checkout@master" + - name: "TODO to Issue" + uses: "alstr/todo-to-issue-action@v4" + id: "todo" \ No newline at end of file From 8b6e3cbb4c967d02e24350a2f8bb928dfa22712b Mon Sep 17 00:00:00 2001 From: Ken Huang Date: Sat, 17 Feb 2024 21:04:56 -0800 Subject: [PATCH 03/32] Replaced 2mdistancesensor with new TimeOfFlight sensors --- .../java/frc/robot/subsystems/Indexer.java | 9 +-- .../java/frc/robot/subsystems/Intake.java | 10 +-- vendordeps/playingwithfusion2024.json | 71 +++++++++++++++++++ 3 files changed, 81 insertions(+), 9 deletions(-) create mode 100644 vendordeps/playingwithfusion2024.json diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index ebddb19..22e0f4a 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.ShooterDataTable; import frc.robot.inputs.PoseEstimator; +import com.playingwithfusion.TimeOfFlight; import lombok.Getter; public class Indexer extends SubsystemBase { @@ -40,7 +41,7 @@ public class Indexer extends SubsystemBase { private final CANSparkMax indexMotor; private final CANSparkMax leadAngleMotor; private final CANSparkMax followAngleMotor; - private final Rev2mDistanceSensor sensor; + private final TimeOfFlight sensor; private final LinearSystemLoop loop; private final ShooterDataTable table; @Getter private State state; @@ -55,7 +56,7 @@ public Indexer(ShooterDataTable table, final PoseEstimator poseEstimator) { followAngleMotor.follow(leadAngleMotor); this.table = table; sensor = - new Rev2mDistanceSensor(Rev2mDistanceSensor.Port.kOnboard); // TODO: Figure out right value + new TimeOfFlight(0); // TODO: Fill with the right sensor id LinearSystem sys = LinearSystemId.identifyPositionSystem(kV, kA); @@ -128,7 +129,7 @@ public void periodic() { } case LOADING -> { indexMotor.set(1); // TODO: Tune - if (sensor.getRange(Rev2mDistanceSensor.Unit.kInches) + if (Units.Inches.of(sensor.getRange()).baseUnitMagnitude() < NOTE_LOADED_THRESHOLD.in(Units.Inches)) { state = State.LOADED; } @@ -143,7 +144,7 @@ public void periodic() { } case FIRING -> { indexMotor.set(1); // TODO: Tune - if (sensor.getRange(Rev2mDistanceSensor.Unit.kInches) + if (Units.Inches.of(sensor.getRange()).baseUnitMagnitude() < SHOT_FIRED_THRESHOLD.in(Units.Inches)) { state = State.EMPTY; } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 50c9de4..ebd9f46 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -2,7 +2,7 @@ import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkMax; -import com.revrobotics.Rev2mDistanceSensor; +import com.playingwithfusion.TimeOfFlight; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Units; @@ -18,7 +18,7 @@ public class Intake extends SubsystemBase { private static final Measure NOTE_PASSED_THRESHOLD = Units.Inches.of(0); // TODO: Tune private final CANSparkMax leadMotor; private final CANSparkMax followMotor; - private final Rev2mDistanceSensor sensor; + private final TimeOfFlight sensor; @Getter private State state; @@ -27,7 +27,7 @@ public Intake() { followMotor = new CANSparkMax(FOLLOW_MOTOR_ID, CANSparkLowLevel.MotorType.kBrushless); followMotor.follow(leadMotor); sensor = - new Rev2mDistanceSensor(Rev2mDistanceSensor.Port.kOnboard); // TODO: Figure out right value + new TimeOfFlight(0); // TODO: Fill in the sensor id state = State.NO_NOTE; } @@ -52,14 +52,14 @@ public void periodic() { switch (state) { case NO_NOTE: leadMotor.set(1); // TODO: Tune speed - if (sensor.getRange(Rev2mDistanceSensor.Unit.kInches) + if (Units.Inches.of(sensor.getRange()).baseUnitMagnitude() < NOTE_FOUND_THRESHOLD.in(Units.Inches)) { state = State.NOTE_FOUND; } break; case NOTE_FOUND: leadMotor.set(1); // TODO: Tune - if (sensor.getRange(Rev2mDistanceSensor.Unit.kInches) + if (Units.Inches.of(sensor.getRange()).baseUnitMagnitude() > NOTE_PASSED_THRESHOLD.in(Units.Inches)) { state = State.NOTE_PASSED; } diff --git a/vendordeps/playingwithfusion2024.json b/vendordeps/playingwithfusion2024.json new file mode 100644 index 0000000..15fc17c --- /dev/null +++ b/vendordeps/playingwithfusion2024.json @@ -0,0 +1,71 @@ +{ + "fileName": "playingwithfusion2024.json", + "name": "PlayingWithFusion", + "version": "2024.01.28", + "uuid": "14b8ad04-24df-11ea-978f-2e728ce88125", + "frcYear": "2024", + "jsonUrl": "https://www.playingwithfusion.com/frc/playingwithfusion2024.json", + "mavenUrls": [ + "https://www.playingwithfusion.com/frc/maven/" + ], + "javaDependencies": [ + { + "groupId": "com.playingwithfusion.frc", + "artifactId": "PlayingWithFusion-java", + "version": "2024.01.28" + } + ], + "jniDependencies": [ + { + "groupId": "com.playingwithfusion.frc", + "artifactId": "PlayingWithFusion-driver", + "version": "2024.01.28", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxarm32", + "linuxarm64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.playingwithfusion.frc", + "artifactId": "PlayingWithFusion-cpp", + "version": "2024.01.28", + "headerClassifier": "headers", + "sharedLibrary": false, + "libName": "PlayingWithFusion", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxarm32", + "linuxarm64" + ] + }, + { + "groupId": "com.playingwithfusion.frc", + "artifactId": "PlayingWithFusion-driver", + "version": "2024.01.28", + "headerClassifier": "headers", + "sharedLibrary": true, + "libName": "PlayingWithFusionDriver", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file From f076dadcc1b99911b962f45f37ed1f0f5b8809ed Mon Sep 17 00:00:00 2001 From: rohan Date: Sat, 17 Feb 2024 21:30:54 -0800 Subject: [PATCH 04/32] code clean up + no more errorprone errors --- src/main/java/frc/robot/RobotContainer.java | 48 +++++------- src/main/java/frc/robot/ShooterDataTable.java | 11 ++- src/main/java/frc/robot/Superstructure.java | 78 +++++++++++-------- .../java/frc/robot/inputs/PoseEstimator.java | 6 +- .../frc/robot/lib/SimpleVelocitySystem.java | 13 ++-- .../robot/lib/controllers/Thrustmaster.java | 2 +- .../java/frc/robot/subsystems/Climber.java | 27 ++----- .../java/frc/robot/subsystems/Indexer.java | 19 +++-- .../java/frc/robot/subsystems/Intake.java | 9 +-- .../java/frc/robot/subsystems/Shooter.java | 13 ++-- .../java/frc/robot/subsystems/Swerve.java | 7 +- vendordeps/REV2mDistanceSensor.json | 56 ------------- 12 files changed, 110 insertions(+), 179 deletions(-) delete mode 100644 vendordeps/REV2mDistanceSensor.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c81accf..d564154 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,6 +1,5 @@ package frc.robot; -import com.pathplanner.lib.auto.AutoBuilder; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.Distance; import edu.wpi.first.units.Measure; @@ -16,7 +15,6 @@ import frc.robot.subsystems.Intake; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Swerve; -import io.github.jdiemke.triangulation.NotEnoughPointsException; import java.io.File; import java.io.IOException; import org.photonvision.PhotonCamera; @@ -26,22 +24,12 @@ import swervelib.telemetry.SwerveDriveTelemetry; public class RobotContainer { - private final Measure> maximumSpeed = Units.FeetPerSecond.of(20); private final Swerve drivebase; - private final PoseEstimator poseEstimator; - private final Intake intake; - private final Shooter shooter; - private final Indexer indexer; - private final NoteDetector noteDetector; - private final ShooterDataTable shooterDataTable; // TODO: Ensure to get the actual points private static final Thrustmaster leftThrustmaster = new Thrustmaster(0); private static final Thrustmaster rightThrustmaster = new Thrustmaster(1); - private final SwerveDrive swerveDrive; - private final Superstructure superstructure; private final SendableChooser autoChooser = new SendableChooser<>(); - private final PhotonCamera photonCamera = new PhotonCamera("photonvision"); // TODO: Remember to replace with the actual camera name static { SwerveDriveTelemetry.verbosity = SwerveDriveTelemetry.TelemetryVerbosity.HIGH; @@ -54,7 +42,9 @@ public RobotContainer() { // Motor conversion factor is (PI * WHEEL DIAMETER IN METERS) / (GEAR RATIO * // ENCODER RESOLUTION). double driveConversionFactor = SwerveMath.calculateMetersPerRotation(0, 6.75, 1); + SwerveDrive swerveDrive; try { + Measure> maximumSpeed = Units.FeetPerSecond.of(20); swerveDrive = new SwerveParser(new File(Filesystem.getDeployDirectory(), "swerve")) .createSwerveDrive( @@ -65,12 +55,15 @@ public RobotContainer() { throw new RuntimeException(e); } - try { - shooterDataTable = new ShooterDataTable(null, null); - } catch (NotEnoughPointsException e) { - throw new RuntimeException(e); - } + // TODO: Ensure to get the actual points + ShooterDataTable shooterDataTable; + shooterDataTable = new ShooterDataTable(null, null); + PoseEstimator poseEstimator; + Intake intake; + Shooter shooter; + Indexer indexer; + NoteDetector noteDetector; try { poseEstimator = new PoseEstimator( @@ -79,6 +72,8 @@ public RobotContainer() { swerveDrive.swerveDrivePoseEstimator::getEstimatedPosition, swerveDrive::getModulePositions, swerveDrive.swerveDrivePoseEstimator::update); + // TODO: Remember to replace with the actual camera name + PhotonCamera photonCamera = new PhotonCamera("photonvision"); noteDetector = new NoteDetector(photonCamera, poseEstimator); intake = new Intake(); shooter = new Shooter(shooterDataTable, poseEstimator); @@ -88,17 +83,14 @@ public RobotContainer() { throw new RuntimeException(e); } - try { - drivebase = - new Swerve( - swerveDrive, - new ShooterDataTable(new Translation2d[] {}, new ShooterSpec[] {}), - poseEstimator); - } catch (NotEnoughPointsException e) { - throw new RuntimeException(e); // GG go next - } - - superstructure = new Superstructure(intake, indexer, shooter, drivebase, noteDetector, poseEstimator); + drivebase = + new Swerve( + swerveDrive, + new ShooterDataTable(new Translation2d[] {}, new ShooterSpec[] {}), + poseEstimator); + + Superstructure superstructure = + new Superstructure(intake, indexer, shooter, drivebase, noteDetector, poseEstimator); Command driveFieldOrientedDirectAngle = drivebase.driveCommand( diff --git a/src/main/java/frc/robot/ShooterDataTable.java b/src/main/java/frc/robot/ShooterDataTable.java index 550c8e8..c778d67 100644 --- a/src/main/java/frc/robot/ShooterDataTable.java +++ b/src/main/java/frc/robot/ShooterDataTable.java @@ -8,13 +8,12 @@ import io.github.jdiemke.triangulation.NotEnoughPointsException; public class ShooterDataTable { - private BarycentricInterpolation interpolatorAngle; - private BarycentricInterpolation interpolatorSpeedL; - private BarycentricInterpolation interpolatorSpeedR; - private BarycentricInterpolation interpolatorOffset; + private final BarycentricInterpolation interpolatorAngle; + private final BarycentricInterpolation interpolatorSpeedL; + private final BarycentricInterpolation interpolatorSpeedR; + private final BarycentricInterpolation interpolatorOffset; - public ShooterDataTable(Translation2d[] points, ShooterSpec[] specs) - throws NotEnoughPointsException { + public ShooterDataTable(Translation2d[] points, ShooterSpec[] specs) { double[] x = new double[points.length]; double[] y = new double[points.length]; double[] angle = new double[points.length]; diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 2d511a6..eaea01c 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -18,36 +18,54 @@ public class Superstructure extends SubsystemBase { private final Indexer indexer; private final Shooter shooter; private final Swerve swerve; - private final NoteDetector noteDetector; - private final PoseEstimator poseEstimator; - - private final PathPlannerPath leftTopToNoteToAmpTraj = PathPlannerPath.fromChoreoTrajectory("leftTopToAmp"); - private final PathPlannerPath topLeftToMiddle1 = PathPlannerPath.fromChoreoTrajectory("topLeftToMiddle1"); - - private final PathPlannerPath middle1ToMiddle2 = PathPlannerPath.fromChoreoTrajectory("middle1ToMiddle2"); - private final PathPlannerPath middle1ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle1ToSpeaker"); - private final PathPlannerPath middle2ToMiddle3 = PathPlannerPath.fromChoreoTrajectory("middle2ToMiddle3"); - private final PathPlannerPath middle2ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle2ToSpeaker"); - private final PathPlannerPath middle3ToMiddle4 = PathPlannerPath.fromChoreoTrajectory("middle3ToMiddle4"); - private final PathPlannerPath middle3ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle3ToSpeaker"); - private final PathPlannerPath middle4ToMiddle5 = PathPlannerPath.fromChoreoTrajectory("middle4ToMiddle5"); - private final PathPlannerPath middle4ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle4ToSpeaker"); - private final PathPlannerPath middle5ToSpeaker = PathPlannerPath.fromChoreoTrajectory("middle5ToMiddle6"); - private final PathPlannerPath ampToMiddle1 = PathPlannerPath.fromChoreoTrajectory("ampToMiddle1"); - private final PathPlannerPath bottomLeftToMiddle5 = PathPlannerPath.fromChoreoTrajectory("bottomLeftToMiddle5"); - - private final PathPlannerPath middle5ToMiddle4 = PathPlannerPath.fromChoreoTrajectory("middle5ToMiddle4"); - private final PathPlannerPath middle4ToMiddle3 = PathPlannerPath.fromChoreoTrajectory("middle4ToMiddle3"); - private final PathPlannerPath middle3ToMiddle2 = PathPlannerPath.fromChoreoTrajectory("middle3ToMiddle2"); - private final PathPlannerPath middle2ToMiddle1 = PathPlannerPath.fromChoreoTrajectory("middle2ToMiddle1"); - private final PathPlannerPath speakerToMiddle2 = PathPlannerPath.fromChoreoTrajectory("speakerToMiddle2"); - private final PathPlannerPath speakerToMiddle3 = PathPlannerPath.fromChoreoTrajectory("speakerToMiddle3"); - private final PathPlannerPath speakerToMiddle4 = PathPlannerPath.fromChoreoTrajectory("speakerToMiddle4"); - private final PathPlannerPath speakerToMiddle5 = PathPlannerPath.fromChoreoTrajectory("speakerToMiddle5"); + private final PathPlannerPath leftTopToNoteToAmpTraj = + PathPlannerPath.fromChoreoTrajectory("leftTopToAmp"); + private final PathPlannerPath topLeftToMiddle1 = + PathPlannerPath.fromChoreoTrajectory("topLeftToMiddle1"); + + private final PathPlannerPath middle1ToMiddle2 = + PathPlannerPath.fromChoreoTrajectory("middle1ToMiddle2"); + private final PathPlannerPath middle1ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle1ToSpeaker"); + private final PathPlannerPath middle2ToMiddle3 = + PathPlannerPath.fromChoreoTrajectory("middle2ToMiddle3"); + private final PathPlannerPath middle2ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle2ToSpeaker"); + private final PathPlannerPath middle3ToMiddle4 = + PathPlannerPath.fromChoreoTrajectory("middle3ToMiddle4"); + private final PathPlannerPath middle3ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle3ToSpeaker"); + private final PathPlannerPath middle4ToMiddle5 = + PathPlannerPath.fromChoreoTrajectory("middle4ToMiddle5"); + private final PathPlannerPath middle4ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle4ToSpeaker"); + private final PathPlannerPath middle5ToSpeaker = + PathPlannerPath.fromChoreoTrajectory("middle5ToMiddle6"); - private final PathPlannerPath fromStartingMiddleToMiddle3 = PathPlannerPath - .fromChoreoTrajectory("fromStartingMiddleToMiddle3"); + private final PathPlannerPath ampToMiddle1 = PathPlannerPath.fromChoreoTrajectory("ampToMiddle1"); + private final PathPlannerPath bottomLeftToMiddle5 = + PathPlannerPath.fromChoreoTrajectory("bottomLeftToMiddle5"); + + private final PathPlannerPath middle5ToMiddle4 = + PathPlannerPath.fromChoreoTrajectory("middle5ToMiddle4"); + private final PathPlannerPath middle4ToMiddle3 = + PathPlannerPath.fromChoreoTrajectory("middle4ToMiddle3"); + private final PathPlannerPath middle3ToMiddle2 = + PathPlannerPath.fromChoreoTrajectory("middle3ToMiddle2"); + private final PathPlannerPath middle2ToMiddle1 = + PathPlannerPath.fromChoreoTrajectory("middle2ToMiddle1"); + private final PathPlannerPath speakerToMiddle2 = + PathPlannerPath.fromChoreoTrajectory("speakerToMiddle2"); + private final PathPlannerPath speakerToMiddle3 = + PathPlannerPath.fromChoreoTrajectory("speakerToMiddle3"); + private final PathPlannerPath speakerToMiddle4 = + PathPlannerPath.fromChoreoTrajectory("speakerToMiddle4"); + private final PathPlannerPath speakerToMiddle5 = + PathPlannerPath.fromChoreoTrajectory("speakerToMiddle5"); + + private final PathPlannerPath fromStartingMiddleToMiddle3 = + PathPlannerPath.fromChoreoTrajectory("fromStartingMiddleToMiddle3"); public Superstructure( Intake intake, @@ -60,8 +78,6 @@ public Superstructure( this.indexer = indexer; this.shooter = shooter; this.swerve = swerve; - this.noteDetector = noteDetector; - this.poseEstimator = poseEstimator; } @Override @@ -114,7 +130,7 @@ public Command fromStartingMiddleWithoutAmp() { checkAndHandleNote(fromMiddle3ToSpeaker(), fromMiddle3ToMiddle2(), fromSpeakerToMiddle2()), checkAndHandleNote( fromMiddle2ToSpeaker(), fromMiddle2ToMiddle3(), null) // NOTE: we could do more... - ); + ); } public Command fromMiddle1ToMiddle5() { diff --git a/src/main/java/frc/robot/inputs/PoseEstimator.java b/src/main/java/frc/robot/inputs/PoseEstimator.java index 8ad43d9..5f9dc91 100644 --- a/src/main/java/frc/robot/inputs/PoseEstimator.java +++ b/src/main/java/frc/robot/inputs/PoseEstimator.java @@ -21,8 +21,6 @@ public class PoseEstimator implements Subsystem { private static final Translation2d BLUE_SPEAKER_POSITION = new Translation2d(-1.5, 218.42); private static final Translation2d RED_SPEAKER_POSITION = new Translation2d(652.73, 218.42); - private final AprilTagFieldLayout aprilTagFieldLayout; - private final Transform3d robotToCam; private final PhotonPoseEstimator photonPoseEstimator; private final Pigeon2 gyro; private final BiConsumer addVisionMeasurement; @@ -43,10 +41,10 @@ public PoseEstimator( this.getModulePositions = getModulePositions; this.update = update; gyro = new Pigeon2(13, "*"); - aprilTagFieldLayout = + AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); - robotToCam = + Transform3d robotToCam = new Transform3d( new Translation3d( Units.inchesToMeters(14.5), Units.inchesToMeters(4.5), Units.inchesToMeters(18)), diff --git a/src/main/java/frc/robot/lib/SimpleVelocitySystem.java b/src/main/java/frc/robot/lib/SimpleVelocitySystem.java index 78c014e..23bd9bf 100644 --- a/src/main/java/frc/robot/lib/SimpleVelocitySystem.java +++ b/src/main/java/frc/robot/lib/SimpleVelocitySystem.java @@ -8,13 +8,14 @@ import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.LinearSystemLoop; import edu.wpi.first.math.system.plant.LinearSystemId; +import lombok.Getter; /** Defines a simple velocity system using a kalman filter and a linear quadratic regulator */ public class SimpleVelocitySystem { private final double kS; private final double maxControlEffort; // volts - private final LinearSystem system; + @Getter private final LinearSystem system; private final LinearQuadraticRegulator regulator; private final KalmanFilter filter; private final LinearSystemLoop loop; @@ -48,6 +49,8 @@ public SimpleVelocitySystem( } /** + * Sets output + * * @param output The desired output of the system */ public void set(double output) { @@ -55,6 +58,8 @@ public void set(double output) { } /** + * Updates the system + * * @param current The measured velocity by the encoder */ public void update(double current) { @@ -64,6 +69,8 @@ public void update(double current) { } /** + * Gets output + * * @return The percent output that the controller should run at */ public double getOutput() { @@ -75,10 +82,6 @@ public double getVelocity() { return filteredVelocity; // loop.getXHat(0); } - public LinearSystem getSystem() { - return system; - } - public LinearQuadraticRegulator getLQR() { return regulator; } diff --git a/src/main/java/frc/robot/lib/controllers/Thrustmaster.java b/src/main/java/frc/robot/lib/controllers/Thrustmaster.java index b14740a..9d601cf 100644 --- a/src/main/java/frc/robot/lib/controllers/Thrustmaster.java +++ b/src/main/java/frc/robot/lib/controllers/Thrustmaster.java @@ -41,4 +41,4 @@ public enum Button { val = i; } } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 2605dd2..833d84f 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -7,13 +7,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { - private final int LEFT_MOTOR_ID = 15; // TODO: add motor ports - private final int RIGHT_MOTOR_ID = 16; - private TalonFX leftMotor; - private TalonFX rightMotor; + private final TalonFX leftMotor; + private final TalonFX rightMotor; public Climber() { + // TODO: add motor ports + int LEFT_MOTOR_ID = 15; leftMotor = new TalonFX(LEFT_MOTOR_ID); + int RIGHT_MOTOR_ID = 16; rightMotor = new TalonFX(RIGHT_MOTOR_ID); leftMotor.setInverted(true); // TODO: Change based on change rightMotor.setInverted(true); @@ -27,25 +28,11 @@ private void setTelescopeSpeed(double speed) { } public Command telescopeUp() { - return new StartEndCommand( - () -> { - setTelescopeSpeed(0.5); - }, - () -> { - setTelescopeSpeed(0); - }, - this); + return new StartEndCommand(() -> setTelescopeSpeed(0.5), () -> setTelescopeSpeed(0), this); } public Command telescopeDown() { - return new StartEndCommand( - () -> { - setTelescopeSpeed(-0.5); - }, - () -> { - setTelescopeSpeed(0); - }, - this); + return new StartEndCommand(() -> setTelescopeSpeed(-0.5), () -> setTelescopeSpeed(0), this); } public Command leftUp() { diff --git a/src/main/java/frc/robot/subsystems/Indexer.java b/src/main/java/frc/robot/subsystems/Indexer.java index 22e0f4a..10902e8 100644 --- a/src/main/java/frc/robot/subsystems/Indexer.java +++ b/src/main/java/frc/robot/subsystems/Indexer.java @@ -1,5 +1,8 @@ package frc.robot.subsystems; +import static edu.wpi.first.units.Units.Degrees; + +import com.playingwithfusion.TimeOfFlight; import com.revrobotics.*; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; @@ -17,7 +20,6 @@ import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import frc.robot.ShooterDataTable; import frc.robot.inputs.PoseEstimator; -import com.playingwithfusion.TimeOfFlight; import lombok.Getter; public class Indexer extends SubsystemBase { @@ -32,15 +34,15 @@ public class Indexer extends SubsystemBase { private static final Measure ANGLE_ERROR_TOLERANCE = null; private static final Measure> ANGLE_SPEED_STANDARD_DEVIATION = null; private static final Measure> ANGLE_SPEED_ERROR_TOLERANCE = null; - private static final Measure TICKS_TO_ANGLE = null; // TODO: adjust for this year's robot + private static final Measure TICKS_TO_ANGLE = + Degrees.of(0); // TODO: adjust for this year's robot private static final Measure MAX_ANGLE_MOTOR_VOLTAGE = Units.Volts.of(12); private static final Measure NOTE_LOADED_THRESHOLD = Units.Inches.of(0); // TODO: Tune private static final Measure SHOT_FIRED_THRESHOLD = Units.Inches.of(0); // TODO: Tune - private static final Measure