diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json index e589b1b..95beffa 100644 --- a/.OutlineViewer/outlineviewer.json +++ b/.OutlineViewer/outlineviewer.json @@ -11,6 +11,9 @@ "client@2": { "open": true }, + "client@3": { + "open": true + }, "client@4": { "Publishers": { "open": true diff --git a/src/main/deploy/pathplanner/paths/New New Path.path b/src/main/deploy/pathplanner/paths/New New Path.path new file mode 100644 index 0000000..15c46cb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New New Path.path @@ -0,0 +1,54 @@ +{ + "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": 4.891, + "y": 5.284 + }, + "prevControl": { + "x": 3.891, + "y": 5.284 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 2.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 400.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 15f1afb..17af840 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.events.EventTrigger; import com.pathplanner.lib.path.EventMarker; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -159,7 +160,7 @@ public class RobotContainer { drivetrain.goToPose( () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], - () -> Units.degreesToRadians( 180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]) + () -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180)) ) ); @@ -167,7 +168,7 @@ public class RobotContainer { drivetrain.goToPose( () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], () -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], - () -> Units.degreesToRadians(180-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]) + () -> Rotation2d.fromRadians(Units.degreesToRadians(VisionConstants.globalTagCoords[closestTag.getAsInt()][3]+180)) ) ); @@ -231,9 +232,10 @@ 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", manipulator.runManipulator(() -> 0.4, true).withTimeout(2)); - NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).withTimeout(0.5)); - NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position)); + NamedCommands.registerCommand("Shoot Coral L4", manipulator.runManipulator(() -> 0.4, true).withTimeout(2).andThen(manipulator.runUntilCollected(() -> 0))); + NamedCommands.registerCommand("Collect Coral", manipulator.runUntilCollected(() -> 0.30).andThen(manipulator.runUntilCollected(() -> 0)).withTimeout(0.5)); + NamedCommands.registerCommand("Lift L4", safeMoveManipulator(ElevatorConstants.kL4Position, ManipulatorPivotConstants.kL4Position) + .andThen(Commands.parallel(elevator.maintainPosition(), manipulatorPivot.maintainPosition()))); NamedCommands.registerCommand("HP Pickup", safeMoveManipulator(ElevatorConstants.kCoralIntakePosition, ManipulatorPivotConstants.kStartingPosition)); } @@ -314,6 +316,8 @@ public class RobotContainer { sensorTab.addDouble("velocity", drivetrain::getVelocity); + sensorTab.addDouble("heading", drivetrain::getHeading); + //sensorTab.add("odometry", drivetrain::getPose); @@ -324,6 +328,7 @@ public class RobotContainer { apriltagTab.addDouble("global x", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getX()); apriltagTab.addDouble("global y", () -> drivetrain.vision.getOrangeGlobalPose(drivetrain.getGyroBuffer()).getY()); apriltagTab.addDouble("closest tag", () -> drivetrain.getClosestTag()); + apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS()); // sensorTab.addDouble(" ID", vision::getOrangeClosestTag); diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 4763f0c..10c35c2 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -57,7 +57,7 @@ public class DrivetrainConstants { public static final boolean kGyroReversed = true; - public static final double kHeadingP = 0.01; + public static final double kHeadingP = 0.1; public static final double kXTranslationP = 0.5; public static final double kYTranslationP = 0.5; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 284323f..763fb20 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -54,7 +54,7 @@ public class VisionConstants { {3.703, 3.975, 3.982, 2.806}, {3.183, 4.191, 3.183, 3.857}, {3.986, 5.24, 3.701, 5.076}, - {5.275, 5.075, 4.991, 5.246}, + {5.275, 5.075, 4.891, 5.284},//4.991, 5.246}, {5.789, 3.862, 5.789, 4.194}, {4.993, 2.816, 5.272, 2.996} }; diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 7207fa1..a3dcaef 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -8,17 +8,20 @@ import java.io.File; import java.util.Optional; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; 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; @@ -112,7 +115,8 @@ public class Drivetrain extends SubsystemBase { ); pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0); - pidHeading.enableContinuousInput(-180, 180); + pidHeading.setTolerance(Units.degreesToRadians(3)); + pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180)); pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0); pidTranslationX.setTolerance(Units.inchesToMeters(0.5)); @@ -171,11 +175,11 @@ public class Drivetrain extends SubsystemBase { gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue()); - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(360))); if(vision.getOrangeTagDetected()){ if(vision.getOrangeDist() < 60){ - m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360))); + m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(360))); } // if the detected tags match your alliances reef tags use their pose estimates @@ -187,8 +191,9 @@ public class Drivetrain extends SubsystemBase { orangePose2d = vision.getOrangeGlobalPose(gyroBuffer); m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp()); } - Logger.recordOutput("orange pose", new Pose3d()); } + Logger.recordOutput("orange pose", new Pose3d(orangePose2d)); + if(vision.getBlackTagDetected()){ if(vision.getBlackDist() < 60){ @@ -310,16 +315,22 @@ public class Drivetrain extends SubsystemBase { }); } - public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){ - return run(() -> { - drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.1, 0.1), - MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.1, 0.1), - pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()), + public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier headingSetpoint){ + return startRun(() -> { + pidHeading.reset(); + pidTranslationX.reset(); + pidTranslationY.reset(); + }, + () -> { + drive(MathUtil.clamp(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()), -0.05, 0.05), + MathUtil.clamp(pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), -0.05, 0.05), + pidHeading.calculate(Units.degreesToRadians(getHeading()), headingSetpoint.get().getRadians()), true); + Logger.recordOutput("reeeee", pidHeading.getError()); Logger.recordOutput("reef setpoint", new Pose3d(new Pose2d( new Translation2d(xSetpoint.getAsDouble(), ySetpoint.getAsDouble()), - new Rotation2d(headingSetpoint.getAsDouble())))); + headingSetpoint.get()))); }); } @@ -409,7 +420,7 @@ public class Drivetrain extends SubsystemBase { * @return the robot's heading in degrees, from -180 to 180 */ public double getHeading() { - return Rotation2d.fromDegrees(getGyroValue()).getDegrees(); + return m_estimator.getEstimatedPosition().getRotation().getDegrees(); } public TimeInterpolatableBuffer getGyroBuffer(){