diff --git a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto index 49c8daa..19947fe 100644 --- a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto @@ -15,12 +15,6 @@ "data": { "name": "place-coral-left-l4" } - }, - { - "type": "path", - "data": { - "pathName": "Reef 4 to Bottom Feed" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/1 Coral Right Barge Bottom.auto b/src/main/deploy/pathplanner/autos/1 Coral Right Barge Bottom.auto new file mode 100644 index 0000000..7cfee77 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 Coral Right Barge Bottom.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Cage 6 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Copy of 1 Coral Right Barge Bottom.auto b/src/main/deploy/pathplanner/autos/Copy of 1 Coral Right Barge Bottom.auto new file mode 100644 index 0000000..066cfec --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Copy of 1 Coral Right Barge Bottom.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Cage 1 to Reef" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Forward L4 Left.auto b/src/main/deploy/pathplanner/autos/Forward L4 Left.auto new file mode 100644 index 0000000..440a1ea --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Forward L4 Left.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index bde395e..91a4afd 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 4.0, + "x": 3.53925, "y": 6.0 }, "prevControl": { - "x": 4.249520880858856, + "x": 3.7887708808588547, "y": 5.984529705386749 }, "nextControl": null, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index af0efd4..983d15f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -536,6 +536,12 @@ public class RobotContainer { new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive), new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive) ); + + private Command moveForwardL4 = new SequentialCommandGroup( + new InstantCommand(() -> m_robotSwerveDrive.startSlowPeriod(), m_robotSwerveDrive), + new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 1500, true), + new InstantCommand(() -> m_robotSwerveDrive.endSlowPeriod(), m_robotSwerveDrive) + ); private Boolean operatorManualMode = false; @@ -596,6 +602,8 @@ public class RobotContainer { new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.PrimedFour)) )); + NamedCommands.registerCommand("move-forward-l4", moveForwardL4); + configureButtonBindings(); configureVirtualButtonBindings(); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 3ec7d30..1e6893d 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2025RidgeScape"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 183; - public static final String GIT_SHA = "4d20c5064279d9e0ac890c93e644e81dbabf929c"; - public static final String GIT_DATE = "2025-10-11 15:24:47 MDT"; + public static final int GIT_REVISION = 184; + public static final String GIT_SHA = "52fc524c0f614a15bb103c76c3bbecf8f7476f9e"; + public static final String GIT_DATE = "2025-10-11 17:35:41 MDT"; public static final String GIT_BRANCH = "advantagekit"; - public static final String BUILD_DATE = "2025-10-11 16:46:07 MDT"; - public static final long BUILD_UNIX_TIME = 1760222767896L; + public static final String BUILD_DATE = "2025-10-12 09:09:45 MDT"; + public static final long BUILD_UNIX_TIME = 1760281785887L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 22e0068..19226c8 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -181,7 +181,7 @@ public final class Constants { //Max for elevator = 50% public static final double GROUND_POSITION_ELEVATOR = 0 * GEAR_RATIO_ELEVATOR; - public static final double WAITING_POSITION_ELEVATOR = -9.5 - 1.5; // TODO: find 2-4 in off the pipe + public static final double WAITING_POSITION_ELEVATOR = -9.5 - 0.5; // TODO: find 2-4 in off the pipe public static final double HOVERING_POSITION_ELEVATOR = -7.5; // TODO: find 2-4 in off the pipe public static final double WAITING_POSITION_BEAM_BREAK_ELEVATOR = -5; // TODO: find on the pipe public static final double SCORING_THREE_ELEVATOR = -9.25; diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index df29015..4d509b0 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems.swerve; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -76,7 +77,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - return getPose2d(); + return io.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); }, // Robot pose supplier this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java index fa7de1a..e297519 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java @@ -1,6 +1,7 @@ package frc4388.robot.subsystems.swerve; import java.util.List; +import java.util.Optional; import org.littletonrobotics.junction.AutoLog; @@ -30,4 +31,6 @@ public interface SwerveIO { public default void addVisionMeasurement(List poses) {} public default void updateInputs(SwerveState state) {} + + public default Optional samplePoseAt(double time) {return Optional.empty();} } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java index 05d9e8f..98f0afb 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java @@ -1,6 +1,7 @@ package frc4388.robot.subsystems.swerve; import java.util.List; +import java.util.Optional; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -10,6 +11,7 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Pose2d; import frc4388.robot.subsystems.vision.Vision; import frc4388.robot.subsystems.vision.VisionIO.PoseObservation; @@ -53,6 +55,11 @@ public class SwerveReal implements SwerveIO { } } + @Override + public Optional samplePoseAt(double time) { + return swerveDriveTrain.samplePoseAt(time); + } + @Override public void addVisionMeasurement(List poses) { for(PoseObservation pose : poses) {