From d693faf5c94a6439946dced84207f1fd51a2560a Mon Sep 17 00:00:00 2001 From: Tylr-J42 <84476781+Tylr-J42@users.noreply.github.com> Date: Mon, 24 Mar 2025 01:00:35 -0400 Subject: [PATCH] mirrored paths and vision disconnection detection --- .../pathplanner/autos/3 Coral Left.auto | 6 ++ .../pathplanner/autos/Two Coral Right.auto | 49 ------------- .../pathplanner/paths/30 Right to HP.path | 2 +- .../pathplanner/paths/330 Right to HP.path | 2 +- .../pathplanner/paths/HP to 330 Right.path | 2 +- .../deploy/pathplanner/paths/HP to K.path | 2 +- .../deploy/pathplanner/paths/L Backup.path | 2 +- .../deploy/pathplanner/paths/New Path.path | 65 ----------------- .../pathplanner/paths/Start to 30 Right.path | 10 +-- src/main/deploy/pathplanner/paths/fein.path | 70 ------------------- src/main/deploy/pathplanner/settings.json | 4 +- src/main/java/frc/robot/RobotContainer.java | 51 +++++++------- .../java/frc/robot/subsystems/Drivetrain.java | 10 +-- .../frc/robot/subsystems/MAXSwerveModule.java | 3 - .../robot/subsystems/ManipulatorPivot.java | 1 - .../java/frc/robot/subsystems/Vision.java | 8 +++ 16 files changed, 55 insertions(+), 232 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Two Coral Right.auto delete mode 100644 src/main/deploy/pathplanner/paths/New Path.path delete mode 100644 src/main/deploy/pathplanner/paths/fein.path diff --git a/src/main/deploy/pathplanner/autos/3 Coral Left.auto b/src/main/deploy/pathplanner/autos/3 Coral Left.auto index 7e98e95..bfa0b4e 100644 --- a/src/main/deploy/pathplanner/autos/3 Coral Left.auto +++ b/src/main/deploy/pathplanner/autos/3 Coral Left.auto @@ -83,6 +83,12 @@ "data": { "pathName": "HP to K" } + }, + { + "type": "named", + "data": { + "name": "Shoot Coral L4" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Two Coral Right.auto b/src/main/deploy/pathplanner/autos/Two Coral Right.auto deleted file mode 100644 index 829bc04..0000000 --- a/src/main/deploy/pathplanner/autos/Two Coral Right.auto +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Right Start to E" - } - }, - { - "type": "named", - "data": { - "name": "Shoot Coral L4" - } - }, - { - "type": "named", - "data": { - "name": "HP Pickup" - } - }, - { - "type": "path", - "data": { - "pathName": "E to HP" - } - }, - { - "type": "named", - "data": { - "name": "Collect Coral" - } - }, - { - "type": "path", - "data": { - "pathName": "HP to D" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/30 Right to HP.path b/src/main/deploy/pathplanner/paths/30 Right to HP.path index a44ed39..34c74e8 100644 --- a/src/main/deploy/pathplanner/paths/30 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/30 Right to HP.path @@ -45,7 +45,7 @@ "rotation": -53.97262661489646 }, "reversed": false, - "folder": null, + "folder": "Left Paths", "idealStartingState": { "velocity": 0, "rotation": -120.06858282186238 diff --git a/src/main/deploy/pathplanner/paths/330 Right to HP.path b/src/main/deploy/pathplanner/paths/330 Right to HP.path index b78098f..549fc93 100644 --- a/src/main/deploy/pathplanner/paths/330 Right to HP.path +++ b/src/main/deploy/pathplanner/paths/330 Right to HP.path @@ -57,7 +57,7 @@ "rotation": -53.98486432191523 }, "reversed": false, - "folder": null, + "folder": "Left Paths", "idealStartingState": { "velocity": 0, "rotation": -59.99999999999999 diff --git a/src/main/deploy/pathplanner/paths/HP to 330 Right.path b/src/main/deploy/pathplanner/paths/HP to 330 Right.path index ce5ff97..7ee14ee 100644 --- a/src/main/deploy/pathplanner/paths/HP to 330 Right.path +++ b/src/main/deploy/pathplanner/paths/HP to 330 Right.path @@ -57,7 +57,7 @@ "rotation": -59.69923999693802 }, "reversed": false, - "folder": null, + "folder": "Left Paths", "idealStartingState": { "velocity": 0, "rotation": -53.97262661489646 diff --git a/src/main/deploy/pathplanner/paths/HP to K.path b/src/main/deploy/pathplanner/paths/HP to K.path index 5fdf785..3f834b0 100644 --- a/src/main/deploy/pathplanner/paths/HP to K.path +++ b/src/main/deploy/pathplanner/paths/HP to K.path @@ -57,7 +57,7 @@ "rotation": -60.49491285058726 }, "reversed": false, - "folder": null, + "folder": "Left Paths", "idealStartingState": { "velocity": 0, "rotation": -53.97262661489646 diff --git a/src/main/deploy/pathplanner/paths/L Backup.path b/src/main/deploy/pathplanner/paths/L Backup.path index 64b1ce9..53135f0 100644 --- a/src/main/deploy/pathplanner/paths/L Backup.path +++ b/src/main/deploy/pathplanner/paths/L Backup.path @@ -45,7 +45,7 @@ "rotation": -53.97262661489646 }, "reversed": false, - "folder": null, + "folder": "Left Paths", "idealStartingState": { "velocity": 0, "rotation": -59.69923999693802 diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path deleted file mode 100644 index efaa1b2..0000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,65 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 12.272, - "y": 2.975 - }, - "prevControl": { - "x": 10.940715672291898, - "y": 2.975 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [ - { - "fieldPosition": { - "x": 0.4, - "y": 5.5 - }, - "rotationOffset": 0.0, - "minWaypointRelativePos": 0.15, - "maxWaypointRelativePos": 0.4, - "name": "Point Towards Zone" - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 1.75, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 400.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 59.99999999999999 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Start to 30 Right.path b/src/main/deploy/pathplanner/paths/Start to 30 Right.path index ba0cede..70e06de 100644 --- a/src/main/deploy/pathplanner/paths/Start to 30 Right.path +++ b/src/main/deploy/pathplanner/paths/Start to 30 Right.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.1686475409836055, - "y": 7.573360655737705 + "x": 7.150967037968244, + "y": 7.5521014571037055 }, "prevControl": null, "nextControl": { - "x": 5.933913934426228, - "y": 6.5544057377049185 + "x": 5.916233431410867, + "y": 6.533146539070919 }, "isLocked": false, "linkedName": null @@ -57,7 +57,7 @@ "rotation": -119.71497744813712 }, "reversed": false, - "folder": null, + "folder": "Left Paths", "idealStartingState": { "velocity": 0, "rotation": -90.0 diff --git a/src/main/deploy/pathplanner/paths/fein.path b/src/main/deploy/pathplanner/paths/fein.path deleted file mode 100644 index edd51cc..0000000 --- a/src/main/deploy/pathplanner/paths/fein.path +++ /dev/null @@ -1,70 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 7.600204918039607, - "y": 6.374590163938041 - }, - "prevControl": null, - "nextControl": { - "x": 7.600204918032786, - "y": 7.573360655737705 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.209631147540984, - "y": 6.074897540983606 - }, - "prevControl": { - "x": 7.120696721311476, - "y": 6.074897540983606 - }, - "nextControl": { - "x": 4.745363537952068, - "y": 6.074897540983606 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.8670081967213115, - "y": 6.973975409836065 - }, - "prevControl": { - "x": 5.826024590163935, - "y": 6.9979508196721305 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.5, - "maxAcceleration": 1.75, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 400.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 180.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 05d1cf0..a1e375e 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,7 +2,9 @@ "robotWidth": 0.8763, "robotLength": 0.8763, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Left Paths" + ], "autoFolders": [], "defaultMaxVel": 3.5, "defaultMaxAccel": 1.75, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8508d22..3ecaa17 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,11 +7,9 @@ package frc.robot; import frc.robot.constants.ManipulatorPivotConstants; import frc.robot.constants.ClimberPivotConstants; import frc.robot.constants.ElevatorConstants; -import frc.robot.constants.ManipulatorConstants; import frc.robot.constants.OIConstants; import frc.robot.constants.VisionConstants; import frc.robot.subsystems.ManipulatorPivot; -import frc.robot.subsystems.Vision; import frc.robot.subsystems.ClimberPivot; import frc.robot.subsystems.ClimberRollers; import frc.robot.subsystems.Drivetrain; @@ -22,8 +20,7 @@ import java.util.function.IntSupplier; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.events.EventTrigger; -import com.pathplanner.lib.path.EventMarker; +import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; @@ -33,9 +30,7 @@ import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import edu.wpi.first.wpilibj2.command.button.Trigger; public class RobotContainer { private ClimberPivot climberPivot; @@ -78,6 +73,9 @@ public class RobotContainer { operator = new CommandXboxController(OIConstants.kOperatorControllerPort); autoChooser = AutoBuilder.buildAutoChooser(); + autoChooser.addOption("One Coral Left", new PathPlannerAuto("One Coral Left", true)); + autoChooser.addOption("2.5 Coral Right", new PathPlannerAuto("2.5 Coral Left", true)); + autoChooser.addOption("3 Coral Right", new PathPlannerAuto("3 Coral Left", true)); closestTag = drivetrain::getClosestTag; @@ -238,7 +236,8 @@ public class RobotContainer { //new EventTrigger("HP Pickup").onTrue(safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand()); - NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(0.5).andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.01)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition()))); + NamedCommands.registerCommand("Shoot Coral L4", Commands.race(manipulator.runManipulator(() -> 0.4, true).withTimeout(0.5) + .andThen(manipulator.runManipulator(() -> 0.0, false).withTimeout(0.01)), Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition()))); NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runManipulator(() -> 0, false).withTimeout(0.01))); NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position) .andThen(elevator.maintainPosition().withTimeout(0.1), manipulatorPivot.maintainPosition().withTimeout(0.01))); @@ -261,7 +260,7 @@ public class RobotContainer { sensorTab.addDouble("Elevator Position", elevator::getEncoderPosition) .withSize(2, 1) .withPosition(0, 0) - .withWidget(BuiltInWidgets.kGraph); + .withWidget(BuiltInWidgets.kTextView); sensorTab.addDouble("Manipulator Position", manipulatorPivot::getEncoderPosition) .withSize(2, 1) @@ -316,10 +315,6 @@ public class RobotContainer { sensorTab.addDouble("manipulator output", manipulatorPivot::getPivotOutput); - sensorTab.addDouble("manipulator cg position", manipulatorPivot::getCGPosition); - - sensorTab.addDouble("dt distance", drivetrain::driveDistance); - sensorTab.addDouble("velocity", drivetrain::getVelocity); sensorTab.addDouble("heading", drivetrain::getHeading); @@ -328,33 +323,39 @@ public class RobotContainer { //sensorTab.add("odometry", drivetrain::getPose); apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag()) - .withSize(1,1).withPosition(1,1); + .withSize(2,1).withPosition(1,1); apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX()) - .withSize(2,1).withPosition(2,1); + .withSize(2,1).withPosition(3,1); apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY()) - .withSize(3,1).withPosition(3,1); + .withSize(2,1).withPosition(5,1); apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist()) - .withSize(4,1).withPosition(4,1); + .withSize(2,1).withPosition(7,1); apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS()) - .withSize(6,1).withPosition(5,1); + .withSize(2,1).withPosition(9,1); apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected()) - .withSize(7,1).withPosition(6,1); + .withSize(2,1).withPosition(11,1); apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag()) - .withSize(1,1).withPosition(1,2); + .withSize(2,1).withPosition(1,2); apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX()) - .withSize(1,1).withPosition(2,2); + .withSize(2,1).withPosition(3,2); apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY()) - .withSize(1,1).withPosition(3,2); + .withSize(2,1).withPosition(5,2); apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist()) - .withSize(1,1).withPosition(4,2); + .withSize(2,1).withPosition(7,2); apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS()) - .withSize(1,1).withPosition(5,2); + .withSize(2,1).withPosition(7,2); apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected()) - .withSize(1,1).withPosition(6,2); + .withSize(2,1).withPosition(9,2); apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag()) - .withSize(1,1).withPosition(4,4); + .withSize(2,1).withPosition(4,4); + + apriltagTab.addBoolean("Is orange connected?", () -> drivetrain.vision.isOrangeConnected()) + .withSize(2, 1).withPosition(4, 2); + apriltagTab.addBoolean("Is black connected?", () -> drivetrain.vision.isBlackConnected()) + .withSize(2, 1).withPosition(6, 2); + } diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 9fe92ec..56a5ed8 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -14,14 +14,12 @@ import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.Orchestra; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.RotationTarget; import com.studica.frc.AHRS; import com.studica.frc.AHRS.NavXComType; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -176,7 +174,7 @@ public class Drivetrain extends SubsystemBase { m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.3, 0.3, Units.degreesToRadians(360))); - if(vision.getOrangeTagDetected()){ + if(vision.getOrangeTagDetected() && vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360))); } @@ -194,7 +192,7 @@ public class Drivetrain extends SubsystemBase { Logger.recordOutput("orange pose", new Pose3d(orangePose2d)); - if(vision.getBlackTagDetected()){ + if(vision.getBlackTagDetected() && vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60){ m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.1, 0.1, Units.degreesToRadians(360))); } @@ -442,10 +440,6 @@ public class Drivetrain extends SubsystemBase { return gyro.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0); } - public double driveDistance(){ - return m_frontLeft.getTotalDist(); - } - public double getVelocity(){ return m_frontLeft.getState().speedMetersPerSecond; } diff --git a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java index 01c0e16..8460aac 100644 --- a/src/main/java/frc/robot/subsystems/MAXSwerveModule.java +++ b/src/main/java/frc/robot/subsystems/MAXSwerveModule.java @@ -144,7 +144,4 @@ public class MAXSwerveModule { m_drive.setPosition(0); } - public double getTotalDist(){ - return m_drive.getPosition().getValueAsDouble() * ModuleConstants.kWheelCircumferenceMeters; - } } diff --git a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java index f1f26e3..042ce89 100644 --- a/src/main/java/frc/robot/subsystems/ManipulatorPivot.java +++ b/src/main/java/frc/robot/subsystems/ManipulatorPivot.java @@ -9,7 +9,6 @@ import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.util.Units; diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 8868c38..6e33690 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -175,5 +175,13 @@ public class Vision{ public double getOrangeFPS(){ return orangeFramerate.get(); } + + public boolean isBlackConnected(){ + return Timer.getFPGATimestamp()-black_tx.getLastChange() > 2.0; + } + + public boolean isOrangeConnected(){ + return Timer.getFPGATimestamp()-orange_tx.getLastChange() > 2.0; + } }